AP_AHRS: Prevent copter from switching to DCM unless EKF has severe errors

libraries/AP_AHRS/AP_AHRS.cpp

@@ -118,8 +118,8 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
 #if AP_AHRS_NAVEKF_AVAILABLE && !AHRS_EKF_USE_ALWAYS
     // @Param: EKF_USE
     // @DisplayName: Use NavEKF Kalman filter for attitude and position estimation
-    // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation
-    // @Values: 0:Disabled,1:Enabled
+    // @Description: This controls whether the NavEKF Kalman filter is used for attitude and position estimation and whether fallback to the DCM algorithm is allowed
+    // @Values: 0:Disabled,1:Enabled, 2:Enabled - No Fallback
     // @User: Advanced
     AP_GROUPINFO("EKF_USE",  13, AP_AHRS, _ekf_use, AHRS_EKF_USE_DEFAULT),
 #endif

libraries/AP_AHRS/AP_AHRS.h

@@ -46,6 +46,10 @@
 #define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
 #define AP_AHRS_YAW_P_MIN  0.05f        // minimum value for AHRS_YAW_P parameter
 
+#define EKF_DO_NOT_USE            0     // Prevents the EKF from being used by the flight controllers
+#define EKF_USE_WITH_FALLBACK     1     // Uses the EKF unless its solution is unhealthy or not initialised. This allows sensor errors to cause reversion.
+#define EKF_USE_WITHOUT_FALLBACK  2     // Uses the EKF unless it encounters numerical processing errors or isn't iniitalised. Sensor errors will not cause reversion.
+
 enum AHRS_VehicleClass {
     AHRS_VEHICLE_UNKNOWN,
     AHRS_VEHICLE_GROUND,
@@ -367,7 +371,7 @@ protected:
     AP_Int8 _gps_delay;
 
 #if AHRS_EKF_USE_ALWAYS
-    static const int8_t _ekf_use = 1;
+    static const int8_t _ekf_use = EKF_USE_WITHOUT_FALLBACK;
 #else
     AP_Int8 _ekf_use;
 #endif

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -312,7 +312,12 @@ bool AP_AHRS_NavEKF::get_relative_position_NED(Vector3f &vec) const
 
 bool AP_AHRS_NavEKF::using_EKF(void) const
 {
-    bool ret = ekf_started && _ekf_use && EKF.healthy();
+    uint8_t ekf_faults;
+    EKF.getFilterFaults(ekf_faults);
+    // If EKF is started we switch away if it reports unhealthy. This could be due to bad
+    // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
+    // an internal processing error, but not for bad sensor data.
+    bool ret = ekf_started && ((_ekf_use == EKF_USE_WITH_FALLBACK && EKF.healthy()) || (_ekf_use == EKF_USE_WITHOUT_FALLBACK && ekf_faults == 0));
     if (!ret) {
         return false;
     }
@@ -338,7 +343,10 @@ bool AP_AHRS_NavEKF::using_EKF(void) const
 */
 bool AP_AHRS_NavEKF::healthy(void) const
 {
-    if (_ekf_use) {
+    // If EKF is started we switch away if it reports unhealthy. This could be due to bad
+    // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters
+    // an internal processing error, but not for bad sensor data.
+    if (_ekf_use != EKF_DO_NOT_USE) {
         return ekf_started && EKF.healthy();
     }
     return AP_AHRS_DCM::healthy();