From 6f36267a661837b42bade1f8f3d85a5ab074d7f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 16 Oct 2015 14:58:46 +1100 Subject: [PATCH] AP_NavEKF: fixed EKF error message during 10 second warmup --- libraries/AP_NavEKF/AP_NavEKF.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 55b9367047..5c73f37391 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -5434,19 +5434,22 @@ bool NavEKF::calcGpsGoodToAlign(void) return true; } + if (lastGpsVelFail_ms == 0) { + // first time through, start with a failure + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF warmup"); + lastGpsVelFail_ms = imuSampleTime_ms; + } + // record time of fail - // assume fail first time called - if (gpsVelFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) { + if (gpsVelFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) { lastGpsVelFail_ms = imuSampleTime_ms; } // continuous period without fail required to return healthy if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { return true; - } else { - hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF checking GPS"); - return false; } + return false; } // report the reason for why the backend is refusing to initialise