diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 936ceccc7b..6e337083ff 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -301,6 +301,13 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("GLITCH_RAD", 24, NavEKF, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT), + // @Param: FALLBACK + // @DisplayName: Fallback strictness + // @Description: This parameter controls the conditions necessary to trigger a fallback to DCM and INAV. A value of 1 will cause fallbacks to occur on loss of GPS and other conditions. A value of 0 will trust the EKF more. + // @Values: 0:Trust EKF more, 1:Trust DCM more + // @User: Advanced + AP_GROUPINFO("FALLBACK", 31, NavEKF, _fallback, 1), + AP_GROUPEND }; @@ -372,7 +379,7 @@ bool NavEKF::healthy(void) const // If measurements have failed innovation consistency checks for long enough to time-out // and force fusion then the nav solution can be conidered to be unhealthy // This will only be set as a transient - if (posTimeout || velTimeout || hgtTimeout) { + if (_fallback && (posTimeout || velTimeout || hgtTimeout)) { return false; } diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 00d63af6e4..de48e7c9ec 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -332,6 +332,7 @@ private: AP_Int8 _magCal; // Sets activation condition for in-flight magnetometer calibration AP_Int16 _gpsGlitchAccelMax; // Maximum allowed discrepancy between inertial and GPS Horizontal acceleration before GPS data is ignored : cm/s^2 AP_Int8 _gpsGlitchRadiusMax; // Maximum allowed discrepancy between inertial and GPS Horizontal position before GPS glitch is declared : m + AP_Int8 _fallback; // EKF-to-DCM fallback strictness. 0 = trust EKF more, 1 = fallback more conservatively. // Tuning parameters AP_Float _gpsNEVelVarAccScale; // scale factor applied to NE velocity measurement variance due to Vdot