AP_NavEKF: option to align extnav to optflow pos estimate

ArduPilot · Jul 9, 2024 · 5a8ed5f · 5a8ed5f
1 parent 1249388
commit 5a8ed5f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 5 deletions.
diff --git 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
@@ -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