GPS yaw: Extract and refactor GPS yaw control logic

- Rename GPS yaw fusion functions:
resetGpsAntYaw -> resetYawToGps
fuseGpsAntYaw -> fuseGpsYaw

- Move last fusion time in reset function

EKF/control.cpp

@@ -518,40 +518,7 @@ 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)
-		    || _is_gps_yaw_faulty) {
-
-			stopGpsYawFusion();
-
-		} else {
-			if (ISFINITE(_gps_sample_delayed.yaw)) {
-
-				if (!_control_status.flags.gps_yaw
-				    && _control_status.flags.tilt_align
-				    && !_gps_hgt_intermittent) {
-
-					// Activate GPS yaw fusion
-					if (resetGpsAntYaw()) {
-						_control_status.flags.yaw_align = true;
-						startGpsYawFusion();
-					}
-				}
-
-				if (_control_status.flags.gps_yaw) {
-					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)
-			    && _control_status.flags.gps_yaw) {
-				_is_gps_yaw_faulty = true;
-				stopGpsYawFusion();
-			}
-		}
+		controlGpsYawFusion();
 
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
@@ -761,6 +728,43 @@ void Ekf::controlGpsFusion()
 	}
 }
 
+void Ekf::controlGpsYawFusion()
+{
+	if (!(_params.fusion_mode & MASK_USE_GPSYAW)
+	    || _is_gps_yaw_faulty) {
+
+		stopGpsYawFusion();
+		return;
+	}
+
+	if (ISFINITE(_gps_sample_delayed.yaw)) {
+
+		if (_control_status.flags.gps_yaw) {
+			fuseGpsYaw();
+
+		} else {
+			// Try to activate GPS yaw fusion
+			if (_control_status.flags.tilt_align
+			    && !_gps_hgt_intermittent) {
+
+				if (resetYawToGps()) {
+					_control_status.flags.yaw_align = true;
+					startGpsYawFusion();
+				}
+			}
+		}
+	}
+
+	// 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 (_control_status.flags.gps_yaw
+	    && isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
+		_is_gps_yaw_faulty = true;
+		stopGpsYawFusion();
+	}
+}
+
 void Ekf::controlHeightSensorTimeouts()
 {
 	/*

EKF/ekf.h

@@ -553,11 +553,11 @@ private:
 	void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const float (&yaw_jacobian)[4]);
 
 	// fuse the yaw angle obtained from a dual antenna GPS unit
-	void fuseGpsAntYaw();
+	void fuseGpsYaw();
 
 	// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
 	// return true if the reset was successful
-	bool resetGpsAntYaw();
+	bool resetYawToGps();
 
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
@@ -691,6 +691,7 @@ private:
 
 	// control fusion of GPS observations
 	void controlGpsFusion();
+	void controlGpsYawFusion();
 
 	// control fusion of magnetometer observations
 	void controlMagFusion();

EKF/gps_yaw_fusion.cpp

@@ -45,7 +45,7 @@
 #include <mathlib/mathlib.h>
 #include <cstdlib>
 
-void Ekf::fuseGpsAntYaw()
+void Ekf::fuseGpsYaw()
 {
 	// assign intermediate state variables
 	const float q0 = _state.quat_nominal(0);
@@ -262,7 +262,7 @@ void Ekf::fuseGpsAntYaw()
 	}
 }
 
-bool Ekf::resetGpsAntYaw()
+bool Ekf::resetYawToGps()
 {
 	// define the predicted antenna array vector and rotate into earth frame
 	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
@@ -279,5 +279,7 @@ bool Ekf::resetGpsAntYaw()
 	const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance, true);
 
+	_time_last_gps_yaw_fuse = _time_last_imu;
+
 	return true;
 }