GPS Yaw: Always reset Yaw when GPS Yaw fusion is starting

Also avoid fusing fake mag data when an other source of yaw aiding
is active, even if in MAG_TYPE_NONE mode.

EKF/control.cpp

@@ -522,20 +522,14 @@ void Ekf::controlGpsFusion()
 		if ((_params.fusion_mode & MASK_USE_GPSYAW)
 				&& ISFINITE(_gps_sample_delayed.yaw)
 				&& _control_status.flags.tilt_align
-				&& (!_control_status.flags.gps_yaw || !_control_status.flags.yaw_align)
+				&& !_control_status.flags.gps_yaw
 				&& !_gps_hgt_intermittent) {
 
 			if (resetGpsAntYaw()) {
 				// flag the yaw as aligned
 				_control_status.flags.yaw_align = true;
 
-				// turn on fusion of external vision yaw measurements and disable all other yaw fusion
-				_control_status.flags.gps_yaw = true;
-				_control_status.flags.ev_yaw = false;
-				_control_status.flags.mag_dec = false;
-
-				stopMagHdgFusion();
-				stopMag3DFusion();
+				startGpsYawFusion();
 
 				ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
 			}

EKF/ekf.h

@@ -693,7 +693,7 @@ private:
 	void controlMagFusion();
 	void updateMagFilter();
 
-	bool canRunMagFusion() const;
+	bool noOtherYawAidingThanMag() const;
 
 	void checkHaglYawResetReq();
 	float getTerrainVPos() const;
@@ -842,6 +842,7 @@ private:
 
 	void stopGpsVelFusion();
 
+	void startGpsYawFusion();
 	void stopGpsYawFusion();
 
 	void stopEvFusion();

EKF/ekf_helper.cpp

@@ -1634,6 +1634,15 @@ void Ekf::stopGpsVelFusion()
 	_gps_vel_test_ratio.setZero();
 }
 
+void Ekf::startGpsYawFusion()
+{
+	_control_status.flags.mag_dec = false;
+	stopEvYawFusion();
+	stopMagHdgFusion();
+	stopMag3DFusion();
+	_control_status.flags.gps_yaw = true;
+}
+
 void Ekf::stopGpsYawFusion()
 {
 	_control_status.flags.gps_yaw = false;

EKF/gps_yaw_fusion.cpp

@@ -303,20 +303,11 @@ bool Ekf::resetGpsAntYaw()
 			return false;
 		}
 
-		const float predicted_yaw =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
-
 		// get measurement and correct for antenna array yaw offset
 		const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
 
-		// calculate the amount the yaw needs to be rotated by
-		const float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
-
-		// update the quaternion state estimates and corresponding covariances only if
-		// the change in angle has been large or the yaw is not yet aligned
-		if(fabsf(yaw_delta) > math::radians(15.0f) || !_control_status.flags.yaw_align) {
-			const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
-			resetQuatStateYaw(measured_yaw, yaw_variance, true);
-		}
+		const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+		resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
 		return true;
 	}

EKF/mag_control.cpp

@@ -46,12 +46,15 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
-	// When operating without a magnetometer, yaw fusion is run selectively to prevent
-	// enable yaw gyro bias learning hen stationary on ground and to prevent uncontrolled
-	// yaw variance growth
+	// When operating without a magnetometer and no other source of yaw aiding is active,
+	// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
+	// ground and to prevent uncontrolled yaw variance growth
 	if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) {
-		_yaw_use_inhibit = true;
-		fuseHeading();
+		if (noOtherYawAidingThanMag())
+		{
+			_yaw_use_inhibit = true;
+			fuseHeading();
+		}
 		return;
 	}
 
@@ -70,7 +73,7 @@ void Ekf::controlMagFusion()
 		return;
 	}
 
-	if (canRunMagFusion()) {
+	if (noOtherYawAidingThanMag() && _mag_data_ready) {
 		if (_control_status.flags.in_air) {
 			checkHaglYawResetReq();
 			runInAirYawReset();
@@ -116,11 +119,10 @@ void Ekf::updateMagFilter()
 	}
 }
 
-bool Ekf::canRunMagFusion() const
+bool Ekf::noOtherYawAidingThanMag() const
 {
-	// check for new magnetometer data that has fallen behind the fusion time horizon
 	// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
-	return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready;
+	return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw;
 }
 
 void Ekf::checkHaglYawResetReq()

test/test_EKF_gps_yaw.cpp

@@ -88,26 +88,32 @@ TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
 	_sensor_simulator.runSeconds(11);
 
 	// THEN: after a while the fusion should be stopped
-	// TODO: THIS IS NOT HAPPENING
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 }
 
-TEST_F(EkfGpsHeadingTest, fusionStartWithoutReset)
+TEST_F(EkfGpsHeadingTest, yawConvergence)
 {
-	// GIVEN:EKF that fuses GPS
-
-	// WHEN: enabling GPS heading fusion and heading difference is smaller than 15 degrees
-	const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
+	// 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();
-	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
-	_sensor_simulator.runSeconds(0.2);
+	_sensor_simulator.runSeconds(5);
 
-	// THEN: GPS heading fusion should have started;
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	// THEN: the estimate is reset and stays close to the measurement
+	float yaw_est = _ekf_wrapper.getYawAngle();
+	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.05f))
+		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
 
-	// AND: no reset to GPS heading should be performed
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+	// AND WHEN: the the measurement changes
+	gps_heading += math::radians(2.f);
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(6);
 
-	// TODO: investigate why heading is not converging exactly to GPS heading
+	// THEN: the estimate slowly converges to the new measurement
+	// Note that the process is slow, because the gyro did not detect any motion
+	yaw_est = _ekf_wrapper.getYawAngle();
+	EXPECT_NEAR(yaw_est, gps_heading, math::radians(0.5f))
+		<< "yaw est: " << math::degrees(yaw_est) << "gps yaw: " << math::degrees(gps_heading);
 }