From 4fee059696b72d745a7e89df874aaf6c48a2999a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 21 Mar 2022 14:16:17 -0400
Subject: [PATCH] ekf2: simplify mag yaw reset request when transitioning to
 mag enabled

---
 src/modules/ekf2/EKF/ekf.h           |  4 ----
 src/modules/ekf2/EKF/mag_control.cpp | 31 ++++++++++------------------
 2 files changed, 11 insertions(+), 24 deletions(-)

diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 65e39b42fc..9684acdca6 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -449,7 +449,6 @@ private:
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
 	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements
-	bool _non_mag_yaw_aiding_running_prev{false};  ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS).
 	bool _is_yaw_fusion_inhibited{false};		///< true when yaw sensor use is being inhibited
 
 	SquareMatrix24f P{};	///< state covariance matrix
@@ -848,9 +847,6 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	bool noOtherYawAidingThanMag() const;
-	bool otherHeadingSourcesHaveStopped();
-
 	void checkHaglYawResetReq();
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp
index c829506d62..a165549ad1 100644
--- a/src/modules/ekf2/EKF/mag_control.cpp
+++ b/src/modules/ekf2/EKF/mag_control.cpp
@@ -86,17 +86,18 @@ void Ekf::controlMagFusion()
 		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 (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) {
+
+		const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D;
+
 		// 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) {
 		default:
-
-		/* fallthrough */
+		// FALLTHROUGH
 		case MagFuseType::AUTO:
 			selectMagAuto();
 			break;
@@ -113,6 +114,12 @@ void Ekf::controlMagFusion()
 			break;
 		}
 
+		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
+
+		if (!mag_enabled_previously && mag_enabled) {
+			_mag_yaw_reset_req = true;
+		}
+
 		if (_control_status.flags.in_air) {
 			checkHaglYawResetReq();
 			runInAirYawReset(mag_sample.mag);
@@ -133,12 +140,6 @@ void Ekf::controlMagFusion()
 	}
 }
 
-bool Ekf::noOtherYawAidingThanMag() const
-{
-	// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
-	return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw;
-}
-
 void Ekf::checkHaglYawResetReq()
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
@@ -378,13 +379,3 @@ void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 		}
 	}
 }
-
-bool Ekf::otherHeadingSourcesHaveStopped()
-{
-	// detect rising edge of noOtherYawAidingThanMag()
-	bool result = noOtherYawAidingThanMag() && _non_mag_yaw_aiding_running_prev;
-
-	_non_mag_yaw_aiding_running_prev = !noOtherYawAidingThanMag();
-
-	return  result;
-}