diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 64ffb57f3a..d86d93dbe7 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 8550703c75..9088f55537 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index cf51007480..eb95168069 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/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();