From 2f0c2fa126f7de0ed4a04edfa0e00f5cc65be223 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 21 Mar 2022 15:41:31 -0400
Subject: [PATCH] ekf2: replace _inhibit_ev_yaw_use with local flag

---
 src/modules/ekf2/EKF/control.cpp     | 6 ++++--
 src/modules/ekf2/EKF/ekf.h           | 1 -
 src/modules/ekf2/EKF/ekf_helper.cpp  | 7 -------
 src/modules/ekf2/EKF/gps_control.cpp | 1 -
 4 files changed, 4 insertions(+), 11 deletions(-)

diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index 7771b669d4..54313c2322 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -201,7 +201,9 @@ void Ekf::controlExternalVisionFusion()
 			reset = true;
 		}
 
-		if (_inhibit_ev_yaw_use) {
+		bool inhibit_ev_yaw_use = _control_status.flags.gps || _control_status.flags.gps_yaw;
+
+		if (inhibit_ev_yaw_use) {
 			stopEvYawFusion();
 		}
 
@@ -232,7 +234,7 @@ void Ekf::controlExternalVisionFusion()
 		}
 
 		// external vision yaw aiding selection logic
-		if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
+		if (!inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw
 		    && _control_status.flags.tilt_align) {
 
 			// don't start using EV data unless data is arriving frequently
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index 518ce537fa..84804ea21b 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -371,7 +371,6 @@ private:
 	Vector2f _hpos_pred_prev{};		///< previous value of NE position state used by odometry fusion (m)
 	bool _hpos_prev_available{false};	///< true when previous values of the estimate and measurement are available for use
 	Dcmf _R_ev_to_ekf;			///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
-	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index d14ba0a9e1..7011116fb5 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1579,10 +1579,6 @@ void Ekf::stopGpsFusion()
 	if (_control_status.flags.gps_yaw) {
 		stopGpsYawFusion();
 	}
-
-	// We do not need to know the true North anymore
-	// EV yaw can start again
-	_inhibit_ev_yaw_use = false;
 }
 
 void Ekf::stopGpsPosFusion()
@@ -1767,9 +1763,6 @@ bool Ekf::resetYawToEKFGSF()
 	} else if (_control_status.flags.gps_yaw) {
 		_control_status.flags.gps_yaw_fault = true;
 		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;
-
-	} else if (_control_status.flags.ev_yaw) {
-		_inhibit_ev_yaw_use = true;
 	}
 
 	_ekfgsf_yaw_reset_time = _time_last_imu;
diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index 48a9f526fd..bbb0a923ed 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -130,7 +130,6 @@ void Ekf::controlGpsFusion()
 
 					// Stop the vision for yaw fusion and do not allow it to start again
 					stopEvYawFusion();
-					_inhibit_ev_yaw_use = true;
 
 				} else {
 					startGpsFusion();