ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp

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