dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fix bug preventing fallback from GPS mode if GPS is lost

This commit is contained in:
Paul Riseborough 2016-10-25 08:32:01 +11:00
parent 63cf3d50be
commit 968cfae632
1 changed files with 26 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
EKF/control.cpp
View File

@ -353,36 +353,25 @@ void Ekf::controlGpsFusion()
}
// handle the case when we are relying on GPS fusion and lose it
// handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
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 {
// We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max);
// We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max);
// Our position measurments have been rejected for more than 14 seconds
do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max;
// Our position measurments have been rejected for more than 14 seconds
do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max;
if (do_reset) {
// Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
if (do_reset) {
// 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;
// Reset the timeout counters
_time_last_pos_fuse = _time_last_imu;
_time_last_vel_fuse = _time_last_imu;
}
}
}
// Only use GPS data for position and velocity aiding if enabled
@ -414,6 +403,19 @@ void Ekf::controlGpsFusion()
_control_status.flags.ev_hgt = false;
_fuse_height = true;
}
} else {
// handle the case where we do not have GPS and have not been using it for an extended period
if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6)) {
// if we don't have a source of aiding to constrain attitude drift,
// 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");
}
}
}