From 23cef70846f6fd6e8a6dd19aa6383ed5b0852fea Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 26 Dec 2015 14:12:20 +1100
Subject: [PATCH] AP_NavEKF2: use get_loop_delta_t() from INS

---
 libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp |  2 +-
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp         | 13 ++++---------
 2 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index 579d06c88a..37a0d9e997 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -233,7 +233,7 @@ void NavEKF2_core::readIMUData()
     const AP_InertialSensor &ins = _ahrs->get_ins();
 
     // average IMU sampling rate
-    dtIMUavg = 1.0f/ins.get_sample_rate();
+    dtIMUavg = ins.get_loop_delta_t();
 
     // the imu sample time is used as a common time reference throughout the filter
     imuSampleTime_ms = AP_HAL::millis();
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index 24936bf894..1a3193fafa 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
@@ -68,16 +68,11 @@ bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
       than 100Hz is downsampled. For 50Hz main loop rate we need a
       shorter buffer.
      */
-    switch (_ahrs->get_ins().get_sample_rate()) {
-    case AP_InertialSensor::RATE_50HZ:
+    if (_ahrs->get_ins().get_sample_rate() < 100) {
         imu_buffer_length = 13;
-        break;
-    case AP_InertialSensor::RATE_100HZ:
-    case AP_InertialSensor::RATE_200HZ:
-    case AP_InertialSensor::RATE_400HZ:
+    } else {
         // maximum 260 msec delay at 100 Hz fusion rate
         imu_buffer_length = 26;
-        break;
     }
     if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
         return false;
@@ -117,7 +112,7 @@ void NavEKF2_core::InitialiseVariables()
 {
     // calculate the nominal filter update rate
     const AP_InertialSensor &ins = _ahrs->get_ins();
-    localFilterTimeStep_ms = (uint8_t)(1000.0f/(float)ins.get_sample_rate());
+    localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
     localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
 
     // initialise time stamps
@@ -279,7 +274,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
     InitialiseVariables();
 
     // Initialise IMU data
-    dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
+    dtIMUavg = _ahrs->get_ins().get_loop_delta_t();
     dtEkfAvg = MIN(0.01f,dtIMUavg);
     readIMUData();
     storedIMU.reset_history(imuDataNew);