AP_AHRS: added AHRS_OPTIONS parameter

the first option is to disable DCM fallback on fixed wing. This is suitable in environments with a high likelyhood of GPS interference
tridge · Sep 7, 2023 · 508a097 · 508a097
1 parent f2494ea
commit 508a097
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 4 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp
@@ -178,6 +178,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = {
 
     // index 17
 
+    // @Param: OPTIONS
+    // @DisplayName: Optional AHRS behaviour
+    // @Description: This conrols optional AHRS behaviour. Setting DisableDCMFallback will change the AHRS behaviour for fixed wing aircraft to not fall back to DCM when the EKF stops fusing GPS measurements while there is 3D GPS lock
+    // @Bitmask: 0:DisableDCMFallback
+    // @User: Advanced
+    AP_GROUPINFO("OPTIONS",  18, AP_AHRS, _options, 0),
+
     AP_GROUPEND
 };
 
@@ -1791,6 +1798,7 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
       wait for good GPS position on fixed wing and rover
      */
     if (ret != EKFType::NONE &&
+        !option_set(Options::DISABLE_DCM_FALLBACK) &&
         (_vehicle_class == VehicleClass::FIXED_WING ||
          _vehicle_class == VehicleClass::GROUND)) {
         if (!dcm.yaw_source_available() && !fly_forward) {
@@ -1826,14 +1834,13 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
         if (hal.util->get_soft_armed() &&
             (!filt_state.flags.using_gps ||
              !filt_state.flags.horiz_pos_abs) &&
-             !filt_state.flags.horiz_pos_rel &&
             should_use_gps &&
             AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
             // if the EKF is not fusing GPS or doesn't have a 2D fix
-            // and is not doing ground or wind relative dead reckoning
             // and we have a 3D lock, then plane and rover would
-            // prefer to use the GPS position from DCM.
-            // Note: This is a last resort fallback and makes the navigation highly vulnerable to GPS noise.
+            // prefer to use the GPS position from DCM. This is a
+            // safety net while some issues with the EKF get sorted
+            // out
             return EKFType::NONE;
         }
         if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) {

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
@@ -952,6 +952,15 @@ class AP_AHRS {
         Vector3f velocity_NED;
         bool velocity_NED_ok;
     } state;
+
+    enum class Options : uint16_t {
+        DISABLE_DCM_FALLBACK=(1U<<0),
+    };
+    AP_Int16 _options;
+
+    bool option_set(Options option) const {
+        return (_options & uint16_t(option)) != 0;
+    }
 };
 
 namespace AP {