From 9a929e0857b1d88f47bcaf0c926d8ce1a9b965c1 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 23 Mar 2022 15:29:00 -0400
Subject: [PATCH] ekf2: mag in air reset fall back to regular resetMagHeading()
 if realignYawGPS() fails

---
 src/modules/ekf2/EKF/mag_control.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp
index 6a2ecd7632..93d34d0714 100644
--- a/src/modules/ekf2/EKF/mag_control.cpp
+++ b/src/modules/ekf2/EKF/mag_control.cpp
@@ -197,8 +197,9 @@ void Ekf::runInAirYawReset(const Vector3f &mag_sample)
 
 		if (_control_status.flags.gps && _control_status.flags.fixed_wing) {
 			has_realigned_yaw = realignYawGPS(mag_sample);
+		}
 
-		} else if (canResetMagHeading()) {
+		if (!has_realigned_yaw && canResetMagHeading()) {
 			has_realigned_yaw = resetMagHeading();
 		}