fix AP attitude hil - with randys help

ArduPlane/system.pde

@@ -414,7 +414,6 @@ static void check_short_failsafe()
 
 static void startup_IMU_ground(bool force_accel_level)
 {
-#if HIL_MODE != HIL_MODE_ATTITUDE
     gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
     mavlink_delay(500);
 
@@ -434,6 +433,7 @@ static void startup_IMU_ground(bool force_accel_level)
     ahrs.set_fly_forward(true);
     ahrs.reset();
 
+#if HIL_MODE != HIL_MODE_ATTITUDE
     // read Baro pressure at ground
     //-----------------------------
     init_barometer();
@@ -446,8 +446,7 @@ static void startup_IMU_ground(bool force_accel_level)
         gcs_send_text_P(SEVERITY_LOW,PSTR("NO airspeed"));
     }
 
-#endif // HIL_MODE_ATTITUDE
-
+#endif
     digitalWrite(B_LED_PIN, LED_ON);                    // Set LED B high to indicate IMU ready
     digitalWrite(A_LED_PIN, LED_OFF);
     digitalWrite(C_LED_PIN, LED_OFF);

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -22,7 +22,9 @@ public:
 
     // Methods
     void            update(void) {
+        _imu->update();
     }
+    
     void            setHil(float roll, float pitch, float yaw,
                            float rollRate, float pitchRate, float yawRate);
 

libraries/AP_IMU/AP_IMU_Shim.cpp

@@ -0,0 +1,6 @@
+
+#include "AP_IMU_Shim.h"
+
+uint16_t  AP_IMU_Shim::_count; // number of samples captured
+uint32_t  AP_IMU_Shim::_first_sample_time_micros; // time first sample began (equal to the last sample time of the previous iteration)
+uint32_t  AP_IMU_Shim::_last_sample_time_micros;  // time that the latest sample was captured

libraries/AP_IMU/AP_IMU_Shim.h

@@ -8,11 +8,15 @@
 
 #include "IMU.h"
 
+#define AP_IMU_SHIM_UPDATE_MICROS 10000     // 10000 micrxos = 10 ms = 100hz
+
 class AP_IMU_Shim : public IMU
 {
 public:
     AP_IMU_Shim(void) {
         _product_id = AP_PRODUCT_ID_NONE;
+        _first_sample_time_micros = 0;
+        _last_sample_time_micros = micros();        
     }
 
 
@@ -22,6 +26,7 @@ public:
                              void                   (*callback)(unsigned long t) = delay,
                              void                   (*flash_leds_cb)(bool on) = NULL,
                              AP_PeriodicProcess *   scheduler = NULL) {
+                                scheduler->register_process( AP_IMU_Shim::read );
     };
     virtual void        init_accel(void (*callback)(unsigned long t) = delay,
                                    void (*flash_leds_cb)(bool on) = NULL) {
@@ -32,19 +37,21 @@ public:
     virtual bool        update(void) {
         bool        updated = _updated;
         _updated = false;
-
-        // return number of microseconds since last call
-        uint32_t        us = micros();
-        uint32_t        ret = us - last_ch6_micros;
-        last_ch6_micros = us;
-
-        _sample_time = ret;
+        _count = 0;
+        _sample_time = _last_sample_time_micros - _first_sample_time_micros;
+        _first_sample_time_micros = _last_sample_time_micros;
         return updated;
     }
     //@}
 
     virtual bool        new_data_available(void) {
-        return true;
+        return _count > 0;
+    }
+
+    /// Get number of samples read from the sensors
+    /// @returns    number of samples read from the sensors
+    uint16_t     num_samples_available(void) {
+        return _count;
     }
 
     float           gx()                            {
@@ -101,9 +108,20 @@ public:
 
 
 private:
+
+    // read function that pretends to capture new data
+    static void read(uint32_t tnow) {
+        _last_sample_time_micros = tnow;
+       _count++;
+    }
+
+    static uint16_t  _count;                    // number of samples captured
+    static uint32_t  _first_sample_time_micros; // time first sample began (equal to the last sample time of the previous iteration)
+    static uint32_t  _last_sample_time_micros;  // time that the latest sample was captured
+
     /// set true when new data is delivered
     bool            _updated;
-    uint32_t        last_ch6_micros;
+
 };
 
 #endif