From df3481be3aa0238c0a60df9f33cf5d5b110819e6 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 22 Jul 2021 11:33:45 +0200
Subject: [PATCH] mag_control: fuse fake heading during leveling fine alignment

---
 EKF/mag_control.cpp | 36 ++++++++++++++++++------------------
 EKF/mag_fusion.cpp  |  8 ++++++++
 2 files changed, 26 insertions(+), 18 deletions(-)

diff --git a/EKF/mag_control.cpp b/EKF/mag_control.cpp
index 500405536f..6ef0a990e3 100644
--- a/EKF/mag_control.cpp
+++ b/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;
 	}
 
diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 5a799f8119..3135b6a809 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -767,6 +767,10 @@ 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 (fabsf(_last_static_yaw) < 1e-6f) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}
@@ -818,6 +822,10 @@ 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 (fabsf(_last_static_yaw) < 1e-6f) {
+					_last_static_yaw = predicted_hdg;
+				}
+
 				measured_hdg = _last_static_yaw;
 
 			}