mirror of https://github.com/ArduPilot/ardupilot
75 lines
2.1 KiB
C++
75 lines
2.1 KiB
C++
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||
|
|
|
||
|
|
#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 == NULL) {
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
if (!sensor->init_sensor()) {
|
||
|
|
delete sensor;
|
||
|
|
return NULL;
|
||
|
|
}
|
||
|
|
return sensor;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool AP_InertialSensor_QFLIGHT::init_sensor(void)
|
||
|
|
{
|
||
|
|
gyro_instance = _imu.register_gyro(1000);
|
||
|
|
accel_instance = _imu.register_accel(1000);
|
||
|
|
|
||
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_QFLIGHT::timer_update, void));
|
||
|
|
_product_id = AP_PRODUCT_ID_MPU9250;
|
||
|
|
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
|