GNSS yaw unit test: test fallback to non yaw aiding mode

test/sensor_simulator/ekf_wrapper.cpp

@@ -165,6 +165,11 @@ bool EkfWrapper::isIntendingMag3DFusion() const
 	return _ekf->control_status_flags().mag_3D;
 }
 
+void EkfWrapper::setMagFuseTypeNone()
+{
+	_ekf_params->mag_fusion_type = MAG_FUSE_TYPE_NONE;
+}
+
 bool EkfWrapper::isWindVelocityEstimated() const
 {
 	return _ekf->control_status_flags().wind;

test/sensor_simulator/ekf_wrapper.h

@@ -87,6 +87,7 @@ public:
 
 	bool isIntendingMagHeadingFusion() const;
 	bool isIntendingMag3DFusion() const;
+	void setMagFuseTypeNone();
 
 	void enableExternalVisionAlignment();
 	void disableExternalVisionAlignment();

test/test_EKF_gps_yaw.cpp

@@ -257,4 +257,15 @@ TEST_F(EkfGpsHeadingTest, stop_on_ground)
 	// should stop as well because the yaw is not aligned anymore
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+
+	// AND IF: the mag fusion type is set to NONE
+	_ekf_wrapper.setMagFuseTypeNone();
+
+	// WHEN: running without yaw aiding
+	const matrix::Vector<float, 4> quat_variance_before = _ekf_wrapper.getQuaternionVariance();
+	_sensor_simulator.runSeconds(20.0);
+	const matrix::Vector<float, 4> quat_variance_after = _ekf_wrapper.getQuaternionVariance();
+
+	// THEN: the yaw variance is constrained by fusing constant data
+	EXPECT_LT(quat_variance_after(3), quat_variance_before(3));
 }