From 26f5490de4fb6b465517ddba75e01a881868ef0b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 27 Mar 2024 14:31:38 +0900
Subject: [PATCH] AP_NavEKF: ExtNav yaw aligned when not used

---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index ececde2d87..679215d296 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -288,6 +288,20 @@ void AP_NavEKF_Source::align_inactive_sources()
         }
     }
     visual_odom->align_position_to_ahrs(align_posxy, align_posz);
+
+    // consider aligning yaw:
+    if ((getYawSource() == SourceYaw::COMPASS) ||
+        (getYawSource() == SourceYaw::GPS) ||
+        (getYawSource() == SourceYaw::GPS_COMPASS_FALLBACK) ||
+        (getYawSource() == SourceYaw::GSF)) {
+        for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
+            if (_source_set[i].yaw == SourceYaw::EXTNAV) {
+                // ExtNav could potentially be used, so align it
+                visual_odom->request_align_yaw_to_ahrs();
+                break;
+            }
+        }
+    }
 #endif
 }