From 7cf1187473e47b52861eccdd23fe2ea4405455f4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= <v.mayoralv@gmail.com>
Date: Thu, 15 May 2014 12:13:48 +0200
Subject: [PATCH] AP_InertialSensor_MPU9150: Clock-based wait_for_sample()
 impl.

Adapt the I2C driver to a system clock based wait_for_sample()
implementation.

The sample rate of the sensor has been corrected to 800 Hz (could be
further pushed up to 1KHz).

Filters for the mag have also been created but remain commented until
the code for the mag is ready.
---
 .../AP_InertialSensor_MPU9150.cpp             | 31 +++++++++++++------
 .../AP_InertialSensor_MPU9150.h               |  7 +++--
 2 files changed, 26 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
index 2f15c5ed5d..648b83fc94 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
@@ -28,6 +28,7 @@
 #include "AP_InertialSensor_MPU9150.h"
 #include <stdio.h>
 #include <unistd.h>
+#include <time.h>     
 
 const extern AP_HAL::HAL& hal;
 
@@ -327,6 +328,9 @@ AP_InertialSensor_MPU9150::AP_InertialSensor_MPU9150() :
     _gyro_filter_x(800, 10),
     _gyro_filter_y(800, 10),
     _gyro_filter_z(800, 10)    
+    // _mag_filter_x(800, 10),    
+    // _mag_filter_y(800, 10),    
+    // _mag_filter_z(800, 10)    
 {
 
 }
@@ -361,23 +365,19 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
     case RATE_50HZ:
         _default_filter_hz = 10;
         _sample_period_usec = (1000*1000) / 50;
-        _gyro_samples_needed = 16;
         break;
     case RATE_100HZ:
         _default_filter_hz = 20;
         _sample_period_usec = (1000*1000) / 100;
-        _gyro_samples_needed = 8;
         break;
     case RATE_200HZ:
         _default_filter_hz = 20;
         _sample_period_usec = 5000;
-        _gyro_samples_needed = 4;
         break;
     case RATE_400HZ:
     default:
         _default_filter_hz = 20;
         _sample_period_usec = 2500;
-        _gyro_samples_needed = 2;
         break;
     }
 
@@ -446,7 +446,7 @@ uint16_t AP_InertialSensor_MPU9150::_init_sensor( Sample_rate sample_rate )
         goto failed;    
     }
     // Set sampling rate (value must be between 4Hz and 1KHz)
-    if (mpu_set_sample_rate(400)){
+    if (mpu_set_sample_rate(800)){
         hal.scheduler->panic(PSTR("AP_InertialSensor_MPU9150: mpu_set_sample_rate.\n"));
         goto failed;    
     }
@@ -1103,21 +1103,29 @@ void AP_InertialSensor_MPU9150::_accumulate(void){
 
 bool AP_InertialSensor_MPU9150::_sample_available(void)
 {
-    return (_gyro_samples_available >= _gyro_samples_needed);
+    uint64_t tnow =  hal.scheduler->micros();
+    while (tnow - _last_sample_timestamp > _sample_period_usec) {
+        _have_sample_available = true;
+        _last_sample_timestamp += _sample_period_usec;
+    }
+    return _have_sample_available;    
 }
 
 bool AP_InertialSensor_MPU9150::wait_for_sample(uint16_t timeout_ms)
 {
     if (_sample_available()) {
-        _last_sample_time = hal.scheduler->micros();
         return true;
     }
     uint32_t start = hal.scheduler->millis();
     while ((hal.scheduler->millis() - start) < timeout_ms) {
-        hal.scheduler->delay_microseconds(100);
-        _accumulate();
+        uint64_t tnow = hal.scheduler->micros(); 
+        // we spin for the last timing_lag microseconds. Before that
+        // we yield the CPU to allow IO to happen
+        const uint16_t timing_lag = 400;
+        if (_last_sample_timestamp + _sample_period_usec > tnow+timing_lag) {
+            hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_period_usec - (tnow+timing_lag));
+        }
         if (_sample_available()) {
-            _last_sample_time = hal.scheduler->micros();
             return true;
         }
     }
@@ -1164,6 +1172,9 @@ bool AP_InertialSensor_MPU9150::update(void)
         _set_filter_frequency(_mpu6000_filter);
         _last_filter_hz = _mpu6000_filter;
     }
+
+    _have_sample_available = false;
+
     return true;
 }
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
index 006088dcc4..664c39ebc2 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.h
@@ -32,9 +32,9 @@ private:
     Vector3f        _accel_filtered;
     Vector3f        _gyro_filtered;
     uint32_t        _sample_period_usec;
-    uint32_t        _last_sample_time;
     volatile uint32_t _gyro_samples_available;
-    volatile uint8_t  _gyro_samples_needed;
+    uint64_t        _last_sample_timestamp;    
+    bool     _have_sample_available;    
 
     // // support for updating filter at runtime
     uint8_t         _last_filter_hz;
@@ -62,6 +62,9 @@ private:
     LowPassFilter2p _gyro_filter_x;
     LowPassFilter2p _gyro_filter_y;
     LowPassFilter2p _gyro_filter_z;
+    // LowPassFilter2p _mag_filter_x;
+    // LowPassFilter2p _mag_filter_y;
+    // LowPassFilter2p _mag_filter_z;
 
 
 };