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.

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());
+}