mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
469efb00f6
This allows each sensor to be uniquely identified in the system by using either the index inside the backend or for those that use the Device interface, to use the bus type, location, and device id. We leave 16-bit for each sensor to be able to change its own identification in future, which allows them to be changed in an incompatible manner forcing a re-calibration.
129 lines
3.3 KiB
C++
129 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 == 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
|