GNSS yaw: unset yaw_align if fusion stops before takeoff

We do not want to rely on a potentially badly initialized heading as it
could lead to a flyaway directly after takeoff

Also remove the resetMagHeading() call from the GPS fusion control logic
as this is properly handled in mag_control.

EKF/control.cpp

@@ -524,27 +524,20 @@ void Ekf::controlGpsFusion()
 				const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
 								       _control_status.flags.ev_yaw ||
 								       _mag_inhibit_yaw_reset_req;
-				if (want_to_reset_mag_heading && canResetMagHeading()) {
-					_control_status.flags.ev_yaw = false;
-					_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
-					// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
-					// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
-					if (_mag_inhibit_yaw_reset_req) {
-						_mag_inhibit_yaw_reset_req = false;
-						// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
-						P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
-					}
-				}
 
-				// If the heading is valid start using gps aiding
-				if (_control_status.flags.yaw_align) {
+				if (want_to_reset_mag_heading) {
+					_mag_yaw_reset_req = true;
+					_control_status.flags.ev_yaw = false;
+
+				} else {
+					// If the heading is valid start using gps aiding
 					startGpsFusion();
 				}
 			}
 
-		}  else if (!(_params.fusion_mode & MASK_USE_GPS)) {
-			_control_status.flags.gps = false;
-
+		}  else if (_control_status.flags.gps
+		            && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) {
+			stopGpsFusion();
 		}
 
 		// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
@@ -725,14 +718,12 @@ void Ekf::controlGpsFusion()
 
 	} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
 		stopGpsFusion();
-		stopGpsYawFusion();
 		_warning_events.flags.gps_data_stopped = true;
 		ECL_WARN("GPS data stopped");
 	}  else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
 		// Handle the case where we are fusing another position source along GPS,
 		// stop waiting for GPS after 1 s of lost signal
 		stopGpsFusion();
-		stopGpsYawFusion();
 		_warning_events.flags.gps_data_stopped_using_alternate = true;
 		ECL_WARN("GPS data stopped, using only EV, OF or air data" );
 	}
@@ -812,6 +803,14 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
 		// No yaw data in the message anymore. Stop until it comes back.
 		stopGpsYawFusion();
 	}
+
+	// Before takeoff, we do not want to continue to rely on the current heading
+	// if we had to stop the fusion
+	if (!_control_status.flags.in_air
+	    && !_control_status.flags.gps_yaw
+	    && _control_status_prev.flags.gps_yaw) {
+		_control_status.flags.yaw_align = false;
+	}
 }
 
 void Ekf::controlHeightSensorTimeouts()

EKF/mag_control.cpp

@@ -67,12 +67,14 @@ void Ekf::controlMagFusion()
 		_num_bad_flight_yaw_events = 0;
 	}
 
-	if (_control_status.flags.mag_fault || !_control_status.flags.yaw_align) {
+	if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) {
 		stopMagFusion();
 		return;
 	}
 
 	_mag_yaw_reset_req |= otherHeadingSourcesHaveStopped();
+	_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
+	_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
 
 	if (noOtherYawAidingThanMag() && _mag_data_ready) {
 		if (_control_status.flags.in_air) {
@@ -84,6 +86,11 @@ void Ekf::controlMagFusion()
 			runOnGroundYawReset();
 		}
 
+		if (!_control_status.flags.yaw_align) {
+			// Having the yaw aligned is mandatory to continue
+			return;
+		}
+
 		// Determine if we should use simple magnetic heading fusion which works better when
 		// there are large external disturbances or the more accurate 3-axis fusion
 		switch (_params.mag_fusion_type) {
@@ -139,7 +146,18 @@ void Ekf::runOnGroundYawReset()
 					       ? resetMagHeading(_mag_lpf.getState())
 					       : false;
 
-		_mag_yaw_reset_req = !has_realigned_yaw;
+		if (has_realigned_yaw) {
+			_mag_yaw_reset_req = false;
+			_control_status.flags.yaw_align = true;
+
+			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
+			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
+			if (_mag_inhibit_yaw_reset_req) {
+				_mag_inhibit_yaw_reset_req = false;
+				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+			}
+		}
 	}
 }
 
@@ -156,8 +174,20 @@ void Ekf::runInAirYawReset()
 		if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
 		else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
 
-		_mag_yaw_reset_req = !has_realigned_yaw;
-		_control_status.flags.mag_aligned_in_flight = has_realigned_yaw;
+		if (has_realigned_yaw) {
+			_mag_yaw_reset_req = false;
+			_control_status.flags.yaw_align = true;
+			_control_status.flags.mag_aligned_in_flight = true;
+
+			// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
+			// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
+			if (_mag_inhibit_yaw_reset_req) {
+				_mag_inhibit_yaw_reset_req = false;
+				// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
+				P.uncorrelateCovarianceSetVariance<1>(12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
+			}
+		}
+
 	}
 }
 

test/test_EKF_gps_yaw.cpp

@@ -239,3 +239,22 @@ TEST_F(EkfGpsHeadingTest, yaw_jump)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
 }
+
+TEST_F(EkfGpsHeadingTest, stop_on_ground)
+{
+	// GIVEN: the GPS yaw fusion activated and there is no mag data
+	_sensor_simulator._mag.stop();
+	float gps_heading = _ekf_wrapper.getYawAngle();
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(5);
+
+	// WHEN: the measurement stops
+	gps_heading = NAN;
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(7.5);
+
+	// THEN: the fusion should stop and the GPS pos/vel aiding
+	// should stop as well because the yaw is not aligned anymore
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+}