Browse Source

AP_NavEKF: Fix bug causing immediate clearing of diverged status on reset

mission-4.1.18
priseborough 11 years ago committed by Andrew Tridgell
parent
commit
0337d44b2e
  1. 3
      libraries/AP_NavEKF/AP_NavEKF.cpp

3
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -355,6 +355,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) : @@ -355,6 +355,7 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
IMU1_weighting = 0.5f;
lastDivergeTime_ms = 0;
filterDiverged = false;
memset(&faultStatus, 0, sizeof(faultStatus));
}
// Check basic filter health metrics and return a consolidated health status
@ -3299,7 +3300,6 @@ void NavEKF::ZeroVariables() @@ -3299,7 +3300,6 @@ void NavEKF::ZeroVariables()
velTimeout = false;
posTimeout = false;
hgtTimeout = false;
filterDiverged = false;
lastStateStoreTime_ms = 0;
lastFixTime_ms = 0;
secondLastFixTime_ms = 0;
@ -3320,7 +3320,6 @@ void NavEKF::ZeroVariables() @@ -3320,7 +3320,6 @@ void NavEKF::ZeroVariables()
dt = 0;
hgtMea = 0;
storeIndex = 0;
memset(&faultStatus, 0, sizeof(faultStatus));
lastGyroBias.zero();
prevDelAng.zero();
lastAngRate.zero();

Loading…
Cancel
Save