From 8b32960d3e1f84c012c353a0dfa6f2cdbf08553f Mon Sep 17 00:00:00 2001 From: Francisco Ferreira Date: Fri, 11 May 2018 13:26:52 +0100 Subject: [PATCH] AP_InertialSensor: remove Qualcomm board support --- .../AP_InertialSensor/AP_InertialSensor.cpp | 6 - .../AP_InertialSensor_QURT.cpp | 128 ------------------ .../AP_InertialSensor_QURT.h | 44 ------ .../AP_InertialSensor_qflight.cpp | 71 ---------- .../AP_InertialSensor_qflight.h | 31 ----- 5 files changed, 280 deletions(-) delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_QURT.h delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp delete mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_qflight.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 359b5db432..541fb526d3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -19,9 +19,7 @@ #include "AP_InertialSensor_LSM9DS1.h" #include "AP_InertialSensor_Invensense.h" #include "AP_InertialSensor_PX4.h" -#include "AP_InertialSensor_QURT.h" #include "AP_InertialSensor_SITL.h" -#include "AP_InertialSensor_qflight.h" #include "AP_InertialSensor_RST.h" #include "AP_InertialSensor_Revo.h" @@ -840,10 +838,6 @@ AP_InertialSensor::detect_backends(void) ADD_BACKEND(AP_InertialSensor_L3G4200D::probe(*this, hal.i2c_mgr->get_device(HAL_INS_L3G4200D_I2C_BUS, HAL_INS_L3G4200D_I2C_ADDR))); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR))); -#elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT - ADD_BACKEND(AP_InertialSensor_QFLIGHT::detect(*this)); -#elif HAL_INS_DEFAULT == HAL_INS_QURT - ADD_BACKEND(AP_InertialSensor_QURT::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_BBBMINI ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT))); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp deleted file mode 100644 index f867361402..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT -#include "AP_InertialSensor_QURT.h" - -const extern AP_HAL::HAL& hal; - -ObjectBuffer *mpu9250_mag_buffer = nullptr; - -AP_InertialSensor_QURT::AP_InertialSensor_QURT(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) -{ -} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_QURT::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_QURT *sensor = new AP_InertialSensor_QURT(_imu); - if (sensor == nullptr) { - return nullptr; - } - if (!sensor->init_sensor()) { - delete sensor; - return nullptr; - } - return sensor; -} - -bool AP_InertialSensor_QURT::init_sensor(void) -{ - gyro_instance = _imu.register_gyro(1000, 1); - accel_instance = _imu.register_accel(1000, 1); - - mpu9250_mag_buffer = new ObjectBuffer(20); - init_mpu9250(); - - return true; -} - -/* - handle data ready interrupt from mpu9250 - */ -extern "C" { - static void *mpu_data_ready_trampoline(void *ctx); -} -static void *mpu_data_ready_trampoline(void *ctx) -{ - ((AP_InertialSensor_QURT *)ctx)->data_ready(); - return nullptr; -} - -void AP_InertialSensor_QURT::init_mpu9250(void) -{ - struct mpu9x50_config config; - - config.gyro_lpf = MPU9X50_GYRO_LPF_184HZ; - config.acc_lpf = MPU9X50_ACC_LPF_184HZ; - config.gyro_fsr = MPU9X50_GYRO_FSR_2000DPS; - config.acc_fsr = MPU9X50_ACC_FSR_16G; - config.gyro_sample_rate = MPU9x50_SAMPLE_RATE_1000HZ; - config.compass_enabled = true; - config.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ; - config.spi_dev_path = "/dev/spi-1"; - - int ret; - ret = mpu9x50_validate_configuration(&config); - if (ret != 0) { - AP_HAL::panic("Bad MPU9x50 configuration"); - } - - ret = mpu9x50_initialize(&config); - if (ret != 0) { - AP_HAL::panic("Failed to initialise mpu9250"); - } - - mpu9x50_register_interrupt(65, mpu_data_ready_trampoline, this); - HAP_PRINTF("Opened MPU9X50"); -} - - -void AP_InertialSensor_QURT::data_ready(void) -{ - uint64_t now = AP_HAL::micros64(); - struct mpu9x50_data data; - int ret = mpu9x50_get_data(&data); - if (ret == 0) { - data.timestamp = now; - buf.push(data); - if (data.mag_data_ready) { - mpu9250_mag_buffer->push(data); - } - } -} - -void AP_InertialSensor_QURT::accumulate(void) -{ - const float ACCEL_SCALE_1G = GRAVITY_MSS / 2048.0; - const float GYRO_SCALE = 0.0174532 / 16.4; - - struct mpu9x50_data data; - - while (buf.pop(data)) { - Vector3f accel(data.accel_raw[0]*ACCEL_SCALE_1G, - data.accel_raw[1]*ACCEL_SCALE_1G, - data.accel_raw[2]*ACCEL_SCALE_1G); - Vector3f gyro(data.gyro_raw[0]*GYRO_SCALE, - data.gyro_raw[1]*GYRO_SCALE, - data.gyro_raw[2]*GYRO_SCALE); - - _rotate_and_correct_accel(accel_instance, accel); - _rotate_and_correct_gyro(gyro_instance, gyro); - - _notify_new_gyro_raw_sample(gyro_instance, gyro, data.timestamp); - _notify_new_accel_raw_sample(accel_instance, accel, data.timestamp); - } -} - -bool AP_InertialSensor_QURT::update(void) -{ - accumulate(); - update_accel(accel_instance); - update_gyro(gyro_instance); - return true; -} - -#endif // HAL_BOARD_QURT diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h deleted file mode 100644 index bc60ad5f0b..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT - -#include "AP_InertialSensor.h" -#include "AP_InertialSensor_Backend.h" - -extern "C" { -#undef DEG_TO_RAD -#include -} -#include - -extern ObjectBuffer *mpu9250_mag_buffer; - -class AP_InertialSensor_QURT : public AP_InertialSensor_Backend -{ -public: - AP_InertialSensor_QURT(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update() override; - void accumulate(void) override; - - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - - void data_ready(void); - -private: - bool init_sensor(); - void init_mpu9250(); - - uint64_t last_timestamp; - uint32_t start_time_ms; - - uint8_t gyro_instance; - uint8_t accel_instance; - - ObjectBuffer buf{100}; -}; - -#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp deleted file mode 100644 index 30d785aae5..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include - -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT - -#include "AP_InertialSensor_qflight.h" -#include -#include - -const extern AP_HAL::HAL& hal; - -AP_InertialSensor_QFLIGHT::AP_InertialSensor_QFLIGHT(AP_InertialSensor &imu) : - AP_InertialSensor_Backend(imu) -{ -} - -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_QFLIGHT::detect(AP_InertialSensor &_imu) -{ - AP_InertialSensor_QFLIGHT *sensor = new AP_InertialSensor_QFLIGHT(_imu); - if (sensor == nullptr) { - return nullptr; - } - if (!sensor->init_sensor()) { - delete sensor; - return nullptr; - } - return sensor; -} - -bool AP_InertialSensor_QFLIGHT::init_sensor(void) -{ - gyro_instance = _imu.register_gyro(1000, 1); - accel_instance = _imu.register_accel(1000, 1); - - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_QFLIGHT::timer_update, void)); - return true; -} - -void AP_InertialSensor_QFLIGHT::timer_update(void) -{ - if (imubuf == nullptr) { - imubuf = QFLIGHT_RPC_ALLOCATE(DSPBuffer::IMU); - if (imubuf == nullptr) { - AP_HAL::panic("unable to allocate IMU buffer"); - } - } - int ret = qflight_get_imu_data((uint8_t *)imubuf, sizeof(*imubuf)); - if (ret != 0) { - return; - } - for (uint16_t i=0; inum_samples; i++) { - DSPBuffer::IMU::BUF &b = imubuf->buf[i]; - Vector3f accel(b.accel[0], b.accel[1], b.accel[2]); - Vector3f gyro(b.gyro[0], b.gyro[1], b.gyro[2]); - _rotate_and_correct_accel(accel_instance, accel); - _rotate_and_correct_gyro(gyro_instance, gyro); - _notify_new_accel_raw_sample(accel_instance, accel, b.timestamp); - _notify_new_gyro_raw_sample(gyro_instance, gyro, b.timestamp); - } -} - -bool AP_InertialSensor_QFLIGHT::update(void) -{ - update_accel(accel_instance); - update_gyro(gyro_instance); - return true; -} - -#endif // HAL_BOARD_QFLIGHT diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h b/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h deleted file mode 100644 index ee55ce0180..0000000000 --- a/libraries/AP_InertialSensor/AP_InertialSensor_qflight.h +++ /dev/null @@ -1,31 +0,0 @@ -#pragma once - -#include -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT - -#include - -#include "AP_InertialSensor.h" -#include "AP_InertialSensor_Backend.h" - -class AP_InertialSensor_QFLIGHT : public AP_InertialSensor_Backend -{ -public: - AP_InertialSensor_QFLIGHT(AP_InertialSensor &imu); - - /* update accel and gyro state */ - bool update(); - - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); - -private: - bool init_sensor(void); - void timer_update(); - - uint8_t gyro_instance; - uint8_t accel_instance; - DSPBuffer::IMU *imubuf; -}; - -#endif