From 5e77119377ef593013057c010eba4b53003951f4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 3 Dec 2012 23:27:21 +1100
Subject: [PATCH] AP_InertialSensor: fixed stub driver to work correctly with
 sensors HIL

---
 .../AP_InertialSensor/AP_InertialSensor.h     |  2 ++
 .../AP_InertialSensor_Stub.cpp                | 25 ++++++++++++++++---
 .../AP_InertialSensor_Stub.h                  |  3 +++
 3 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 0907080e6d..d095e98d34 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -77,6 +77,7 @@ public:
     /// @returns	vector of rotational rates in radians/sec
     ///
     Vector3f            get_gyro(void) { return _gyro; }
+    void                set_gyro(Vector3f gyro) { _gyro = gyro; }
 
     // set gyro offsets in radians/sec
     Vector3f            get_gyro_offsets(void) { return _gyro_offset; }
@@ -87,6 +88,7 @@ public:
     /// @returns	vector of current accelerations in m/s/s
     ///
     Vector3f            get_accel(void) { return _accel; }
+    void                set_accel(Vector3f accel) { _accel = accel; }
 
     // get accel offsets in m/s/s
     Vector3f            get_accel_offsets() { return _accel_offset; }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
index 7d9f02af2a..478f08d542 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp
@@ -3,16 +3,31 @@
 #include "AP_InertialSensor_Stub.h"
 
 uint16_t AP_InertialSensor_Stub::_init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate ) {
+    switch (sample_rate) {
+    case RATE_50HZ:
+        _sample_period_ms = 20;
+        break;
+    case RATE_100HZ:
+        _sample_period_ms = 10;
+        break;
+    case RATE_200HZ:
+        _sample_period_ms = 5;
+        break;
+    }
     return AP_PRODUCT_ID_NONE;
+
 }
 
 /*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
 
 bool AP_InertialSensor_Stub::update( void ) {
+    uint32_t now = millis();
+    _delta_time_usec = (now - _last_update_ms) * 1000;
+    _last_update_ms = now;
     return true;
 }
 bool AP_InertialSensor_Stub::new_data_available( void ) {
-    return true;
+    return num_samples_available() > 0;
 }
 
 
@@ -40,15 +55,17 @@ float AP_InertialSensor_Stub::temperature() {
     return 0.0;
 }
 uint32_t AP_InertialSensor_Stub::get_delta_time_micros() {
-    return 0;
+    return _delta_time_usec;
 }
 uint32_t AP_InertialSensor_Stub::get_last_sample_time_micros() {
-    return 0;
+    return _last_update_ms * 1000;
 }
 float AP_InertialSensor_Stub::get_gyro_drift_rate(void) {
     return 0.0;
 }
 uint16_t AP_InertialSensor_Stub::num_samples_available()
 {
-    return 1;
+    uint16_t ret = (millis() - _last_update_ms) / _sample_period_ms;
+    
+    return ret;
 }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
index ceafae5fe2..1fb85debd6 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h
@@ -33,6 +33,9 @@ public:
 
 protected:
     uint16_t        _init_sensor( AP_PeriodicProcess * scheduler, Sample_rate sample_rate );
+    uint32_t        _sample_period_ms;
+    uint32_t        _last_update_ms;
+    uint32_t        _delta_time_usec;
 };
 
 #endif // __AP_INERTIAL_SENSOR_STUB_H__