EKF: Add persistence criteria to GPS fail check

PX4 · May 7, 2018 · 4ab7823 · 4ab7823
1 parent 67d71ca
commit 4ab7823
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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