diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a18c0dec91e..79abd042d39d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -460,10 +460,9 @@ void Ekf::controlGpsFusion() // 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 + bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); 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 > (uint64_t)5e6)) { - + if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // 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); @@ -500,6 +499,12 @@ void Ekf::controlGpsFusion() } + // Handle the case where we are using GPS and another source of aiding and GPS is failing checks + if (_control_status.flags.gps && !gps_checks_passing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { + _control_status.flags.gps = false; + ECL_WARN("EKF GPS data quality poor - stopping use"); + } + // handle the case when we now have GPS, but have not been using it for an extended period if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time