ekf2: handle GPS vel/pos timeout in addition to overall reset check

- in a system with good EV + GPS it's possible that horizontal vel/pos
fusion continues successfully, but GPS fusion is failing due to mediocre
yaw alignment
 - GPS always check for yaw failure and act on it if there's
corresponding GPS vel or GPS pos fusion timeout
 - add additional protections to prevent multiple yaw resets/alignments
per update (the controllers won't get the proper heading delta if this
happens)

src/modules/ekf2/EKF/control.cpp

@@ -86,10 +86,7 @@ void Ekf::controlFusionModes()
 	}
 
 	if (_gps_buffer) {
-		_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
-
 		// check for arrival of new sensor data at the fusion time horizon
-		_time_prev_gps_us = _gps_sample_delayed.time_us;
 		_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 
 		if (_gps_data_ready) {
@@ -103,8 +100,6 @@ void Ekf::controlFusionModes()
 			const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
 			_gps_sample_delayed.pos -= pos_offset_earth.xy();
 			_gps_sample_delayed.hgt += pos_offset_earth(2);
-
-			_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
 		}
 	}
 
@@ -210,13 +205,14 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 		const bool continuing_conditions_passing = !gps_checks_failing;
 
+		const bool gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
 		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GPS_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _control_status.flags.tilt_align
 				&& gps_checks_passing
 				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
+				&& !gps_intermittent;
 
 		if (_control_status.flags.gps_yaw) {
 

src/modules/ekf2/EKF/ekf.h

@@ -632,7 +632,6 @@ private:
 
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
-	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)
@@ -1015,8 +1014,6 @@ private:
 	void stopAirspeedFusion();
 
 	void stopGpsFusion();
-	void stopGpsPosFusion();
-	void stopGpsVelFusion();
 
 	void startGpsYawFusion(const gpsSample &gps_sample);
 	void stopGpsYawFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1220,8 +1220,10 @@ void Ekf::stopAirspeedFusion()
 void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
-		stopGpsPosFusion();
-		stopGpsVelFusion();
+		ECL_INFO("stopping GPS position and velocity fusion");
+
+		resetEstimatorAidStatus(_aid_src_gnss_pos);
+		resetEstimatorAidStatus(_aid_src_gnss_vel);
 
 		_control_status.flags.gps = false;
 	}
@@ -1235,23 +1237,6 @@ void Ekf::stopGpsFusion()
 	_inhibit_ev_yaw_use = false;
 }
 
-void Ekf::stopGpsPosFusion()
-{
-	if (_control_status.flags.gps) {
-		ECL_INFO("stopping GPS position fusion");
-		_control_status.flags.gps = false;
-
-		resetEstimatorAidStatus(_aid_src_gnss_pos);
-	}
-}
-
-void Ekf::stopGpsVelFusion()
-{
-	ECL_INFO("stopping GPS velocity fusion");
-
-	resetEstimatorAidStatus(_aid_src_gnss_vel);
-}
-
 void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
 {
 	if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
@@ -1336,30 +1321,25 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 // Returns true if the reset was successful
 bool Ekf::resetYawToEKFGSF()
 {
-	if (!isYawEmergencyEstimateAvailable()) {
+	if (!isYawEmergencyEstimateAvailable() || !isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) {
 		return false;
 	}
 
+	// prevent a reset being performed more than once on the same frame
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
+		return false;
+	}
+
+	// otherwise reset yaw
 	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
+	_information_events.flags.yaw_aligned_to_imu_gps = true;
+	ECL_INFO("Yaw aligned using IMU and GPS");
+
 	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;
 	_control_status.flags.yaw_align = true;
 
-	if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
-		// stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around
-		// and cause another navigation failure
-		_control_status.flags.mag_fault = true;
-		_warning_events.flags.emergency_yaw_reset_mag_stopped = true;
-
-	} else if (_control_status.flags.gps_yaw) {
-		_control_status.flags.gps_yaw_fault = true;
-		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-
-	} else if (_control_status.flags.ev_yaw) {
-		_inhibit_ev_yaw_use = true;
-	}
-
 	_ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us;
 	_ekfgsf_yaw_reset_count++;
 
@@ -1398,6 +1378,11 @@ void Ekf::runYawEKFGSF()
 
 	const Vector3f imu_gyro_bias = getGyroBias();
 	_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
+
+	// reset when landed
+	if (!_control_status.flags.in_air) {
+		_ekfgsf_yaw_reset_count = 0;
+	}
 }
 
 void Ekf::resetGpsDriftCheckFilters()

src/modules/ekf2/EKF/gps_control.cpp

@@ -92,8 +92,10 @@ void Ekf::controlGpsFusion()
 		_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
 
 		// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
-		if (gps_checks_passing && !gps_checks_failing) {
-			_yawEstimator.setVelocity(velocity.xy(), gps_sample.sacc);
+		if (_gps_speed_valid && velocity.isAllFinite()
+		    && (gps_sample.sacc > FLT_EPSILON) && (gps_sample.sacc <= _params.req_sacc)) {
+
+			_yawEstimator.setVelocity(velocity.xy(), vel_noise);
 		}
 
 		// if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -203,23 +203,34 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
 
 bool Ekf::resetYawToGps(const float gnss_yaw)
 {
-	// define the predicted antenna array vector and rotate into earth frame
-	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
-
-	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+	// prevent a reset being performed more than once on the same frame
+	if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
 		return false;
 	}
 
-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
-	const float measured_yaw = gnss_yaw;
+	if (PX4_ISFINITE(gnss_yaw) && !_control_status.flags.gps_yaw_fault) {
+		// define the predicted antenna array vector and rotate into earth frame
+		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance);
+		// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+		if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+			return false;
+		}
 
-	_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
-	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
+		// GPS yaw measurement is alreday compensated for antenna offset in the driver
+		const float measured_yaw = gnss_yaw;
 
-	return true;
+		const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
+		resetQuatStateYaw(measured_yaw, yaw_variance);
+
+		_control_status.flags.yaw_align = true;
+
+		_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
+		_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
+
+		return true;
+	}
+
+	return false;
 }