GPS Yaw: move isfinite checks in control.cpp

EKF/control.cpp

@@ -518,15 +518,17 @@ void Ekf::controlGpsFusion()
 		// Detect if coming back after significant time without GPS data
 		bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
-		// GPS yaw aiding selection logic
-		if ((_params.fusion_mode & MASK_USE_GPSYAW)
-				&& ISFINITE(_gps_sample_delayed.yaw)
+		if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
+			stopGpsYawFusion();
+
+		} else {
+			if (ISFINITE(_gps_sample_delayed.yaw)) {
+				if (!_control_status.flags.gps_yaw
 				    && _control_status.flags.tilt_align
-				&& !_control_status.flags.gps_yaw
 				    && !_gps_hgt_intermittent) {
 
+					// Activate GPS yaw fusion
 					if (resetGpsAntYaw()) {
-				// flag the yaw as aligned
 						_control_status.flags.yaw_align = true;
 
 						startGpsYawFusion();
@@ -535,10 +537,11 @@ void Ekf::controlGpsFusion()
 					}
 				}
 
-		// fuse the yaw observation
 				if (_control_status.flags.gps_yaw) {
 					fuseGpsAntYaw();
 				}
+			}
+		}
 
 		// 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

EKF/ekf.h

@@ -351,6 +351,8 @@ private:
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_fake_pos{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 
+	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
+
 	Vector2f _last_known_posNE;		///< last known local NE position vector (m)
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 

EKF/gps_yaw_fusion.cpp

@@ -58,8 +58,6 @@ void Ekf::fuseGpsAntYaw()
 	float H_YAW[4];
 	float measured_hdg;
 
-	// check if data has been set to NAN indicating no measurement
-	if (ISFINITE(_gps_sample_delayed.yaw)) {
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 	measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
 
@@ -130,11 +128,6 @@ void Ekf::fuseGpsAntYaw()
 	// using magnetic heading tuning parameter
 	R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
 
-	} else {
-		// there is nothing to fuse
-		return;
-	}
-
 	// wrap the heading to the interval between +-pi
 	measured_hdg = wrap_pi(measured_hdg);
 
@@ -210,7 +203,6 @@ void Ekf::fuseGpsAntYaw()
 	// we are no longer using 3-axis fusion so set the reported test levels to zero
 	memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
 
-	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 
@@ -273,6 +265,7 @@ void Ekf::fuseGpsAntYaw()
 
 	// only apply covariance and state corrections if healthy
 	if (healthy) {
+		_time_last_gps_yaw_fuse = _time_last_imu;
 		// apply the covariance corrections
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
@@ -291,9 +284,6 @@ void Ekf::fuseGpsAntYaw()
 
 bool Ekf::resetGpsAntYaw()
 {
-	// check if data has been set to NAN indicating no measurement
-	if (ISFINITE(_gps_sample_delayed.yaw)) {
-
 	// 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};
 	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
@@ -311,6 +301,3 @@ bool Ekf::resetGpsAntYaw()
 
 	return true;
 }
-
-	return false;
-}