Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: remove Qualcomm board support

This commit is contained in:
Francisco Ferreira 2018-05-11 13:26:52 +01:00 committed by Andrew Tridgell
parent 6f210131ed
commit 8b32960d3e
5 changed files with 0 additions and 280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -19,9 +19,7 @@
#include "AP_InertialSensor_LSM9DS1.h" #include "AP_InertialSensor_LSM9DS1.h"
#include "AP_InertialSensor_Invensense.h" #include "AP_InertialSensor_Invensense.h"
#include "AP_InertialSensor_PX4.h" #include "AP_InertialSensor_PX4.h"
#include "AP_InertialSensor_QURT.h"
#include "AP_InertialSensor_SITL.h" #include "AP_InertialSensor_SITL.h"
#include "AP_InertialSensor_qflight.h"
#include "AP_InertialSensor_RST.h" #include "AP_InertialSensor_RST.h"
#include "AP_InertialSensor_Revo.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))); 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 #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))); 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 #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)));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT))); ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME_EXT)));

128
libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp
View File

@ -1,128 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_InertialSensor_QURT.h"
const extern AP_HAL::HAL& hal;
ObjectBuffer<mpu9x50_data> *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<mpu9x50_data>(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

44
libraries/AP_InertialSensor/AP_InertialSensor_QURT.h
View File

@ -1,44 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
extern "C" {
#undef DEG_TO_RAD
#include <mpu9x50.h>
}
#include <AP_HAL/utility/RingBuffer.h>
extern ObjectBuffer<mpu9x50_data> *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<mpu9x50_data> buf{100};
};
#endif

71
libraries/AP_InertialSensor/AP_InertialSensor_qflight.cpp
View File

@ -1,71 +0,0 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include "AP_InertialSensor_qflight.h"
#include <AP_HAL_Linux/qflight/qflight_util.h>
#include <AP_HAL_Linux/qflight/qflight_dsp.h>
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; i<imubuf->num_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

31
libraries/AP_InertialSensor/AP_InertialSensor_qflight.h
View File

@ -1,31 +0,0 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
#include <AP_HAL_Linux/qflight/qflight_buffer.h>
#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