AP_InertialSensor: allow HIL sensors to support multiple instances

this allows log replay to test both sets of sensors on a Pixhawk log

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -11,7 +11,7 @@
    maximum number of INS instances available on this platform. If more
    than 1 then redundent sensors may be available
  */
-#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 #define INS_MAX_INSTANCES 2
 #else
 #define INS_MAX_INSTANCES 1
@@ -96,7 +96,7 @@ public:
     ///
     const Vector3f     &get_gyro(uint8_t i) const { return _gyro[i]; }
     const Vector3f     &get_gyro(void) const { return get_gyro(_get_primary_gyro()); }
-    virtual void       set_gyro(const Vector3f &gyro) {}
+    virtual void       set_gyro(uint8_t instance, const Vector3f &gyro) {}
 
     // set gyro offsets in radians/sec
     const Vector3f &get_gyro_offsets(uint8_t i) const { return _gyro_offset[i]; }
@@ -108,7 +108,7 @@ public:
     ///
     const Vector3f     &get_accel(uint8_t i) const { return _accel[i]; }
     const Vector3f     &get_accel(void) const { return get_accel(_get_primary_accel()); }
-    virtual void       set_accel(const Vector3f &accel) {}
+    virtual void       set_accel(uint8_t instance, const Vector3f &accel) {}
 
     // multi-device interface
     virtual bool get_gyro_health(uint8_t instance) const { return true; }

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -67,38 +67,26 @@ bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
     return false;
 }
 
-void AP_InertialSensor_HIL::set_accel(const Vector3f &accel)
+void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
 {
-    _previous_accel[0] = _accel[0];
-    _accel[0] = accel;
-    _last_accel_usec = hal.scheduler->micros();
+    _previous_accel[instance] = _accel[instance];
+    _accel[instance] = accel;
+    _last_accel_usec[instance] = hal.scheduler->micros();
 }
 
-void AP_InertialSensor_HIL::set_gyro(const Vector3f &gyro)
+void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
 {
-    _gyro[0] = gyro;
-    _last_gyro_usec = hal.scheduler->micros();
+    _gyro[instance] = gyro;
+    _last_gyro_usec[instance] = hal.scheduler->micros();
 }
 
-/**
-   try to detect bad accel/gyro sensors
- */
-bool AP_InertialSensor_HIL::healthy(void) const
+bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
 {
-    uint32_t tnow = hal.scheduler->micros();
-    if ((tnow - _last_accel_usec) > 40000) {
-        // accels have not updated
-        return false;
-    }
-    if ((tnow - _last_gyro_usec) > 40000) {
-        // gyros have not updated
-        return false;
-    }
-    if (fabs(_accel[0].x) > 30 && fabs(_accel[0].y) > 30 && fabs(_accel[0].z) > 30 &&
-        (_previous_accel[0] - _accel[0]).length() < 0.01f) {
-        // unchanging accel, large in all 3 axes. This is a likely
-        // accelerometer failure
-        return false;
-    }
-    return true;
+    return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
 }
+
+bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
+{
+    return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
+}
+

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -17,9 +17,10 @@ public:
     float	        get_delta_time() const;
     float           get_gyro_drift_rate();
     bool            wait_for_sample(uint16_t timeout_ms);
-    void            set_accel(const Vector3f &accel);
-    void            set_gyro(const Vector3f &gyro);
-    bool            healthy(void) const;
+    void            set_accel(uint8_t instance, const Vector3f &accel);
+    void            set_gyro(uint8_t instance, const Vector3f &gyro);
+    bool            get_gyro_health(uint8_t instance) const;
+    bool            get_accel_health(uint8_t instance) const;
 
 private:
     bool            _sample_available();
@@ -27,8 +28,8 @@ private:
     float           _sample_period_ms;
     uint32_t        _last_update_ms;
     uint32_t        _delta_time_usec;
-    uint32_t        _last_accel_usec;
-    uint32_t        _last_gyro_usec;
+    uint32_t        _last_accel_usec[INS_MAX_INSTANCES];
+    uint32_t        _last_gyro_usec[INS_MAX_INSTANCES];
 };
 
 #endif // __AP_INERTIAL_SENSOR_STUB_H__