Browse Source

AP_NavEKF: prevent float exception on startup

master
Andrew Tridgell 10 years ago
parent
commit
baf292def1
  1. 4
      libraries/AP_NavEKF/AP_NavEKF.cpp

4
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -3514,8 +3514,10 @@ void NavEKF::RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEn @@ -3514,8 +3514,10 @@ void NavEKF::RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEn
if (numAvg >= 1)
{
omegaAvg = omegaAvg / float(numAvg);
} else {
} else if (dtIMUactual > 0) {
omegaAvg = correctedDelAng / dtIMUactual;
} else {
omegaAvg.zero();
}
}

Loading…
Cancel
Save