From 6639c23685a44bfd6627b54f3398fab6975f8f68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Dec 2015 08:48:25 +1100 Subject: [PATCH] AP_InertialSensor: added QURT driver --- .../AP_InertialSensor/AP_InertialSensor.cpp | 2 + .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_QURT.cpp | 132 ++++++++++++++++++ .../AP_InertialSensor_QURT.h | 42 ++++++ 4 files changed, 177 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_QURT.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 80d57f153e..3b5ba873f7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -530,6 +530,8 @@ AP_InertialSensor::detect_backends(void) 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)); #else #error Unrecognised HAL_INS_TYPE setting #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 3e89615670..22b457953b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -400,6 +400,7 @@ private: #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_SITL.h" #include "AP_InertialSensor_qflight.h" +#include "AP_InertialSensor_QURT.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp new file mode 100644 index 0000000000..92abce6ac4 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.cpp @@ -0,0 +1,132 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include "AP_InertialSensor_QURT.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +const extern AP_HAL::HAL& hal; + +ObjectBuffer *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(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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h new file mode 100644 index 0000000000..8cc7874ef6 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_QURT.h @@ -0,0 +1,42 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +#include "AP_InertialSensor.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +extern "C" { +#undef DEG_TO_RAD +#include "mpu9x50.h" +} +#include + +extern ObjectBuffer *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 buf{100}; +}; + +#endif // CONFIG_HAL_BOARD