zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

master
Paul Riseborough 8 years ago
parent
63cf3d50be
commit
968cfae632
1 changed files with 26 additions and 24 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      EKF/control.cpp

50
EKF/control.cpp
Unescape 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 (_control_status.flags.gps && !_control_status.flags.opt_flow) {
if (_time_last_imu - _time_last_gps > 5e5) { // We are relying on GPS aiding to constrain attitude drift so after 7 seconds without aiding we need to do something
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states 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);
// 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 // Our position measurments have been rejected for more than 14 seconds
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); 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 if (do_reset) {
do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max; // Reset states to the last GPS measurement
resetPosition();
resetVelocity();
ECL_WARN("EKF GPS fusion timout - resetting to GPS");
if (do_reset) { // Reset the timeout counters
// Reset states to the last GPS measurement _time_last_pos_fuse = _time_last_imu;
resetPosition(); _time_last_vel_fuse = _time_last_imu;
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;
}
} }
} }
// Only use GPS data for position and velocity aiding if enabled // Only use GPS data for position and velocity aiding if enabled
@ -414,6 +403,19 @@ void Ekf::controlGpsFusion()
_control_status.flags.ev_hgt = false; _control_status.flags.ev_hgt = false;
_fuse_height = true; _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");
} }
} }
} }

Write Preview
Loading…
Cancel
Save