From 85686c22ec920862f962fef8ece2d351ec052776 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 16 Oct 2014 10:31:21 +1100 Subject: [PATCH] AP_InertialSensor: converted the APM1/Oilpan driver to new API --- .../AP_InertialSensor_Oilpan.cpp | 170 ++++++++---------- .../AP_InertialSensor_Oilpan.h | 43 ++--- 2 files changed, 87 insertions(+), 126 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index 7070ff8e87..3a5af54921 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -1,11 +1,16 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- #include + #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #include "AP_InertialSensor_Oilpan.h" +#include const extern AP_HAL::HAL& hal; +// this driver assumes an AP_ADC object has been declared globally +extern AP_ADC_ADS7844 apm1_adc; + // ADC channel mappings on for the APM Oilpan // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; @@ -14,9 +19,6 @@ const uint8_t AP_InertialSensor_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6 }; const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = { 1, -1, -1, 1, -1, -1 }; -// ADC channel reading the gyro temperature -const uint8_t AP_InertialSensor_Oilpan::_gyro_temp_ch = 3; - // Maximum possible value returned by an offset-corrected sensor channel const float AP_InertialSensor_Oilpan::_adc_constraint = 900; @@ -30,120 +32,94 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900; #define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) #define OILPAN_RAW_GYRO_OFFSET 1658.0f -#define ToRad(x) radians(x) // *pi/180 // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, // 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 -const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4f); -const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41f); -const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_x = radians(0.4f); +const float AP_InertialSensor_Oilpan::_gyro_gain_y = radians(0.41f); +const float AP_InertialSensor_Oilpan::_gyro_gain_z = radians(0.41f); /* ------ Public functions -------------------------------------------*/ -AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) : - AP_InertialSensor(), - _adc(adc) +AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan(AP_InertialSensor &imu) : + AP_InertialSensor_Backend(imu) { } -uint16_t AP_InertialSensor_Oilpan::_init_sensor( Sample_rate sample_rate) -{ - _adc->Init(); - - switch (sample_rate) { - case RATE_50HZ: - _sample_threshold = 20; - break; - case RATE_100HZ: - _sample_threshold = 10; - break; - case RATE_200HZ: - _sample_threshold = 5; - break; - } - -#if defined(__AVR_ATmega1280__) - return AP_PRODUCT_ID_APM1_1280; -#else - return AP_PRODUCT_ID_APM1_2560; -#endif -} - -bool AP_InertialSensor_Oilpan::update() -{ - if (!wait_for_sample(100)) { - return false; - } - float adc_values[6]; - Vector3f gyro_offset = _gyro_offset[0].get(); - Vector3f accel_scale = _accel_scale[0].get(); - Vector3f accel_offset = _accel_offset[0].get(); - - _delta_time_micros = _adc->Ch6(_sensors, adc_values); - _temp = _adc->Ch(_gyro_temp_ch); - - _gyro[0] = Vector3f(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ), - _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET )); - _gyro[0].rotate(_board_orientation); - _gyro[0].x *= _gyro_gain_x; - _gyro[0].y *= _gyro_gain_y; - _gyro[0].z *= _gyro_gain_z; - _gyro[0] -= gyro_offset; - - _previous_accel[0] = _accel[0]; - - _accel[0] = Vector3f(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), - _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); - _accel[0].rotate(_board_orientation); - _accel[0].x *= accel_scale.x; - _accel[0].y *= accel_scale.y; - _accel[0].z *= accel_scale.z; - _accel[0] *= OILPAN_ACCEL_SCALE_1G; - _accel[0] -= accel_offset; - /* - * X = 1619.30 to 2445.69 - * Y = 1609.45 to 2435.42 - * Z = 1627.44 to 2434.82 + detect the sensor */ +AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu, + AP_InertialSensor::Sample_rate sample_rate) +{ + AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu); + if (sensor == NULL) { + return NULL; + } + if (!sensor->_init_sensor(sample_rate)) { + delete sensor; + return NULL; + } + + return sensor; +} + +bool AP_InertialSensor_Oilpan::_init_sensor(AP_InertialSensor::Sample_rate sample_rate) +{ + apm1_adc.Init(); + + switch (sample_rate) { + case AP_InertialSensor::RATE_50HZ: + _sample_threshold = 20; + break; + case AP_InertialSensor::RATE_100HZ: + _sample_threshold = 10; + break; + case AP_InertialSensor::RATE_200HZ: + _sample_threshold = 5; + break; + default: + // can't do this speed + return false; + } + + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); return true; } -float AP_InertialSensor_Oilpan::get_delta_time() const { - return _delta_time_micros * 1.0e-6; -} - -/* ------ Private functions -------------------------------------------*/ - -// return the oilpan gyro drift rate in radian/s/s -float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void) +/* + copy data from ADC to frontend + */ +bool AP_InertialSensor_Oilpan::update() { - // 3.0 degrees/second/minute - return ToRad(3.0/60); + float adc_values[6]; + uint32_t now = hal.scheduler->micros(); + + apm1_adc.Ch6(_sensors, adc_values); + + // copy gyros to frontend + Vector3f v; + v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x, + _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y, + _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z); + _rotate_and_offset_gyro(_gyro_instance, v, now); + + // copy accels to frontend + v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET), + _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET)); + v *= OILPAN_ACCEL_SCALE_1G; + _rotate_and_offset_accel(_accel_instance, v, now); + + return true; } // return true if a new sample is available -bool AP_InertialSensor_Oilpan::_sample_available() +bool AP_InertialSensor_Oilpan::_sample_available() const { - return (_adc->num_samples_available(_sensors) / _sample_threshold) > 0; -} - -bool AP_InertialSensor_Oilpan::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) { - hal.scheduler->delay_microseconds(100); - if (_sample_available()) { - return true; - } - } - return false; + return apm1_adc.num_samples_available(_sensors) >= _sample_threshold; } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index bffb4f2114..8e9d7cb1ac 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -3,50 +3,35 @@ #ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ #define __AP_INERTIAL_SENSOR_OILPAN_H__ -#include - -#include -#include +#include #include "AP_InertialSensor.h" -class AP_InertialSensor_Oilpan : public AP_InertialSensor +class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend { public: + AP_InertialSensor_Oilpan(AP_InertialSensor &imu); - AP_InertialSensor_Oilpan( AP_ADC * adc ); + /* 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(); - - // wait for a sample to be available, with timeout in milliseconds - bool wait_for_sample(uint16_t timeout_ms); - -protected: - uint16_t _init_sensor(Sample_rate sample_rate); + 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); private: - - bool _sample_available(); - - AP_ADC * _adc; - - float _temp; - - uint32_t _delta_time_micros; - + bool _init_sensor(AP_InertialSensor::Sample_rate sample_rate); + bool _sample_available() const; static const uint8_t _sensors[6]; static const int8_t _sensor_signs[6]; - static const uint8_t _gyro_temp_ch; - static const float _gyro_gain_x; static const float _gyro_gain_y; static const float _gyro_gain_z; - static const float _adc_constraint; - uint8_t _sample_threshold; + uint8_t _gyro_instance; + uint8_t _accel_instance; }; #endif // __AP_INERTIAL_SENSOR_OILPAN_H__