GPS Yaw: fall back to other yaw aiding source in case of bad data

If the user selects GPS yaw fusion but that there is no GPS yaw data in
the GPS message or if the fusion is rejected for some time, the GPS yaw
data is declared faulty and the fusion is stopped to allow an other
source of yaw aiding to start.

EKF/control.cpp

@@ -518,7 +518,10 @@ void Ekf::controlGpsFusion()
 		// Detect if coming back after significant time without GPS data
 		bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
-		if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
+
+		if (!(_params.fusion_mode & MASK_USE_GPSYAW)
+		    || _is_gps_yaw_faulty) {
+
 			stopGpsYawFusion();
 
 		} else {
@@ -541,6 +544,14 @@ void Ekf::controlGpsFusion()
 					fuseGpsAntYaw();
 				}
 			}
+
+			// Check if the data is constantly fused by the estimator,
+			// if not, declare the sensor faulty and stop the fusion
+			// By doing this, another source of yaw aiding is allowed to start
+			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
+				_is_gps_yaw_faulty = true;
+				stopGpsYawFusion();
+			}
 		}
 
 		// Determine if we should use GPS aiding for velocity and horizontal position

EKF/ekf.h

@@ -499,6 +499,7 @@ private:
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use
 	bool _gps_hgt_intermittent{false};	///< true if gps height into the buffer is intermittent
+	bool _is_gps_yaw_faulty{false};		///< true if gps yaw data is rejected by the filter for too long
 
 	// imu fault status
 	uint64_t _time_bad_vert_accel{0};	///< last time a bad vertical accel was detected (uSec)

test/sensor_simulator/ekf_wrapper.cpp

@@ -170,6 +170,20 @@ void EkfWrapper::disableExternalVisionAlignment()
 	_ekf_params->fusion_mode &= ~MASK_ROTATE_EV;
 }
 
+bool EkfWrapper::isIntendingMagHeadingFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.mag_hdg;
+}
+
+bool EkfWrapper::isIntendingMag3DFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.mag_3D;
+}
+
 bool EkfWrapper::isWindVelocityEstimated() const
 {
 	filter_control_status_u control_status;

test/sensor_simulator/ekf_wrapper.h

@@ -84,6 +84,9 @@ public:
 	void disableExternalVisionHeadingFusion();
 	bool isIntendingExternalVisionHeadingFusion() const;
 
+	bool isIntendingMagHeadingFusion() const;
+	bool isIntendingMag3DFusion() const;
+
 	void enableExternalVisionAlignment();
 	void disableExternalVisionAlignment();
 

test/test_EKF_gps_yaw.cpp

@@ -117,3 +117,30 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f))
 		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
 }
+
+TEST_F(EkfGpsHeadingTest, fallBackToMag)
+{
+	// GIVEN: an initial GPS yaw, not aligned with the current one
+	float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
+	_sensor_simulator._gps.setYaw(gps_heading);
+
+	// WHEN: the GPS yaw fusion is activated
+	_ekf_wrapper.enableGpsHeadingFusion();
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: GPS heading fusion should have started, and mag
+	// fusion should be disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
+
+	// BUT WHEN: the GPS yaw is suddenly invalid
+	gps_heading = NAN;
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(6);
+
+	// THEN: after a few seconds, the fusion should stop and
+	// the estimator should fall back to mag fusion
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+}