From ec11417705d1f2c7e898e6fe832e7ab4fd9ae1dc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Oct 2014 16:37:59 +1100 Subject: [PATCH] AP_InertialSensor: converted HIL backend, which gets SITL working --- .../AP_InertialSensor/AP_InertialSensor.cpp | 28 +++- .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_HIL.cpp | 141 +++++------------- .../AP_InertialSensor/AP_InertialSensor_HIL.h | 42 +++--- 4 files changed, 86 insertions(+), 126 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 9aa8689cf9..284fcfbdc7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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; igyro_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; + } +} + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0630dcf738..02a094590f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp index 4ef14c30e9..9acb38afa4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.cpp +++ b/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 +#include "AP_InertialSensor_HIL.h" + const extern AP_HAL::HAL& hal; -AP_InertialSensor_HIL::AP_InertialSensor_HIL() : - AP_InertialSensor(), +AP_InertialSensor_HIL::AP_InertialSensor_HIL(AP_InertialSensor &imu, Vector3f &gyro, Vector3f &accel) : + 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 { - return _sample_period_usec * 1.0e-6f; -} - -float AP_InertialSensor_HIL::get_gyro_drift_rate(void) { - // 0.5 degrees/second/minute - return ToRad(0.5/60); +bool AP_InertialSensor_HIL::update(void) +{ + uint32_t now = hal.scheduler->micros(); + while (now - _last_sample_usec > _sample_period_usec) { + _last_sample_usec += _sample_period_usec; + } + return true; } bool AP_InertialSensor_HIL::_sample_available() { - uint32_t tnow = hal.scheduler->micros(); - bool have_sample = false; - while (tnow - _last_sample_usec > _sample_period_usec) { - have_sample = true; - _last_sample_usec += _sample_period_usec; - } - return have_sample; + return (hal.scheduler->micros() - _last_sample_usec > _sample_period_usec); } -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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h b/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h index 55ae828ad8..eaf1bdfe0c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_HIL.h +++ b/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__ -#define __AP_INERTIAL_SENSOR_STUB_H__ +#ifndef __AP_INERTIALSENSOR_HIL_H__ +#define __AP_INERTIALSENSOR_HIL_H__ -#include #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 update(); - float get_delta_time() const; - float get_gyro_drift_rate(); - bool wait_for_sample(uint16_t timeout_ms); - 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; - uint8_t get_gyro_count(void) const; - uint8_t get_accel_count(void) const; + bool gyro_sample_available(void) { return _sample_available(); } + bool accel_sample_available(void) { return _sample_available(); } + + // detect the sensor + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, + AP_InertialSensor::Sample_rate sample_rate, + Vector3f &gyro, + Vector3f &accel); private: - bool _sample_available(); - uint16_t _init_sensor( Sample_rate sample_rate ); - uint32_t _sample_period_usec; - uint32_t _last_sample_usec; - uint32_t _last_accel_usec[INS_MAX_INSTANCES]; - uint32_t _last_gyro_usec[INS_MAX_INSTANCES]; + bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); + bool _sample_available(void); + uint32_t _sample_period_usec; + uint32_t _last_sample_usec; }; -#endif // __AP_INERTIAL_SENSOR_STUB_H__ +#endif // __AP_INERTIALSENSOR_HIL_H__