ekf2: merge runOnGroundYawReset() + runInAirYawReset() into unified magReset()

src/modules/ekf2/EKF/ekf.h

@@ -890,11 +890,10 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	void checkHaglYawResetReq();
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
-	void runOnGroundYawReset();
-	void runInAirYawReset();
+	bool magReset();
+	bool haglYawResetReq();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();

src/modules/ekf2/EKF/mag_control.cpp

@@ -206,18 +206,18 @@ void Ekf::controlMagFusion()
 			break;
 		}
 
-		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
 
-		if ((!mag_enabled_previously && mag_enabled) || mag_sample.reset) {
-			_mag_yaw_reset_req = true;
-		}
+			if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
 
-		if (_control_status.flags.in_air) {
-			checkHaglYawResetReq();
-			runInAirYawReset();
+				if (magReset()) {
+					_mag_yaw_reset_req = false;
 
 				} else {
-			runOnGroundYawReset();
+					// mag reset failed, try again next time
+					_mag_yaw_reset_req = true;
+				}
+			}
 		}
 
 		if (!_control_status.flags.yaw_align) {
@@ -231,40 +231,29 @@ void Ekf::controlMagFusion()
 	}
 }
 
-void Ekf::checkHaglYawResetReq()
+bool Ekf::haglYawResetReq()
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
-	if (!_control_status.flags.mag_aligned_in_flight) {
+	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
-		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
+		return above_mag_anomalies;
 	}
+
+	return false;
 }
 
-void Ekf::runOnGroundYawReset()
-{
-	if (_mag_yaw_reset_req) {
-		const bool has_realigned_yaw = resetMagHeading();
-
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
-		}
-	}
-}
-
-void Ekf::runInAirYawReset()
+bool Ekf::magReset()
 {
 	// prevent a reset being performed more than once on the same frame
 	if ((_flt_mag_align_start_time == _time_delayed_us)
 	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
-		return;
+		return false;
 	}
 
-	if (_mag_yaw_reset_req) {
 	bool has_realigned_yaw = false;
 
 	// use yaw estimator if available
@@ -307,14 +296,19 @@ void Ekf::runInAirYawReset()
 	}
 
 	if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
 		_control_status.flags.yaw_align = true;
+
+		if (_control_status.flags.in_air) {
 			_control_status.flags.mag_aligned_in_flight = true;
 
 			// record the time for the magnetic field alignment event
 			_flt_mag_align_start_time = _time_delayed_us;
 		}
+
+		return true;
 	}
+
+	return false;
 }
 
 void Ekf::selectMagAuto()