From 4ab78230e67c9aef863c5eaa1173c3b9cd05a8da Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 26 Feb 2018 14:56:20 +1100 Subject: [PATCH] EKF: Add persistence criteria to GPS fail check --- EKF/control.cpp | 3 ++- EKF/ekf.h | 1 + EKF/gps_checks.cpp | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 79abd042d39d..8a18c6565513 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -461,6 +461,7 @@ 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); + bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6); if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { 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 @@ -500,7 +501,7 @@ 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)) { + if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data quality poor - stopping use"); } diff --git a/EKF/ekf.h b/EKF/ekf.h index c7bd4ff6ce65..e3877f5accc5 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -372,6 +372,7 @@ class Ekf : public EstimatorInterface float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec) float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec) uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks + uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 139ceadb45c6..c9cbc5e66cd0 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -228,6 +228,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { _last_gps_fail_us = _time_last_imu; + } else { + _last_gps_pass_us = _time_last_imu; } // continuous period without fail of 10 seconds required to return a healthy status