From 82b036282b15547178ea56e9a65c587b0981d54b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 7 Jan 2013 19:39:37 +1100
Subject: [PATCH] MPU6000: use expected sample time, not actual sample time

this avoids problems in the scheduler turning up as errors in the DCM
calculation. The MPU6k produces samples with more reliable timing than
our scheduler does!
---
 libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 7 ++++---
 libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h   | 1 +
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index a673efb90f..1c723fa1fd 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -185,7 +185,6 @@ int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE;
 
 // variables to calculate time period over which a group of samples were collected
 static volatile uint32_t _delta_time_micros = 1;   // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case)
-static volatile uint32_t _delta_time_start_micros = 0;  // time we start collecting sample (reset on update)
 static volatile uint32_t _last_sample_time_micros = 0;  // time latest sample was collected
 
 // DMP related static variables
@@ -253,8 +252,7 @@ bool AP_InertialSensor_MPU6000::update( void )
     _count = 0;
 
     // record sample time
-    _delta_time_micros = _last_sample_time_micros - _delta_time_start_micros;
-    _delta_time_start_micros = _last_sample_time_micros;
+    _delta_time_micros = count * _micros_per_sample;
     SREG = oldSREG;
 
     count_scale = 1.0 / count;
@@ -391,15 +389,18 @@ void AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
     case RATE_50HZ:
         rate = MPUREG_SMPLRT_50HZ;
         default_filter = BITS_DLPF_CFG_20HZ;
+        _micros_per_sample = 20000;
         break;
     case RATE_100HZ:
         rate = MPUREG_SMPLRT_100HZ;
         default_filter = BITS_DLPF_CFG_42HZ;
+        _micros_per_sample = 10000;
         break;
     case RATE_200HZ:
     default:
         rate = MPUREG_SMPLRT_200HZ;
         default_filter = BITS_DLPF_CFG_42HZ;
+        _micros_per_sample = 5000;
         break;
     }
     
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index 3969fe8022..5a32fcd0a7 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -75,6 +75,7 @@ private:
     // ensure we can't initialise twice
     bool                        _initialised;
     static int16_t              _mpu6000_product_id;
+    uint32_t					_micros_per_sample;
 
     // dmp related methods and parameters
     static void                 dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[]); // Method to write multiple bytes into dmp registers.  Requires a "bank"