aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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;
}
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