diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 5cf70b47d6..04437dde9a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -325,8 +325,12 @@ void NavEKF2_core::readIMUData() // Keep track of the number of IMU frames since the last state prediction framesSincePredict++; - // If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data - // to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed + /* + * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle, + * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more + * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the + * IMU data. + */ if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {