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

AP_NavEKF: fixed logic for divergence test

Browse Source 
Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2014-05-11 17:56:01 +10:00
parent 40dc0e56fe
commit 85ebe81ed3
1 changed files with 3 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3370,13 +3370,10 @@ void NavEKF::checkDivergence()
// If position, velocity and magnetometer measurements have all diverged, then fail for 10 seconds
// This is designed to catch a filter divergence and persist for long enough to prevent a badly oscillating solution from being periodically declared healthy
bool divergenceDetected = ((posTestRatio > 1.0f) && (velTestRatio > 1.0f) && (magTestRatio.x > 1.0f) && (magTestRatio.y > 1.0f) && (magTestRatio.z > 1.0f));
bool divergenceTimeout = (hal.scheduler->millis() - lastDivergeTime_ms > 10000);
if (!divergenceTimeout) {
if (divergenceDetected) {
lastDivergeTime_ms = hal.scheduler->millis();
}
if (divergenceDetected) {
filterDiverged = true;
} else {
lastDivergeTime_ms = hal.scheduler->millis();
} else if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) {
filterDiverged = false;
}
}