|
|
|
@ -325,8 +325,12 @@ void NavEKF2_core::readIMUData()
@@ -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)) { |
|
|
|
|
|
|
|
|
|