AP_NavEKF : Clear compass fail on transition between armed and disarmed

This allows a compass that has been declared failed, possibly because of
external disturbances (eg movement of hatches, proximity of tools, etc)
to be given a second chance when the vehicle is armed.
This commit is contained in:
priseborough 2014-08-25 17:19:16 +10:00 committed by Andrew Tridgell
parent e34b0847d7
commit 0f62dbf6ed
1 changed files with 3 additions and 0 deletions

View File

@ -608,6 +608,9 @@ void NavEKF::UpdateFilter()
ResetPosition();
ResetHeight();
StoreStatesReset();
// clear the magnetometer failed status as the failure may have been
// caused by external field disturbances associated with pre-flight activities
magFailed = false;
calcQuatAndFieldStates(_ahrs->roll, _ahrs->pitch);
prevStaticMode = staticMode;
}