mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 02:03:33 +08:00
EKF: Clean up control of observation fusion
All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon. Information and warning messages are improved.
This commit is contained in:
Paul Riseborough
+486
-252
@@ -41,6 +41,7 @@
|
||||
|
||||
#include "../ecl.h"
|
||||
#include "ekf.h"
|
||||
#include "mathlib.h"
|
||||
|
||||
void Ekf::controlFusionModes()
|
||||
{
|
||||
@@ -60,227 +61,350 @@ void Ekf::controlFusionModes()
|
||||
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
|
||||
_control_status.flags.tilt_align = true;
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
ECL_INFO("EKF alignment complete");
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// control use of various external sources for position and velocity aiding
|
||||
// check for arrival of new sensor data at the fusion time horizon
|
||||
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
|
||||
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
|
||||
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
|
||||
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
|
||||
&& (_R_to_earth(2, 2) > 0.7071f);
|
||||
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
|
||||
&& (_R_to_earth(2, 2) > 0.7071f);
|
||||
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
|
||||
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
|
||||
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
controlHeightSensorTimeouts();
|
||||
|
||||
// control use of observations for aiding
|
||||
controlMagAiding();
|
||||
controlExternalVisionAiding();
|
||||
controlOpticalFlowAiding();
|
||||
controlGpsAiding();
|
||||
controlHeightAiding();
|
||||
controlMagAiding();
|
||||
controlBaroAiding();
|
||||
controlRangeFinderAiding();
|
||||
controlAirDataAiding();
|
||||
|
||||
// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
|
||||
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
|
||||
controlVelPosFusion();
|
||||
|
||||
}
|
||||
|
||||
void Ekf::controlExternalVisionAiding()
|
||||
{
|
||||
// external vision position aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// turn on use of external vision measurements for position and height
|
||||
_control_status.flags.ev_pos = true;
|
||||
ECL_INFO("EKF switching to external vision position fusion");
|
||||
// turn off other forms of height aiding
|
||||
// Check for new exernal vision data
|
||||
if (_ev_data_ready) {
|
||||
|
||||
// external vision position aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// turn on use of external vision measurements for position and height
|
||||
_control_status.flags.ev_pos = true;
|
||||
ECL_INFO("EKF switching to external vision position fusion");
|
||||
// turn off other forms of height aiding
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
// reset the position, height and velocity
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
resetHeight();
|
||||
}
|
||||
}
|
||||
|
||||
// external vision yaw aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// reset the yaw angle to the value from the observaton quaternion
|
||||
// get the roll, pitch, yaw estimates from the quaternion states
|
||||
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
|
||||
_state.quat_nominal(3));
|
||||
matrix::Euler<float> euler_init(q_init);
|
||||
|
||||
// get initial yaw from the observation quaternion
|
||||
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
|
||||
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
|
||||
matrix::Euler<float> euler_obs(q_obs);
|
||||
euler_init(2) = euler_obs(2);
|
||||
|
||||
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
||||
Quaternion quat_before_reset = _state.quat_nominal;
|
||||
|
||||
// calculate initial quaternion states for the ekf
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
|
||||
// calculate the amount that the quaternion has changed by
|
||||
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
|
||||
|
||||
// add the reset amount to the output observer buffered data
|
||||
outputSample output_states;
|
||||
unsigned output_length = _output_buffer.get_length();
|
||||
for (unsigned i=0; i < output_length; i++) {
|
||||
output_states = _output_buffer.get_from_index(i);
|
||||
output_states.quat_nominal *= _state_reset_status.quat_change;
|
||||
_output_buffer.push_to_index(i,output_states);
|
||||
}
|
||||
|
||||
// capture the reset event
|
||||
_state_reset_status.quat_counter++;
|
||||
|
||||
// flag the yaw as aligned
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
ECL_INFO("EKF switching to external vision yaw fusion");
|
||||
}
|
||||
}
|
||||
|
||||
// determine if we should use the height observation
|
||||
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
// reset the position, height and velocity
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
resetHeight();
|
||||
_control_status.flags.ev_hgt = true;
|
||||
_fuse_height = true;
|
||||
|
||||
}
|
||||
|
||||
// determine if we should use the horizontal position observations
|
||||
if (_control_status.flags.ev_pos) {
|
||||
_fuse_pos = true;
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
|
||||
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
|
||||
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
|
||||
}
|
||||
|
||||
// determine if we should use the yaw observation
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
fuseHeading();
|
||||
}
|
||||
}
|
||||
|
||||
// external vision yaw aiding selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
|
||||
// check for a exernal vision measurement that has fallen behind the fusion time horizon
|
||||
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
|
||||
// reset the yaw angle to the value from the observaton quaternion
|
||||
// get the roll, pitch, yaw estimates from the quaternion states
|
||||
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
|
||||
_state.quat_nominal(3));
|
||||
matrix::Euler<float> euler_init(q_init);
|
||||
|
||||
// get initial yaw from the observation quaternion
|
||||
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
|
||||
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
|
||||
matrix::Euler<float> euler_obs(q_obs);
|
||||
euler_init(2) = euler_obs(2);
|
||||
|
||||
// save a copy of the quaternion state for later use in calculating the amount of reset change
|
||||
Quaternion quat_before_reset = _state.quat_nominal;
|
||||
|
||||
// calculate initial quaternion states for the ekf
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
|
||||
// calculate the amount that the quaternion has changed by
|
||||
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
|
||||
|
||||
// add the reset amount to the output observer buffered data
|
||||
outputSample output_states;
|
||||
unsigned output_length = _output_buffer.get_length();
|
||||
for (unsigned i=0; i < output_length; i++) {
|
||||
output_states = _output_buffer.get_from_index(i);
|
||||
output_states.quat_nominal *= _state_reset_status.quat_change;
|
||||
_output_buffer.push_to_index(i,output_states);
|
||||
}
|
||||
|
||||
// capture the reset event
|
||||
_state_reset_status.quat_counter++;
|
||||
|
||||
// flag the yaw as aligned
|
||||
_control_status.flags.yaw_align = true;
|
||||
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetoemter fusion
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
ECL_INFO("EKF switching to external vision yaw fusion");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Ekf::controlOpticalFlowAiding()
|
||||
{
|
||||
// optical flow fusion mode selection logic
|
||||
// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
|
||||
if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
|
||||
&& (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) {
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
// Check for new optical flow data that has fallen behind the fusion time horizon
|
||||
if (_flow_data_ready) {
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
|
||||
&& !_control_status.flags.opt_flow // we are not yet using flow data
|
||||
&& _control_status.flags.tilt_align // we know our tilt attitude
|
||||
&& (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate
|
||||
{
|
||||
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
// If the heading is valid, start using optical flow aiding
|
||||
if (_control_status.flags.yaw_align) {
|
||||
// set the flag and reset the fusion timeout
|
||||
_control_status.flags.opt_flow = true;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
|
||||
// if we are not using GPS then the velocity and position states and covariances need to be set
|
||||
if (!_control_status.flags.gps) {
|
||||
// constrain height above ground to be above minimum possible
|
||||
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
|
||||
|
||||
// calculate absolute distance from focal point to centre of frame assuming a flat earth
|
||||
float range = heightAboveGndEst / _R_to_earth(2, 2);
|
||||
|
||||
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
|
||||
// we should have reliable OF measurements so
|
||||
// calculate X and Y body relative velocities from OF measurements
|
||||
Vector3f vel_optflow_body;
|
||||
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
|
||||
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
|
||||
vel_optflow_body(2) = 0.0f;
|
||||
|
||||
// rotate from body to earth frame
|
||||
Vector3f vel_optflow_earth;
|
||||
vel_optflow_earth = _R_to_earth * vel_optflow_body;
|
||||
|
||||
// take x and Y components
|
||||
_state.vel(0) = vel_optflow_earth(0);
|
||||
_state.vel(1) = vel_optflow_earth(1);
|
||||
|
||||
} else {
|
||||
_state.vel(0) = 0.0f;
|
||||
_state.vel(1) = 0.0f;
|
||||
}
|
||||
|
||||
// reset the velocity covariance terms
|
||||
zeroRows(P,4,5);
|
||||
zeroCols(P,4,5);
|
||||
|
||||
// reset the horizontal velocity variance using the optical flow noise variance
|
||||
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
|
||||
|
||||
if (!_control_status.flags.in_air) {
|
||||
// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
|
||||
_state.pos(0) = 0.0f;
|
||||
_state.pos(1) = 0.0f;
|
||||
|
||||
// reset the corresponding covariances
|
||||
// we are by definition at the origin at commencement so variances are also zeroed
|
||||
zeroRows(P,7,8);
|
||||
zeroCols(P,7,8);
|
||||
|
||||
// align the output observer to the EKF states
|
||||
alignOutputFilter();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
|
||||
_control_status.flags.opt_flow = false;
|
||||
|
||||
}
|
||||
|
||||
// If the heading is valid, start using optical flow aiding
|
||||
if (_control_status.flags.yaw_align) {
|
||||
// set the flag and reset the fusion timeout
|
||||
_control_status.flags.opt_flow = true;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
|
||||
// Switch to the non-aiding mode, zero the velocity states
|
||||
// and set the synthetic position to the current estimate
|
||||
_control_status.flags.opt_flow = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
|
||||
// if we are not using GPS then the velocity and position states and covariances need to be set
|
||||
if (!_control_status.flags.gps) {
|
||||
// constrain height above ground to be above minimum possible
|
||||
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
|
||||
|
||||
// calculate absolute distance from focal point to centre of frame assuming a flat earth
|
||||
float range = heightAboveGndEst / _R_to_earth(2, 2);
|
||||
|
||||
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
|
||||
// we should have reliable OF measurements so
|
||||
// calculate X and Y body relative velocities from OF measurements
|
||||
Vector3f vel_optflow_body;
|
||||
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
|
||||
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
|
||||
vel_optflow_body(2) = 0.0f;
|
||||
|
||||
// rotate from body to earth frame
|
||||
Vector3f vel_optflow_earth;
|
||||
vel_optflow_earth = _R_to_earth * vel_optflow_body;
|
||||
|
||||
// take x and Y components
|
||||
_state.vel(0) = vel_optflow_earth(0);
|
||||
_state.vel(1) = vel_optflow_earth(1);
|
||||
|
||||
} else {
|
||||
_state.vel(0) = 0.0f;
|
||||
_state.vel(1) = 0.0f;
|
||||
}
|
||||
|
||||
// reset the velocity covariance terms
|
||||
zeroRows(P,4,5);
|
||||
zeroCols(P,4,5);
|
||||
|
||||
// reset the horizontal velocity variance using the optical flow noise variance
|
||||
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
|
||||
|
||||
if (!_control_status.flags.in_air) {
|
||||
// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
|
||||
_state.pos(0) = 0.0f;
|
||||
_state.pos(1) = 0.0f;
|
||||
|
||||
// reset the corresponding covariances
|
||||
// we are by definition at the origin at commencement so variances are also zeroed
|
||||
zeroRows(P,7,8);
|
||||
zeroCols(P,7,8);
|
||||
|
||||
// align the output observer to the EKF states
|
||||
alignOutputFilter();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
|
||||
_control_status.flags.opt_flow = false;
|
||||
}
|
||||
// fuse the data
|
||||
if (_control_status.flags.opt_flow) {
|
||||
// Update optical flow bias estimates
|
||||
calcOptFlowBias();
|
||||
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
|
||||
// Switch to the non-aiding mode, zero the velocity states
|
||||
// and set the synthetic position to the current estimate
|
||||
_control_status.flags.opt_flow = false;
|
||||
// Fuse optical flow LOS rate observations into the main filter
|
||||
fuseOptFlow();
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGpsAiding()
|
||||
{
|
||||
// GPS fusion mode selection logic
|
||||
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
|
||||
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
|
||||
if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
|
||||
&& (_time_last_imu - _last_gps_fail_us > 5e6)) {
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
// Check for new GPS data that has fallen behind the fusion time horizon
|
||||
if (_gps_data_ready) {
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
|
||||
if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
|
||||
}
|
||||
|
||||
// If the heading is valid start using gps aiding
|
||||
if (_control_status.flags.yaw_align) {
|
||||
_control_status.flags.gps = true;
|
||||
_time_last_gps = _time_last_imu;
|
||||
|
||||
// if we are not already aiding with optical flow, then we need to reset the position and velocity
|
||||
if (!_control_status.flags.opt_flow) {
|
||||
if (resetPosition() && resetVelocity()) {
|
||||
_control_status.flags.gps = true;
|
||||
|
||||
} else {
|
||||
_control_status.flags.gps = false;
|
||||
|
||||
}
|
||||
}
|
||||
if (_control_status.flags.gps) {
|
||||
ECL_INFO("EKF commencing GPS aiding");
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If the heading is valid start using gps aiding
|
||||
if (_control_status.flags.yaw_align) {
|
||||
_control_status.flags.gps = true;
|
||||
_time_last_gps = _time_last_imu;
|
||||
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
||||
_control_status.flags.gps = false;
|
||||
|
||||
}
|
||||
|
||||
// handle the case when we are relying on GPS fusion and lose it
|
||||
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
|
||||
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
|
||||
if (_time_last_imu - _time_last_gps > 5e5) {
|
||||
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
|
||||
// and set the synthetic GPS position to the current estimate
|
||||
_control_status.flags.gps = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
ECL_WARN("EKF GPS fusion timout - stopping GPS aiding");
|
||||
|
||||
} else {
|
||||
// Reset states to the last GPS measurement
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
|
||||
// if we are not already aiding with optical flow, then we need to reset the position and velocity
|
||||
if (!_control_status.flags.opt_flow) {
|
||||
_control_status.flags.gps = resetPosition();
|
||||
_control_status.flags.gps = resetVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
||||
_control_status.flags.gps = false;
|
||||
}
|
||||
// Only use GPS data for position and velocity aiding if enabled
|
||||
if (_control_status.flags.gps) {
|
||||
_fuse_pos = true;
|
||||
_fuse_vert_vel = true;
|
||||
_fuse_hor_vel = true;
|
||||
|
||||
// handle the case when we are relying on GPS fusion and lose it
|
||||
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
|
||||
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
|
||||
if (_time_last_imu - _time_last_gps > 5e5) {
|
||||
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
|
||||
// and set the synthetic GPS position to the current estimate
|
||||
_control_status.flags.gps = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
// correct velocity for offset relative to IMU
|
||||
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
|
||||
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body);
|
||||
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
_gps_sample_delayed.vel -= vel_offset_earth;
|
||||
|
||||
} else {
|
||||
// Reset states to the last GPS measurement
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
// correct position and height for offset relative to IMU
|
||||
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
|
||||
_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
|
||||
_gps_sample_delayed.hgt += pos_offset_earth(2);
|
||||
|
||||
}
|
||||
|
||||
// Determine if GPS should be used as the height source
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && !_gps_hgt_faulty) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
_fuse_height = true;
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -336,31 +460,39 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// set height sensor health
|
||||
_baro_hgt_faulty = true;
|
||||
_gps_hgt_faulty = false;
|
||||
|
||||
// declare the GPS height healthy
|
||||
_gps_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF baro hgt timeout - reset to GPS");
|
||||
|
||||
} else if (reset_to_baro){
|
||||
// set height sensor health
|
||||
_baro_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF baro hgt timeout - reset to baro");
|
||||
|
||||
} else {
|
||||
// we have nothing we can reset to
|
||||
// deny a reset
|
||||
reset_height = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -370,6 +502,7 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
gpsSample gps_init = _gps_buffer.get_newest();
|
||||
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
|
||||
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
|
||||
|
||||
// check the baro height source for consistency and freshness
|
||||
baroSample baro_init = _baro_buffer.get_newest();
|
||||
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
||||
@@ -389,28 +522,35 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// set height sensor health
|
||||
_gps_hgt_faulty = true;
|
||||
_baro_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF gps hgt timeout - reset to baro");
|
||||
|
||||
} else if (reset_to_gps) {
|
||||
// set height sensor health
|
||||
_gps_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF gps hgt timeout - reset to GPS");
|
||||
|
||||
} else {
|
||||
// we have nothing to reset to
|
||||
reset_height = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -419,6 +559,7 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// check if range finder data is available
|
||||
rangeSample rng_init = _range_buffer.get_newest();
|
||||
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
|
||||
|
||||
// check if baro data is available
|
||||
baroSample baro_init = _baro_buffer.get_newest();
|
||||
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
||||
@@ -433,28 +574,35 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// set height sensor health
|
||||
_rng_hgt_faulty = true;
|
||||
_baro_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF rng hgt timeout - reset to baro");
|
||||
|
||||
} else if (reset_to_rng) {
|
||||
// set height sensor health
|
||||
_rng_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF rng hgt timeout - reset to rng hgt");
|
||||
|
||||
} else {
|
||||
// we have nothing to reset to
|
||||
reset_height = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -463,6 +611,7 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// check if vision data is available
|
||||
extVisionSample ev_init = _ext_vision_buffer.get_newest();
|
||||
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
|
||||
|
||||
// check if baro data is available
|
||||
baroSample baro_init = _baro_buffer.get_newest();
|
||||
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
|
||||
@@ -477,26 +626,32 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// set height sensor health
|
||||
_rng_hgt_faulty = true;
|
||||
_baro_hgt_faulty = false;
|
||||
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF ev hgt timeout - reset to baro");
|
||||
|
||||
} else if (reset_to_ev) {
|
||||
// reset the height mode
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = true;
|
||||
|
||||
// request a reset
|
||||
reset_height = true;
|
||||
ECL_INFO("EKF ev hgt timeout - reset to ev hgt");
|
||||
|
||||
} else {
|
||||
// we have nothing to reset to
|
||||
reset_height = false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -505,58 +660,99 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
resetHeight();
|
||||
// Reset the timout timer
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlHeightAiding()
|
||||
void Ekf::controlBaroAiding()
|
||||
{
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
controlHeightSensorTimeouts();
|
||||
if (_baro_data_ready) {
|
||||
// determine if we should use the baro as our height source
|
||||
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && !_baro_hgt_faulty) {
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
_fuse_height = true;
|
||||
|
||||
// Control the source of height measurements for the main filter
|
||||
// do not switch to a sensor if it is unhealthy or the data is stale
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
|
||||
!_baro_hgt_faulty &&
|
||||
(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
|
||||
}
|
||||
|
||||
_control_status.flags.baro_hgt = true;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
|
||||
if (!_control_status.flags.baro_hgt) {
|
||||
float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
|
||||
local_time_step = math::constrain(local_time_step,0.0f,1.0f);
|
||||
last_baro_time_us = _baro_sample_delayed.time_us;
|
||||
float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset;
|
||||
_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
|
||||
!_gps_hgt_faulty &&
|
||||
(((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = true;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
void Ekf::controlRangeFinderAiding()
|
||||
{
|
||||
// determine if we should use range finder data for height
|
||||
if (_range_data_ready) {
|
||||
// set the height data source to range if requested
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) {
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
|
||||
!_rng_hgt_faulty &&
|
||||
(((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) {
|
||||
}
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_control_status.flags.ev_hgt = false;
|
||||
// correct the range data for position offset relative to the IMU
|
||||
Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body;
|
||||
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
|
||||
_range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2);
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
|
||||
(((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) {
|
||||
// always fuse available range finder data into a terrain height estimator if the estimator has been initialised
|
||||
if (_terrain_initialised) {
|
||||
fuseHagl();
|
||||
|
||||
_control_status.flags.baro_hgt = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_control_status.flags.ev_hgt = true;
|
||||
}
|
||||
|
||||
// only use range finder as a height observation in the main filter if specifically enabled
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
_fuse_height = true;
|
||||
|
||||
}
|
||||
|
||||
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
if (!_control_status.flags.in_air) {
|
||||
_range_sample_delayed.rng = _params.rng_gnd_clearance;
|
||||
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
|
||||
|
||||
}
|
||||
|
||||
_fuse_height = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlAirDataAiding()
|
||||
{
|
||||
// TODO This is just to get the logic inside but we will only start fusion once we tested this again
|
||||
//if (_tas_data_ready) {
|
||||
if (false) {
|
||||
fuseAirspeed();
|
||||
|
||||
}
|
||||
|
||||
// If we are on ground, store the local position and time to use as a reference for takeoff checks
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
// control airspeed fusion - TODO move to a function
|
||||
// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
|
||||
if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
|
||||
_control_status.flags.wind = false;
|
||||
|
||||
} else {
|
||||
_control_status.flags.wind = true;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -567,65 +763,103 @@ void Ekf::controlMagAiding()
|
||||
return;
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
// or the more accurate 3-axis fusion
|
||||
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
|
||||
// start 3D fusion if in-flight and height has increased sufficiently
|
||||
// to be away from ground magnetic anomalies
|
||||
// don't switch back to heading fusion until we are back on the ground
|
||||
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
|
||||
bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
|
||||
// If we are on ground, store the local position and time to use as a reference
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
|
||||
if (use_3D_fusion && _control_status.flags.tilt_align) {
|
||||
}
|
||||
|
||||
// checs for new magnetometer data tath has fallen beind the fusion time horizon
|
||||
if (_mag_data_ready) {
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
// or the more accurate 3-axis fusion
|
||||
if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) {
|
||||
// start 3D fusion if in-flight and height has increased sufficiently
|
||||
// to be away from ground magnetic anomalies
|
||||
// don't switch back to heading fusion until we are back on the ground
|
||||
bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f;
|
||||
bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved);
|
||||
|
||||
if (use_3D_fusion && _control_status.flags.tilt_align) {
|
||||
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
// use 3D mag fusion when airborne
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// use heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
|
||||
// always use heading fusion
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
}
|
||||
|
||||
// use 3D mag fusion when airborne
|
||||
// always use 3-axis mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// use heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
|
||||
// always use heading fusion
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
|
||||
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
|
||||
_control_status.flags.mag_dec = true;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
// if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
} else {
|
||||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// always use 3-axis mag fusion
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
// fuse magnetometer data using the selected methods
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
|
||||
fuseMag();
|
||||
|
||||
} else {
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination();
|
||||
}
|
||||
|
||||
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time
|
||||
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
|
||||
_control_status.flags.mag_dec = true;
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
|
||||
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
|
||||
fuseHeading();
|
||||
|
||||
} else {
|
||||
// do no fusion at all
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlVelPosFusion()
|
||||
{
|
||||
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
|
||||
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
|
||||
if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos
|
||||
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
|
||||
_fuse_pos = true;
|
||||
_time_last_fake_gps = _time_last_imu;
|
||||
|
||||
}
|
||||
|
||||
// Fuse available NED velocity and position data into the main filter
|
||||
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
|
||||
fuseVelPosHeight();
|
||||
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid
|
||||
if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) {
|
||||
_control_status.flags.wind = false;
|
||||
} else {
|
||||
_control_status.flags.wind = true;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user