ekf2-test-gnss-yaw: add unit test for fix type requirement

Heading data is already available in "float" mode but really inaccurate
(can be off by 45 degrees). It's then better to not use it when in
"float" mode as it will disturb the EKF2's estimate and the filter can
continue to keep an accurate heading even without direct observations.

src/modules/ekf2/EKF/common.h

@@ -193,6 +193,7 @@ struct gpsSample {
 	float       vacc{};     ///< 1-std vertical position error (m)
 	float       sacc{};     ///< 1-std speed error (m/sec)
 	float       yaw_acc{};  ///< 1-std yaw error (rad)
+	uint8_t     fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic
 };
 
 struct magSample {

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -178,6 +178,8 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 
 		gps_sample_new.hgt = (float)gps.alt * 1e-3f;
 
+		gps_sample_new.fix_type = gps.fix_type;
+
 #if defined(CONFIG_EKF2_GNSS_YAW)
 
 		if (PX4_ISFINITE(gps.yaw)) {

src/modules/ekf2/EKF/gps_control.cpp

@@ -328,7 +328,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
 
 	if (is_new_data_available) {
 
-		const bool continuing_conditions_passing = !gps_checks_failing;
+		const bool continuing_conditions_passing = !gps_checks_failing
+				&& gps_sample.fix_type >= 6; // RTK "fixed" is required for accurate heading
 
 		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
 				2 * GNSS_YAW_MAX_INTERVAL);

src/modules/ekf2/test/test_EKF_gps_yaw.cpp

@@ -69,6 +69,7 @@ public:
 
 		_sensor_simulator.runSeconds(_init_duration_s);
 		_sensor_simulator._gps.setYaw(NAN);
+		_sensor_simulator._gps.setFixType(6);
 		_sensor_simulator.runSeconds(2);
 		_ekf_wrapper.enableGpsFusion();
 		_ekf_wrapper.enableGpsHeadingFusion();
@@ -218,6 +219,30 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
 	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 }
 
+TEST_F(EkfGpsHeadingTest, fixTypeDrop)
+{
+	// GIVEN: valid yaw data with a good fix
+	float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle());
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator._gps.setFixType(6); // "RTk fixed"
+
+	// WHEN: the GPS yaw fusion is activated
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: GPS heading fusion should have started, and mag
+	// fusion should be disabled
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+
+	// BUT WHEN: the GPS fix drops to "RTK float"
+	_sensor_simulator._gps.setFixType(5);
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the fusion should stop and
+	// the estimator should fall back to mag fusion
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
+}
+
 TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one (e.g.: wrong orientation of the antenna array) and no mag.