From ecdade363807c6c3571413817ce4f7e5ed1a0916 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 e9c3461484..c871801917 100644
--- a/src/modules/ekf2/EKF/mag_control.cpp
+++ b/src/modules/ekf2/EKF/mag_control.cpp
@@ -183,8 +183,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();
 		}