Browse Source

AP_NavEKF : clean up convoluted logic used in sensor health checks

This doesn't change the behavior, but does make the code easier to
understand
mission-4.1.18
priseborough 11 years ago committed by Andrew Tridgell
parent
commit
0ae736c3a0
  1. 6
      libraries/AP_NavEKF/AP_NavEKF.cpp

6
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -1661,7 +1661,7 @@ void NavEKF::FuseVelPosNED() @@ -1661,7 +1661,7 @@ void NavEKF::FuseVelPosNED()
// calculate the test ratio
velTestRatio = innovVelSumSq / (varVelSum * sq(_gpsVelInnovGate));
// fail if the ratio is greater than 1
velHealth = !((velTestRatio > 1.0f) && !badIMUdata);
velHealth = ((velTestRatio < 1.0f) || badIMUdata);
// If failed for too long we need to do something with the data
velTimeout = (hal.scheduler->millis() - velFailTime) > gpsRetryTime;
if (velHealth || velTimeout || staticMode)
@ -1694,7 +1694,7 @@ void NavEKF::FuseVelPosNED() @@ -1694,7 +1694,7 @@ void NavEKF::FuseVelPosNED()
float accelScale = (1.0f + 0.1f * accNavMagHoriz);
float maxPosInnov2 = sq(_gpsPosInnovGate * _gpsHorizPosNoise + 0.005f * accelScale * float(_gpsGlitchAccelMax) * sq(0.001f * float(hal.scheduler->millis() - posFailTime)));
posTestRatio = (sq(posInnov[0]) + sq(posInnov[1])) / maxPosInnov2;
posHealth = !((posTestRatio > 1.0f) && !badIMUdata);
posHealth = ((posTestRatio < 1.0f) || badIMUdata);
// if we have timed out or exceeded the max radius, then we offset the GPS position to match the inertial solution and then decay the
// offset to bring the vehicle back gradually to the new GPS position
posTimeout = ((maxPosInnov2 > sq(float(_gpsGlitchRadiusMax))) || ((hal.scheduler->millis() - posFailTime) > gpsRetryTime));
@ -1735,7 +1735,7 @@ void NavEKF::FuseVelPosNED() @@ -1735,7 +1735,7 @@ void NavEKF::FuseVelPosNED()
// calculate the innovation consistency test ratio
hgtTestRatio = sq(hgtInnov) / (sq(_hgtInnovGate) * varInnovVelPos[5]);
// fail if the ratio is > 1, but don't fail if bad IMU data
hgtHealth = !((hgtTestRatio > 1.0f) && !badIMUdata);
hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
hgtTimeout = (hal.scheduler->millis() - hgtFailTime) > hgtRetryTime;
// Fuse height data if healthy or timed out or in static mode
if (hgtHealth || hgtTimeout || staticMode)

Loading…
Cancel
Save