From b067bce140edd23e9466f903ffb8f610fe71f0d1 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 23 May 2016 09:50:58 -0300 Subject: [PATCH] AP_InertialSensor: remove support for flymaple --- .../AP_InertialSensor/AP_InertialSensor.cpp | 3 - .../AP_InertialSensor_Flymaple.cpp | 220 ------------------ .../AP_InertialSensor_Flymaple.h | 36 --- 3 files changed, 259 deletions(-) delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 8640a0f9ce..b2c5e31e1c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -12,7 +12,6 @@ #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" -#include "AP_InertialSensor_Flymaple.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_LSM9DS0.h" @@ -536,8 +535,6 @@ AP_InertialSensor::detect_backends(void) _add_backend(AP_InertialSensor_PX4::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); -#elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE - _add_backend(AP_InertialSensor_Flymaple::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 _add_backend(AP_InertialSensor_LSM9DS0::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp deleted file mode 100644 index 36ca3a52ae..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ /dev/null @@ -1,220 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ -/* - Flymaple IMU driver by Mike McCauley - */ - -// Interface to the Flymaple sensors: -// ITG3205 Gyroscope http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf -// ADXL345 Accelerometer http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE - -#include "AP_InertialSensor_Flymaple.h" - -const extern AP_HAL::HAL& hal; - -// This is how often we wish to make raw samples of the sensors in Hz -const uint32_t raw_sample_rate_hz = 800; -// And the equivalent time between samples in microseconds -const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz); - -/////// -/// Accelerometer ADXL345 register definitions -#define FLYMAPLE_ACCELEROMETER_ADDRESS 0x53 -#define FLYMAPLE_ACCELEROMETER_XL345_DEVID 0xe5 -#define FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE 0x2c -#define FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL 0x2d -#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT 0x31 -#define FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID 0x00 -#define FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0 0x32 -#define FLYMAPLE_ACCELEROMETER_GRAVITY 248 - -// ADXL345 accelerometer scaling -// Result will be scaled to 1m/s/s -// ADXL345 in Full resolution mode (any g scaling) is 256 counts/g, so scale by 9.81/256 = 0.038320312 -#define FLYMAPLE_ACCELEROMETER_SCALE_M_S (GRAVITY_MSS / 256.0f) - -/// Gyro ITG3205 register definitions -#define FLYMAPLE_GYRO_ADDRESS 0x68 -#define FLYMAPLE_GYRO_WHO_AM_I 0x00 -#define FLYMAPLE_GYRO_PWR_MGM 0x3e -#define FLYMAPLE_GYRO_DLPF_FS 0x16 -#define FLYMAPLE_GYRO_INT_CFG 0x17 -#define FLYMAPLE_GYRO_SMPLRT_DIV 0x15 -#define FLYMAPLE_GYRO_GYROX_H 0x1d - -// ITG3200 Gyroscope scaling -// ITG3200 is 14.375 LSB degrees/sec with FS_SEL=3 -// Result wil be radians/sec -#define FLYMAPLE_GYRO_SCALE_R_S (1.0f / 14.375f) * (3.1415926f / 180.0f) - -AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) -{} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_Flymaple::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_Flymaple *sensor = new AP_InertialSensor_Flymaple(_imu); - if (sensor == NULL) { - return NULL; - } - if (!sensor->_init_sensor()) { - delete sensor; - return NULL; - } - return sensor; -} - -bool AP_InertialSensor_Flymaple::_init_sensor(void) -{ - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - - // take i2c bus sempahore - if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) - return false; - - // Init the accelerometer - uint8_t data; - hal.i2c->readRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DEVID, &data); - if (data != FLYMAPLE_ACCELEROMETER_XL345_DEVID) - AP_HAL::panic("AP_InertialSensor_Flymaple: could not find ADXL345 accelerometer sensor"); - hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x00); - hal.scheduler->delay(5); - hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0xff); - hal.scheduler->delay(5); - // Measure mode: - hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_POWER_CTL, 0x08); - hal.scheduler->delay(5); - // Full resolution, 8g: - // Caution, this must agree with FLYMAPLE_ACCELEROMETER_SCALE_1G - // In full resoution mode, the scale factor need not change - hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATA_FORMAT, 0x08); - hal.scheduler->delay(5); - // Normal power, 800Hz Output Data Rate, 400Hz bandwidth: - hal.i2c->writeRegister(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_BW_RATE, 0x0d); - hal.scheduler->delay(5); - // Power up default is FIFO bypass mode. FIFO is not used by the chip - - // Init the Gyro - // Expect to read the same as the Gyro I2C address: - hal.i2c->readRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_WHO_AM_I, &data); - if (data != FLYMAPLE_GYRO_ADDRESS) - AP_HAL::panic("AP_InertialSensor_Flymaple: could not find ITG-3200 accelerometer sensor"); - hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_PWR_MGM, 0x00); - hal.scheduler->delay(1); - // Sample rate divider: with 8kHz internal clock (see FLYMAPLE_GYRO_DLPF_FS), - // get 500Hz sample rate, 2 samples - hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_SMPLRT_DIV, 0x0f); - hal.scheduler->delay(1); - // 2000 degrees/sec, 256Hz LPF, 8kHz internal sample rate - // This is the least amount of filtering we can configure for this device - hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_DLPF_FS, 0x18); - hal.scheduler->delay(1); - // No interrupts - hal.i2c->writeRegister(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_INT_CFG, 0x00); - hal.scheduler->delay(1); - - // give back i2c semaphore - i2c_sem->give(); - - _gyro_instance = _imu.register_gyro(raw_sample_rate_hz); - _accel_instance = _imu.register_accel(raw_sample_rate_hz); - - _product_id = AP_PRODUCT_ID_FLYMAPLE; - - return true; -} - -// This takes about 20us to run -bool AP_InertialSensor_Flymaple::update(void) -{ - update_accel(_accel_instance); - update_gyro(_gyro_instance); - - return true; -} - -// This needs to get called as often as possible. -// Its job is to accumulate samples as fast as is reasonable for the accel and gyro -// sensors. -// Note that this is called from gyro_sample_available() and -// accel_sample_available(), which is really not good enough for -// 800Hz, as it is common for the main loop to take more than 1.5ms -// before wait_for_sample() is called again. We can't just call this -// from a timer as timers run with interrupts disabled, and the I2C -// operations take too long -// So we are stuck with a suboptimal solution. The results are not so -// good in terms of timing. It may be better with the FIFOs enabled -void AP_InertialSensor_Flymaple::accumulate(void) -{ - // get pointer to i2c bus semaphore - AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); - - // take i2c bus sempahore - if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) - return; - - // Read accelerometer - // ADXL345 is in the default FIFO bypass mode, so the FIFO is not used - uint8_t buffer[6]; - uint32_t now = AP_HAL::micros(); - // This takes about 250us at 400kHz I2C speed - if ((now - _last_accel_timestamp) >= raw_sample_interval_us - && hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 6, buffer) == 0) - { - // The order is a bit weird here since the standard we have adopted for Flymaple - // sensor orientation is different to what the board designers intended - // Caution, to support alternative chip orientations on other bords, may - // need to add a chip orientation rotate - int16_t y = -((((int16_t)buffer[1]) << 8) | buffer[0]); // chip X axis - int16_t x = -((((int16_t)buffer[3]) << 8) | buffer[2]); // chip Y axis - int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis - Vector3f accel = Vector3f(x,y,z); - // Adjust for chip scaling to get m/s/s - accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S; - _rotate_and_correct_accel(_accel_instance, accel); - _notify_new_accel_raw_sample(_accel_instance, accel); - _last_accel_timestamp = now; - } - - // Read gyro - now = AP_HAL::micros(); - // This takes about 250us at 400kHz I2C speed - if ((now - _last_gyro_timestamp) >= raw_sample_interval_us - && hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0) - { - // See above re order of samples in buffer - int16_t y = -((((int16_t)buffer[0]) << 8) | buffer[1]); // chip X axis - int16_t x = -((((int16_t)buffer[2]) << 8) | buffer[3]); // chip Y axis - int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // chip Z axis - Vector3f gyro = Vector3f(x,y,z); - // Adjust for chip scaling to get radians/sec - gyro *= FLYMAPLE_GYRO_SCALE_R_S; - _rotate_and_correct_gyro(_gyro_instance, gyro); - _notify_new_gyro_raw_sample(_gyro_instance, gyro); - } - - // give back i2c semaphore - i2c_sem->give(); -} - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h deleted file mode 100644 index c166701e41..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h +++ /dev/null @@ -1,36 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#pragma once - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE - -#include -#include - -#include "AP_InertialSensor.h" -#include "AP_InertialSensor_Backend.h" - -class AP_InertialSensor_Flymaple : public AP_InertialSensor_Backend -{ -public: - AP_InertialSensor_Flymaple(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update(); - - // accumulate samples - void accumulate(void) override; - - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - -private: - bool _init_sensor(void); - - uint8_t _gyro_instance; - uint8_t _accel_instance; - - uint32_t _last_gyro_timestamp; - uint32_t _last_accel_timestamp; -}; -#endif