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 6, 2023 · 8a22207 · 8a22207
1 parent f2494ea
commit 8a22207
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 11 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
 };
 
@@ -1823,17 +1830,24 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
             should_use_gps = true;
         }
 #endif
-        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.
+        const bool have_3D_fix = AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D;
+        const bool is_armed = hal.util->get_soft_armed();
+        if (is_armed && should_use_gps && have_3D_fix &&
+            !option_set(Options::DISABLE_DCM_FALLBACK) &&
+            (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs)) {
+            /*
+              if the EKF is not fusing GPS and we have a 3D fix, and we have not disabled
+              DCM fallback then use DCM
+            */
+            return EKFType::NONE;
+        }
+        if (is_armed && should_use_gps &&
+            (!filt_state.flags.horiz_pos_abs || !filt_state.flags.horiz_pos_rel)) {
+            /*
+              if the EKF is reporting that it cannot do relative
+              aiding or provide an absolute position then fallback to
+              DCM dead-reckoning
+             */
             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 {