Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fixed EKF error message during 10 second warmup

Browse Source
This commit is contained in:
Andrew Tridgell 2015-10-16 14:58:46 +11:00
parent 9889f826b3
commit 6f36267a66
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -5434,19 +5434,22 @@ bool NavEKF::calcGpsGoodToAlign(void)
return true; 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 // record time of fail
// assume fail first time called if (gpsVelFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
if (gpsVelFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) {
lastGpsVelFail_ms = imuSampleTime_ms; lastGpsVelFail_ms = imuSampleTime_ms;
} }
// continuous period without fail required to return healthy // continuous period without fail required to return healthy
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true; 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 // report the reason for why the backend is refusing to initialise