From 085faaac6a618d83852d5c9f9c4a9f6ee8f3a9ac Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 16 Apr 2015 16:57:37 -0700 Subject: [PATCH] AP_NavEKF: fix delay detection so that filter properly resets after a delay --- libraries/AP_NavEKF/AP_NavEKF.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 398c75129c..12ec00eec7 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3982,9 +3982,7 @@ void NavEKF::readIMUData() const AP_InertialSensor &ins = _ahrs->get_ins(); dtIMUavg = 1.0f/ins.get_sample_rate(); - - // limit IMU delta time to prevent numerical problems elsewhere - dtIMUactual = constrain_float(ins.get_delta_time(),0.001f,0.2f); + dtIMUactual = max(ins.get_delta_time(),1.0e-3f); // the imu sample time is sued as a common time reference throughout the filter imuSampleTime_ms = hal.scheduler->millis();