AP_InertialSensor: fixed stub driver to work correctly with sensors HIL

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; }

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;
 }

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__