AP_NavEKF: option to align extnav to optflow pos estimate

rmackay9 · Jul 5, 2024 · fb44135 · fb44135
1 parent fd1d895
commit fb44135
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 3 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:AlignPosWhenUsingOptFlow
     // @User: Advanced
     AP_GROUPINFO("_OPTIONS", 16, AP_NavEKF_Source, _options, (int16_t)SourceOptions::FUSE_ALL_VELOCITIES),
 
@@ -260,7 +260,8 @@ void AP_NavEKF_Source::align_inactive_sources()
     // consider aligning XY position:
     bool align_posxy = false;
     if ((getPosXYSource() == SourceXY::GPS) ||
-        (getPosXYSource() == SourceXY::BEACON)) {
+        (getPosXYSource() == SourceXY::BEACON) ||
+        ((getVelXYSource() == SourceXY::OPTFLOW) && option_is_set(SourceOptions::ALIGN_WHEN_USING_OPTFLOW))) {
         // only align position if active source is GPS or Beacon
         for (uint8_t i=0; i<AP_NAKEKF_SOURCE_SET_MAX; i++) {
             if (_source_set[i].posxy == SourceXY::EXTNAV) {

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_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