From 8acfbb2ee07ebefb5a6b17327be0e107b3e6f24b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 22 Nov 2014 13:26:16 +1100 Subject: [PATCH] AP_NavEKF: add another health check in the EKF if SV, SP and SH are all off then the most likely cause is divergence of the EKF. This was done based on a flight log with bad gyro cal --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 2346c134dc..a02b3a2ba6 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -378,6 +378,12 @@ bool NavEKF::healthy(void) const if (state.velocity.is_nan()) { return false; } + if (_fallback && velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) { + // all three metrics being above 1 means the filter is + // extremely unhealthy. + return false; + } + // 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