diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index dbc1bdae8e..8d7212bcb3 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -5232,6 +5232,8 @@ bool NavEKF::calcGpsGoodToAlign(void) Vector3f eulerAngles; getEulerAngles(eulerAngles); state.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y); + // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds + magYawResetTimer_ms = imuSampleTime_ms; } // fail if magnetometer innovations are outside limits indicating bad yaw