AP_InertialSensor: converted HIL backend, which gets SITL working

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -300,7 +300,13 @@ AP_InertialSensor::init( Start_style style,
 void 
 AP_InertialSensor::_detect_backends(Sample_rate sample_rate)
 {
+#if HAL_INS_DEFAULT == HAL_INS_HIL
+    _backends[0] = AP_InertialSensor_HIL::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
+#elif HAL_INS_DEFAULT == HAL_INS_MPU6000
     _backends[0] = AP_InertialSensor_MPU6000::detect(*this, sample_rate, _gyro[_gyro_count], _accel[_accel_count]);
+#else
+    #error Unrecognised HAL_INS_TYPE setting
+#endif
     if (_backends[0] == NULL ||
         _gyro_count == 0 ||
         _accel_count == 0) {
@@ -917,9 +923,27 @@ void AP_InertialSensor::wait_for_sample(void)
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_backends[i] != NULL) {
                 gyro_available |= _backends[i]->gyro_sample_available();
-                accel_available |= _backends[i]->gyro_sample_available();
+                accel_available |= _backends[i]->accel_sample_available();
             }
         }
-        hal.scheduler->delay_microseconds(100);
+        hal.scheduler->delay(1);
     }
 }
+
+/*
+  support for setting accel and gyro vectors, for use by HIL
+ */
+void AP_InertialSensor::set_accel(uint8_t instance, const Vector3f &accel)
+{
+    if (instance < INS_MAX_INSTANCES) {
+        _accel[instance] = accel;
+    }
+}
+
+void AP_InertialSensor::set_gyro(uint8_t instance, const Vector3f &gyro)
+{
+    if (instance < INS_MAX_INSTANCES) {
+        _gyro[instance] = gyro;
+    }
+}
+

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -251,6 +251,7 @@ private:
 
 #include "AP_InertialSensor_Backend.h"
 #include "AP_InertialSensor_MPU6000.h"
+#include "AP_InertialSensor_HIL.h"
 #include "AP_InertialSensor_UserInteract_Stream.h"
 #include "AP_InertialSensor_UserInteract_MAVLink.h"
 

libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp

@@ -1,132 +1,71 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#if NOT_YET
-#include "AP_InertialSensor_HIL.h"
 #include <AP_HAL.h>
+#include "AP_InertialSensor_HIL.h"
+
 const extern AP_HAL::HAL& hal;
 
-AP_InertialSensor_HIL::AP_InertialSensor_HIL() :
+AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) :
-    AP_InertialSensor(),
+    AP_InertialSensor_Backend(imu, gyro, accel),
     _sample_period_usec(0),
     _last_sample_usec(0)
 {
-    _accel[0] = Vector3f(0, 0, -GRAVITY_MSS);
 }
 
-uint16_t AP_InertialSensor_HIL::_init_sensor( Sample_rate sample_rate ) {
+/*
+  detect the sensor
+ */
+AP_InertialSensor_Backend *AP_InertialSensor_HIL::detect(AP_InertialSensor &_imu, 
+                                                         AP_InertialSensor::Sample_rate sample_rate,
+                                                         Vector3f &gyro, 
+                                                         Vector3f &accel)
+{
+    AP_InertialSensor_HIL *sensor = new AP_InertialSensor_HIL(_imu, gyro, accel);
+    if (sensor == NULL) {
+        return NULL;
+    }
+    if (!sensor->_init_sensor(sample_rate)) {
+        delete sensor;
+        return NULL;
+    }
+    return sensor;
+}
+
+bool AP_InertialSensor_HIL::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) 
+{
     switch (sample_rate) {
-    case RATE_50HZ:
+    case AP_InertialSensor::RATE_50HZ:
         _sample_period_usec = 20000;
         break;
-    case RATE_100HZ:
+    case AP_InertialSensor::RATE_100HZ:
         _sample_period_usec = 10000;
         break;
-    case RATE_200HZ:
+    case AP_InertialSensor::RATE_200HZ:
         _sample_period_usec = 5000;
         break;
-    case RATE_400HZ:
+    case AP_InertialSensor::RATE_400HZ:
         _sample_period_usec = 2500;
         break;
     }
-    return AP_PRODUCT_ID_NONE;
-}
 
-/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
+    // grab the used instances
+    _imu.register_gyro();
+    _imu.register_accel();
 
-bool AP_InertialSensor_HIL::update( void ) {
-    uint32_t now = hal.scheduler->micros();
-    _last_sample_usec = now;
     return true;
 }
 
-float AP_InertialSensor_HIL::get_delta_time() const {
+bool AP_InertialSensor_HIL::update(void) 
-    return _sample_period_usec * 1.0e-6f;
+{
-}
+    uint32_t now = hal.scheduler->micros();
-
+    while (now - _last_sample_usec > _sample_period_usec) {
-float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
+        _last_sample_usec += _sample_period_usec;
-    // 0.5 degrees/second/minute
+    }
-    return ToRad(0.5/60);
+    return true;
 }
 
 bool AP_InertialSensor_HIL::_sample_available()
 {
-    uint32_t tnow = hal.scheduler->micros();
+    return (hal.scheduler->micros() - _last_sample_usec > _sample_period_usec);
-    bool have_sample = false;
-    while (tnow - _last_sample_usec > _sample_period_usec) {
-        have_sample = true;
-        _last_sample_usec += _sample_period_usec;
-    }
-    return have_sample;
 }
 
-bool AP_InertialSensor_HIL::wait_for_sample(uint16_t timeout_ms)
-{
-    if (_sample_available()) {
-        return true;
-    }
-    uint32_t start = hal.scheduler->millis();
-    while ((hal.scheduler->millis() - start) < timeout_ms) {
-        uint32_t tnow = hal.scheduler->micros();
-        uint32_t tdelay = (_last_sample_usec + _sample_period_usec) - tnow;
-        if (tdelay < 100000) {
-            hal.scheduler->delay_microseconds(tdelay);
-        }
-        if (_sample_available()) {
-            return true;
-        }
-    }
-    return false;
-}
-
-void AP_InertialSensor_HIL::set_accel(uint8_t instance, const Vector3f &accel)
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return;
-    }
-    _previous_accel[instance] = _accel[instance];
-    _accel[instance] = accel;
-    _last_accel_usec[instance] = hal.scheduler->micros();
-}
-
-void AP_InertialSensor_HIL::set_gyro(uint8_t instance, const Vector3f &gyro)
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return;
-    }
-    _gyro[instance] = gyro;
-    _last_gyro_usec[instance] = hal.scheduler->micros();
-}
-
-bool AP_InertialSensor_HIL::get_gyro_health(uint8_t instance) const
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return false;
-    }
-    return (hal.scheduler->micros() - _last_gyro_usec[instance]) < 40000;
-}
-
-bool AP_InertialSensor_HIL::get_accel_health(uint8_t instance) const
-{
-    if (instance >= INS_MAX_INSTANCES) {
-        return false;
-    }
-    return (hal.scheduler->micros() - _last_accel_usec[instance]) < 40000;
-}
-
-uint8_t AP_InertialSensor_HIL::get_gyro_count(void) const
-{
-    if (get_gyro_health(1)) {
-        return 2;
-    }
-    return 1;
-}
-
-uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
-{
-    if (get_accel_health(1)) {
-        return 2;
-    }
-    return 1;
-}
-
-#endif

libraries/AP_InertialSensor/AP_InertialSensor_HIL.h

@@ -1,36 +1,32 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#ifndef __AP_INERTIAL_SENSOR_STUB_H__
+#ifndef __AP_INERTIALSENSOR_HIL_H__
-#define __AP_INERTIAL_SENSOR_STUB_H__
+#define __AP_INERTIALSENSOR_HIL_H__
 
-#include <AP_Progmem.h>
 #include "AP_InertialSensor.h"
 
-class AP_InertialSensor_HIL : public AP_InertialSensor
+class AP_InertialSensor_HIL : public AP_InertialSensor_Backend
 {
 public:
+    AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel);
 
-    AP_InertialSensor_HIL();
+    /* update accel and gyro state */
+    bool update();
 
-    /* Concrete implementation of AP_InertialSensor functions: */
+    bool gyro_sample_available(void) { return _sample_available(); }
-    bool            update();
+    bool accel_sample_available(void) { return _sample_available(); }
-    float	        get_delta_time() const;
+
-    float           get_gyro_drift_rate();
+    // detect the sensor
-    bool            wait_for_sample(uint16_t timeout_ms);
+    static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu,
-    void            set_accel(uint8_t instance, const Vector3f &accel);
+                                             AP_InertialSensor::Sample_rate sample_rate,
-    void            set_gyro(uint8_t instance, const Vector3f &gyro);
+                                             Vector3f &gyro,
-    bool            get_gyro_health(uint8_t instance) const;
+                                             Vector3f &accel);
-    bool            get_accel_health(uint8_t instance) const;
-    uint8_t         get_gyro_count(void) const;
-    uint8_t         get_accel_count(void) const;
 
 private:
-    bool            _sample_available();
+    bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate);
-    uint16_t        _init_sensor( Sample_rate sample_rate );
+    bool _sample_available(void);
-    uint32_t         _sample_period_usec;
+    uint32_t _sample_period_usec;
-    uint32_t        _last_sample_usec;
+    uint32_t _last_sample_usec;
-    uint32_t        _last_accel_usec[INS_MAX_INSTANCES];
-    uint32_t        _last_gyro_usec[INS_MAX_INSTANCES];
 };
 
-#endif // __AP_INERTIAL_SENSOR_STUB_H__
+#endif // __AP_INERTIALSENSOR_HIL_H__