From 3142f2136392800fddf268e0329a35fd9867f34b Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 3 Nov 2015 11:46:29 -0200 Subject: [PATCH] AP_InertialSensor: remove checks for HAL_BOARD_APM2 and HAL_BOARD_APM1 --- libraries/AP_HAL/AP_HAL_Boards.h | 1 - .../AP_InertialSensor/AP_InertialSensor.cpp | 2 - .../AP_InertialSensor/AP_InertialSensor.h | 1 - .../AP_InertialSensor_MPU6000.cpp | 6 +- .../AP_InertialSensor_MPU9250.cpp | 1 - .../AP_InertialSensor_Oilpan.cpp | 128 ------------------ .../AP_InertialSensor_Oilpan.h | 37 ----- .../examples/INS_generic/INS_generic.cpp | 10 -- 8 files changed, 2 insertions(+), 184 deletions(-) delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 9b6af348b7..9be12445ff 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -49,7 +49,6 @@ #define HAL_BOARD_SUBTYPE_VRUBRAIN_V52 4004 // InertialSensor driver types -#define HAL_INS_OILPAN 1 #define HAL_INS_MPU60XX_SPI 2 #define HAL_INS_MPU60XX_I2C 3 #define HAL_INS_HIL 4 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 78814c621d..28cf6b65a9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -500,8 +500,6 @@ AP_InertialSensor::_detect_backends(void) _add_backend(AP_InertialSensor_MPU6000::detect_i2c(*this, hal.i2c2, HAL_INS_MPU60XX_I2C_ADDR)); #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN _add_backend(AP_InertialSensor_PX4::detect(*this)); -#elif HAL_INS_DEFAULT == HAL_INS_OILPAN - _add_backend(AP_InertialSensor_Oilpan::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250 _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250))); #elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 75262d23eb..92823f8ae5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -381,7 +381,6 @@ private: #include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_MPU6000.h" #include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_Oilpan.h" #include "AP_InertialSensor_MPU9250.h" #include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_Flymaple.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index a636305e57..e4e377fb2e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -10,9 +10,7 @@ extern const AP_HAL::HAL& hal; // MPU6000 accelerometer scaling #define MPU6000_ACCEL_SCALE_1G (GRAVITY_MSS / 4096.0f) -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 -#define MPU6000_DRDY_PIN 70 -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF #define MPU6000_DRDY_PIN BBB_P8_14 @@ -808,7 +806,7 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) hal.scheduler->panic("MPU6000: Unable to get semaphore"); } - // initially run the bus at low speed (500kHz on APM2) + // initially run the bus at low speed _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // Chip reset diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 0cc6e3495e..320c67093c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -514,7 +514,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) _register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN); // now that we have initialised, we set the SPI bus speed to high - // (8MHz on APM2) _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _spi_sem->give(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp deleted file mode 100644 index 361a8a9a6f..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/// -*- 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 }; - -// ADC result sign adjustment for sensors. -const int8_t AP_InertialSensor_Oilpan::_sensor_signs[6] = -{ 1, -1, -1, 1, -1, -1 }; - -// Maximum possible value returned by an offset-corrected sensor channel -const float AP_InertialSensor_Oilpan::_adc_constraint = 900; - -// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step -// ADXL335 Sensitivity(from datasheet) => 330mV/g, -// 0.8mV/ADC step => 330/0.8 = 412 -// Tested value : 418 - -// Oilpan accelerometer scaling & offsets -#define OILPAN_ACCEL_SCALE_1G (GRAVITY_MSS * 2.0f / (2465.0f - 1617.0f)) -#define OILPAN_RAW_ACCEL_OFFSET ((2465.0f + 1617.0f) * 0.5f) -#define OILPAN_RAW_GYRO_OFFSET 1658.0f - -// 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 = 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_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) -{ -} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_Oilpan::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_Oilpan *sensor = new AP_InertialSensor_Oilpan(_imu); - if (sensor == NULL) { - return NULL; - } - if (!sensor->_init_sensor()) { - delete sensor; - return NULL; - } - - return sensor; -} - -bool AP_InertialSensor_Oilpan::_init_sensor(void) -{ - apm1_adc.Init(); - - switch (_imu.get_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(); - - _product_id = AP_PRODUCT_ID_APM1_2560; - - return true; -} - -/* - copy data from ADC to frontend - */ -bool AP_InertialSensor_Oilpan::update() -{ - float adc_values[6]; - - 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_correct_gyro(_gyro_instance, v); - _publish_gyro(_gyro_instance, v); - - // 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_correct_accel(_accel_instance, v); - _publish_accel(_accel_instance, v); - - return true; -} - -// return true if a new sample is available -bool AP_InertialSensor_Oilpan::_sample_available() const -{ - 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 deleted file mode 100644 index 3a785b1afe..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ /dev/null @@ -1,37 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_OILPAN_H__ -#define __AP_INERTIAL_SENSOR_OILPAN_H__ - -#include -#include "AP_InertialSensor.h" - -class AP_InertialSensor_Oilpan : public AP_InertialSensor_Backend -{ -public: - AP_InertialSensor_Oilpan(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update(); - - 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); - -private: - bool _init_sensor(void); - bool _sample_available() const; - static const uint8_t _sensors[6]; - static const int8_t _sensor_signs[6]; - 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__ diff --git a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp index 44a959bfd1..0a7c75de2e 100644 --- a/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp +++ b/libraries/AP_InertialSensor/examples/INS_generic/INS_generic.cpp @@ -13,10 +13,6 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); AP_InertialSensor ins; -#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 -AP_ADC_ADS7844 apm1_adc; -#endif - static void display_offsets_and_scaling(); static void run_test(); static void run_calibration(); @@ -25,12 +21,6 @@ void setup(void) { hal.console->println("AP_InertialSensor startup..."); -#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 - // we need to stop the barometer from holding the SPI bus - hal.gpio->pinMode(40, HAL_GPIO_OUTPUT); - hal.gpio->write(40, 1); -#endif - ins.init(AP_InertialSensor::RATE_100HZ); // display initial values