mirror of https://github.com/ArduPilot/ardupilot
131 lines
3.3 KiB
C++
131 lines
3.3 KiB
C++
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_InertialSensor_QURT.h"
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
|
|
|
|
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 == NULL) {
|
|
return NULL;
|
|
}
|
|
if (!sensor->init_sensor()) {
|
|
delete sensor;
|
|
return NULL;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
bool AP_InertialSensor_QURT::init_sensor(void)
|
|
{
|
|
gyro_instance = _imu.register_gyro(1000);
|
|
accel_instance = _imu.register_accel(1000);
|
|
|
|
mpu9250_mag_buffer = new ObjectBuffer<mpu9x50_data>(20);
|
|
init_mpu9250();
|
|
|
|
_product_id = AP_PRODUCT_ID_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 NULL;
|
|
}
|
|
|
|
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
|