AP_InertialSensor: added QURT driver

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

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"
 

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 <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

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 <AP_HAL/utility/RingBuffer.h>
+
+extern ObjectBuffer<mpu9x50_data> *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<mpu9x50_data> buf{100};
+};
+
+#endif // CONFIG_HAL_BOARD