AP_NavEKF: prevent float exception on startup
This commit is contained in:
parent
7fc0f026d2
commit
baf292def1
@ -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…
Reference in New Issue
Block a user