mag_control: fuse fake heading during leveling fine alignment (#17964)

Co-authored-by: Daniel Agar <daniel@agar.ca>

src/modules/ekf2/EKF/ekf.h

@@ -395,7 +395,7 @@ private:
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation manoeuvre was detected
 	uint8_t _num_bad_flight_yaw_events{0};	///< number of times a bad heading has been detected in flight and required a yaw reset
 	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetometer use was inhibited
-	float _last_static_yaw{0.0f};		///< last yaw angle recorded when on ground motion checks were passing (rad)
+	float _last_static_yaw{NAN};		///< last yaw angle recorded when on ground motion checks were passing (rad)
 
 	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested

src/modules/ekf2/EKF/mag_control.cpp

@@ -41,23 +41,6 @@
 
 void Ekf::controlMagFusion()
 {
-	// handle undefined behaviour
-	if (_params.mag_fusion_type > MAG_FUSE_TYPE_NONE) {
-		return;
-	}
-
-	// 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) {
-		if (noOtherYawAidingThanMag()) {
-			_is_yaw_fusion_inhibited = true;
-			fuseHeading();
-		}
-
-		return;
-	}
-
 	checkMagFieldStrength();
 
 	// If we are on ground, reset the flight alignment flag so that the mag fields will be
@@ -67,8 +50,25 @@ void Ekf::controlMagFusion()
 		_num_bad_flight_yaw_events = 0;
 	}
 
-	if (_control_status.flags.mag_fault || !_control_status.flags.tilt_align) {
+	// 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
+	// Also fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low
+	if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE
+	    || _control_status.flags.mag_fault
+	    || !_control_status.flags.tilt_align) {
+
 		stopMagFusion();
+
+		if (noOtherYawAidingThanMag()) {
+			// TODO: setting _is_yaw_fusion_inhibited to true is required to tell
+			// fuseHeading to perform a "zero innovation heading fusion"
+			// We should refactor it to avoid using this flag here
+			_is_yaw_fusion_inhibited = true;
+			fuseHeading();
+			_is_yaw_fusion_inhibited = false;
+		}
+
 		return;
 	}
 

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -781,6 +781,11 @@ void Ekf::fuseHeading()
 				// Vehicle is at rest so use the last moving prediction as an observation
 				// to prevent the heading from drifting and to enable yaw gyro bias learning
 				// before takeoff.
+
+				if (!PX4_ISFINITE(_last_static_yaw)) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}
@@ -836,6 +841,11 @@ void Ekf::fuseHeading()
 				// Vehicle is at rest so use the last moving prediction as an observation
 				// to prevent the heading from drifting and to enable yaw gyro bias learning
 				// before takeoff.
+
+				if (!PX4_ISFINITE(_last_static_yaw)) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}