aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Prevent start-up transients re-tripping divergence test

Browse Source
This commit is contained in:
priseborough 2014-05-16 21:55:10 +10:00 committed by Andrew Tridgell
parent 73976e2ca4
commit e40e50e2e1
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -354,7 +354,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
mag_state.DCM.identity(); mag_state.DCM.identity();
IMU1_weighting = 0.5f; IMU1_weighting = 0.5f;
lastDivergeTime_ms = 0; lastDivergeTime_ms = 0;
filterDiverged = false;
memset(&faultStatus, 0, sizeof(faultStatus)); memset(&faultStatus, 0, sizeof(faultStatus));
} }
@ -370,7 +369,7 @@ bool NavEKF::healthy(void) const
if (state.velocity.is_nan()) { if (state.velocity.is_nan()) {
return false; return false;
} }
if (filterDiverged) { if (filterDiverged || (hal.scheduler->millis() - lastDivergeTime_ms < 10000)) {
return false; return false;
} }
// If measurements have failed innovation consistency checks for long enough to time-out // If measurements have failed innovation consistency checks for long enough to time-out
@ -3300,6 +3299,7 @@ void NavEKF::ZeroVariables()
velTimeout = false; velTimeout = false;
posTimeout = false; posTimeout = false;
hgtTimeout = false; hgtTimeout = false;
filterDiverged = false;
lastStateStoreTime_ms = 0; lastStateStoreTime_ms = 0;
lastFixTime_ms = 0; lastFixTime_ms = 0;
secondLastFixTime_ms = 0; secondLastFixTime_ms = 0;
@ -3394,6 +3394,7 @@ bool NavEKF::assume_zero_sideslip(void) const
void NavEKF::checkDivergence() void NavEKF::checkDivergence()
{ {
// If filter is diverging, then fail for 10 seconds // If filter is diverging, then fail for 10 seconds
// delay checking to allow bias estimate to settle after reset
// filter divergence is detected by looking for rapid changes in gyro bias // filter divergence is detected by looking for rapid changes in gyro bias
Vector3f tempVec = state.gyro_bias - lastGyroBias; Vector3f tempVec = state.gyro_bias - lastGyroBias;
float tempLength = tempVec.length(); float tempLength = tempVec.length();
@ -3403,13 +3404,15 @@ void NavEKF::checkDivergence()
} }
bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f); bool divergenceDetected = (scaledDeltaGyrBiasLgth > 1.0f);
lastGyroBias = state.gyro_bias; lastGyroBias = state.gyro_bias;
if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) {
if (divergenceDetected) { if (divergenceDetected) {
filterDiverged = true; filterDiverged = true;
faultStatus.diverged = true; faultStatus.diverged = true;
lastDivergeTime_ms = hal.scheduler->millis(); lastDivergeTime_ms = hal.scheduler->millis();
} else if (hal.scheduler->millis() - lastDivergeTime_ms > 10000) { } else {
filterDiverged = false; filterDiverged = false;
} }
}
} }