From 5a8ed5fb5193916becc8e0156faca71027e91055 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 27 Mar 2024 14:22:19 +0900
Subject: [PATCH] AP_NavEKF: option to align extnav to optflow pos estimate

---
 libraries/AP_NavEKF/AP_NavEKF_Source.cpp | 9 +++++----
 libraries/AP_NavEKF/AP_NavEKF_Source.h   | 6 +++++-
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
index 1132168e469c8..72e55a965733a 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp
@@ -135,7 +135,7 @@ const AP_Param::GroupInfo AP_NavEKF_Source::var_info[] = {
     // @Param: _OPTIONS
     // @DisplayName: EKF Source Options
     // @Description: EKF Source Options
-    // @Bitmask: 0:FuseAllVelocities
+    // @Bitmask: 0:FuseAllVelocities, 1:AlignExtNavPosWhenUsingOptFlow
     // @User: Advanced
     AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),
 
@@ -257,11 +257,12 @@ void AP_NavEKF_Source::align_inactive_sources()
         return;
     }
 
-    // consider aligning XY position:
+    // consider aligning ExtNav XY position:
     bool align_posxy = false;
     if ((getPosXYSource() == SourceXY::GPS) ||
-        (getPosXYSource() == SourceXY::BEACON)) {
-        // only align position if active source is GPS or Beacon
+        (getPosXYSource() == SourceXY::BEACON) ||
+        ((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW))) {
+        // align ExtNav position if active source is GPS, Beacon or (optionally) Optflow
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].posxy == SourceXY::EXTNAV) {
                 // ExtNav could potentially be used, so align it
diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.h b/libraries/AP_NavEKF/AP_NavEKF_Source.h
index b8bfa67d18a8d..bbb1d9067f849 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_Source.h
+++ b/libraries/AP_NavEKF/AP_NavEKF_Source.h
@@ -47,7 +47,8 @@ class AP_NavEKF_Source
 
     // enum for OPTIONS parameter
     enum class SourceOptions {
-        FUSE_ALL_VELOCITIES = (1 << 0)  // fuse all velocities configured in source sets
+        FUSE_ALL_VELOCITIES = (1 << 0),                 // fuse all velocities configured in source sets
+        ALIGN_EXTNAV_POS_WHEN_USING_OPTFLOW = (1 << 1)  // align position of inactive sources to ahrs when using optical flow
     };
 
     // initialisation
@@ -118,6 +119,9 @@ class AP_NavEKF_Source
         AP_Enum<SourceYaw> yaw;    // yaw source
     } _source_set[AP_NAKEKF_SOURCE_SET_MAX];
 
+    // helper to check if an option parameter bit has been set
+    bool option_is_set(SourceOptions option) const { return (_options.get() & int16_t(option)) != 0; }
+
     AP_Int16 _options;      // source options bitmask
 
     uint8_t active_source_set; // index of active source set