AP_NavEKF: ExtNav yaw aligned when not used

ArduPilot · Mar 27, 2024 · 409f830 · 409f830
1 parent 256d1fc
commit 409f830
Showing 1 changed file with 14 additions and 0 deletions.
diff --git 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
 }