AP_NavEKF: add option to align ExtNav position when using OptFlow

libraries/AP_DAL/AP_DAL_VisualOdom.cpp

@@ -8,6 +8,15 @@
 #include "AP_DAL.h"
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 
+// request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
+void AP_DAL_VisualOdom::request_align_yaw_to_ahrs()
+{
+#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
+    auto *vo = AP::visualodom();
+    vo->request_align_yaw_to_ahrs();
+#endif
+}
+
 /*
    update position offsets to align to AHRS position
    should only be called when this library is not being used as the position source

libraries/AP_DAL/AP_DAL_VisualOdom.h

@@ -27,6 +27,9 @@ class AP_DAL_VisualOdom {
         return RVOH.pos_offset;
     }
 
+    // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
+    void request_align_yaw_to_ahrs();
+
     // update position offsets to align to AHRS position
     // should only be called when this library is not being used as the position source
     void align_position_to_ahrs(bool align_xy, bool align_z);

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
 }