temp mahony filter

main/CMakeLists.txt

@@ -2,6 +2,8 @@ idf_component_register(
     SRCS
         "bbot.c"
         "bmi160.c"
+        "fixedpoint.c"
+        "mahony_filter.c"
         "motors.c"
     INCLUDE_DIRS 
         "."

main/bbot.c

@@ -1,4 +1,6 @@
 #include <stdio.h>
+#include <inttypes.h>
+#include <stdbool.h>
 
 #include "driver/gpio.h"
 #include "driver/i2c.h"
@@ -6,10 +8,13 @@
 #include "esp_log.h"
 #include "freertos/FreeRTOS.h"
 #include "freertos/queue.h"
+#include "freertos/semphr.h"
 #include "freertos/task.h"
 
 #include "motors.h"
 #include "bmi160.h"
+#include "fixedpoint.h"
+#include "mahony_filter.h"
 
 #define BBOT_DRDY_INPUT_PIN GPIO_NUM_1
 
@@ -18,8 +23,77 @@
 #define BBOT_I2C_SDA_IO GPIO_NUM_2
 #define BBOT_I2C_FREQ_HZ 100000
 
+#define BBOT_FILTER_FRAC_BITS 12
+#define BBOT_FILTER_KP FIXED_FROM_INT(2, BBOT_FILTER_FRAC_BITS)
+#define BBOT_FILTER_KI 0
+#define BBOT_PROCESS_TASK_STACK_SIZE 4096
+#define BBOT_PROCESS_TASK_PRIORITY 5
+#define BBOT_PRINT_PERIOD_MS 20
+
 static bmi160_t imu;
 static QueueHandle_t imu_queue;
+static SemaphoreHandle_t imu_state_mutex;
+
+typedef struct
+{
+    mahony_filter_t filter;
+    bmi160_value_t sample;
+    int32_t yaw;
+    int32_t pitch;
+    int32_t roll;
+    uint32_t last_time;
+    bool has_sample;
+    bool has_orientation;
+} imu_state_t;
+
+static imu_state_t imu_state;
+
+static void imu_process_task(void *arg)
+{
+    QueueHandle_t queue = (QueueHandle_t)arg;
+
+    for (;;)
+    {
+        bmi160_value_t sample;
+
+        if (xQueueReceive(queue, &sample, portMAX_DELAY) != pdTRUE)
+        {
+            continue;
+        }
+
+        if (xSemaphoreTake(imu_state_mutex, portMAX_DELAY) != pdTRUE)
+        {
+            continue;
+        }
+
+        if (imu_state.has_sample)
+        {
+            const uint32_t dt_ticks = sample.time - imu_state.last_time;
+            const int32_t dt = FIXED_FROM_RATIO((int32_t)dt_ticks, 100, BBOT_FILTER_FRAC_BITS);
+
+            mahony_filter_update_imu(
+                &imu_state.filter,
+                sample.gyr.x,
+                sample.gyr.y,
+                sample.gyr.z,
+                sample.acc.x,
+                sample.acc.y,
+                sample.acc.z,
+                dt);
+            mahony_filter_get_yaw_pitch_roll(
+                &imu_state.filter,
+                &imu_state.yaw,
+                &imu_state.pitch,
+                &imu_state.roll);
+            imu_state.has_orientation = true;
+        }
+
+        imu_state.sample = sample;
+        imu_state.last_time = sample.time;
+        imu_state.has_sample = true;
+        xSemaphoreGive(imu_state_mutex);
+    }
+}
 
 static esp_err_t init_i2c()
 {
@@ -39,16 +113,80 @@ static esp_err_t init_i2c()
 
 void app_main(void)
 {
+    imu_state_mutex = xSemaphoreCreateMutex();
+    if (imu_state_mutex == NULL)
+    {
+        ESP_ERROR_CHECK(ESP_ERR_NO_MEM);
+    }
+
+    mahony_filter_init(&imu_state.filter, BBOT_FILTER_FRAC_BITS);
+    mahony_filter_set_gains(&imu_state.filter, BBOT_FILTER_KP, BBOT_FILTER_KI);
+
     ESP_ERROR_CHECK(init_motors());
     ESP_ERROR_CHECK(init_i2c());
     ESP_ERROR_CHECK(imu_init(&imu, BBOT_I2C_PORT, BBOT_DRDY_INPUT_PIN, &imu_queue));
+    if (xTaskCreate(
+            imu_process_task,
+            "imu_process",
+            BBOT_PROCESS_TASK_STACK_SIZE,
+            imu_queue,
+            BBOT_PROCESS_TASK_PRIORITY,
+            NULL) != pdPASS)
+    {
+        ESP_ERROR_CHECK(ESP_ERR_NO_MEM);
+    }
 
     while (1)
     {
-        bmi160_value_t v;
-        while (xQueueReceive(imu_queue, &v, pdMS_TO_TICKS(1000)) == pdTRUE)
+        bmi160_value_t sample;
+        int32_t q0;
+        int32_t q1;
+        int32_t q2;
+        int32_t q3;
+        int32_t yaw;
+        int32_t pitch;
+        int32_t roll;
+        bool has_sample;
+        bool has_orientation;
+
+        if (xSemaphoreTake(imu_state_mutex, pdMS_TO_TICKS(50)) == pdTRUE)
         {
-            ESP_LOGI("[<IMU>]", "%d %d %d %d %d %d %d", v.time, v.acc.x, v.acc.y, v.acc.z, v.gyr.x, v.gyr.y, v.gyr.z);
+            sample = imu_state.sample;
+            mahony_filter_get_quaternion(&imu_state.filter, &q0, &q1, &q2, &q3);
+            yaw = imu_state.yaw;
+            pitch = imu_state.pitch;
+            roll = imu_state.roll;
+            has_sample = imu_state.has_sample;
+            has_orientation = imu_state.has_orientation;
+            xSemaphoreGive(imu_state_mutex);
         }
+        else
+        {
+            vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS));
+            continue;
+        }
+
+        if (has_sample && has_orientation)
+        {
+            ESP_LOGI(
+                "[<IMU>]",
+                "%" PRIu32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32,
+                sample.time,
+                sample.acc.x,
+                sample.acc.y,
+                sample.acc.z,
+                sample.gyr.x,
+                sample.gyr.y,
+                sample.gyr.z,
+                q0,
+                q1,
+                q2,
+                q3,
+                yaw,
+                pitch,
+                roll);
+        }
+
+        vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS));
     }
 }

main/bmi160.c

@@ -236,9 +236,9 @@ esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value)
     value->acc.y = FIXED_FROM_RATIO(acc_y, 2048, 12);
     value->acc.z = FIXED_FROM_RATIO(acc_z, 2048, 12);
 
-    value->gyr.x = FIXED_FROM_RATIO((int32_t)gyr_x * 5, 82, 12);
-    value->gyr.y = FIXED_FROM_RATIO((int32_t)gyr_y * 5, 82, 12);
-    value->gyr.z = FIXED_FROM_RATIO((int32_t)gyr_z * 5, 82, 12);
+    value->gyr.x = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_x * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453)
+    value->gyr.y = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_y * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453)
+    value->gyr.z = FIXED_MUL(FIXED_FROM_RATIO((int32_t)gyr_z * 5, 82, 12), FIXED_FROM_RATIO(31416, 1800000, 12), 12); // Scale to rad/s (pi/180 = 0.017453)
 
     uint8_t time[3];
     ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME));

main/fixedpoint.c

@@ -0,0 +1,156 @@
+#include "fixedpoint.h"
+
+#include <limits.h>
+#include <stddef.h>
+
+static const int32_t s_fixed_atan_table_q30[] = {
+    843314857, 497837829, 263043836, 133525159,
+    67021687,  33543516,  16775851,  8388437,
+    4194283,   2097149,   1048575,   524288,
+    262144,    131072,    65536,     32768,
+};
+
+static uint32_t fixed_isqrt_u64(uint64_t value)
+{
+    uint64_t bit = (uint64_t)1 << 62;
+    uint64_t result = 0;
+
+    while (bit > value)
+    {
+        bit >>= 2;
+    }
+
+    while (bit != 0)
+    {
+        if (value >= result + bit)
+        {
+            value -= result + bit;
+            result = (result >> 1) + bit;
+        }
+        else
+        {
+            result >>= 1;
+        }
+
+        bit >>= 2;
+    }
+
+    return (uint32_t)result;
+}
+
+int32_t fixed_sqrt(int32_t value, uint8_t frac_bits)
+{
+    uint64_t scaled_value;
+    uint32_t root;
+
+    if (value <= 0)
+    {
+        return 0;
+    }
+
+    scaled_value = ((uint64_t)(uint32_t)value) << frac_bits;
+    root = fixed_isqrt_u64(scaled_value);
+    if (root > (uint32_t)INT32_MAX)
+    {
+        return INT32_MAX;
+    }
+
+    return (int32_t)root;
+}
+
+int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits)
+{
+    int32_t root;
+
+    if (value <= 0)
+    {
+        return 0;
+    }
+
+    root = fixed_sqrt(value, frac_bits);
+    return FIXED_DIV(FIXED_ONE(frac_bits), root, frac_bits);
+}
+
+int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits)
+{
+    int64_t x_q30;
+    int64_t y_q30;
+    int64_t angle_q30 = 0;
+    int shift;
+
+    if (x == 0)
+    {
+        if (y > 0)
+        {
+            return FIXED_HALF_PI(frac_bits);
+        }
+        if (y < 0)
+        {
+            return -FIXED_HALF_PI(frac_bits);
+        }
+        return 0;
+    }
+
+    if (x < 0)
+    {
+        if (y >= 0)
+        {
+            return fixed_atan2(-y, -x, frac_bits) + FIXED_PI(frac_bits);
+        }
+        return fixed_atan2(-y, -x, frac_bits) - FIXED_PI(frac_bits);
+    }
+
+    shift = 30 - frac_bits;
+    if (shift >= 0)
+    {
+        x_q30 = (int64_t)x << shift;
+        y_q30 = (int64_t)y << shift;
+    }
+    else
+    {
+        x_q30 = (int64_t)x >> (-shift);
+        y_q30 = (int64_t)y >> (-shift);
+    }
+
+    for (size_t i = 0; i < sizeof(s_fixed_atan_table_q30) / sizeof(s_fixed_atan_table_q30[0]); ++i)
+    {
+        int64_t x_next;
+        int64_t y_next;
+
+        if (y_q30 > 0)
+        {
+            x_next = x_q30 + (y_q30 >> i);
+            y_next = y_q30 - (x_q30 >> i);
+            angle_q30 += s_fixed_atan_table_q30[i];
+        }
+        else
+        {
+            x_next = x_q30 - (y_q30 >> i);
+            y_next = y_q30 + (x_q30 >> i);
+            angle_q30 -= s_fixed_atan_table_q30[i];
+        }
+
+        x_q30 = x_next;
+        y_q30 = y_next;
+    }
+
+    if (shift >= 0)
+    {
+        return (int32_t)(angle_q30 >> shift);
+    }
+    return (int32_t)(angle_q30 << (-shift));
+}
+
+int32_t fixed_asin(int32_t value, uint8_t frac_bits)
+{
+    const int32_t one = FIXED_ONE(frac_bits);
+    const int32_t clamped = FIXED_CLAMP(value, -one, one);
+    const int32_t one_minus_square = one - FIXED_MUL(clamped, clamped, frac_bits);
+
+    if (one_minus_square <= 0)
+    {
+        return clamped >= 0 ? FIXED_HALF_PI(frac_bits) : -FIXED_HALF_PI(frac_bits);
+    }
+
+    return fixed_atan2(clamped, fixed_sqrt(one_minus_square, frac_bits), frac_bits);
+}

main/fixedpoint.h

@@ -20,6 +20,8 @@
 #define FIXED_ONE(frac_bits) ((int32_t)1 << (frac_bits))
 #define FIXED_HALF(frac_bits) (FIXED_ONE(frac_bits) >> 1)
 #define FIXED_FRACTION_MASK(frac_bits) (FIXED_ONE(frac_bits) - 1)
+#define FIXED_PI(frac_bits) FIXED_FROM_RATIO(355, 113, frac_bits)
+#define FIXED_HALF_PI(frac_bits) (FIXED_PI(frac_bits) >> 1)
 
 #define FIXED_FROM_INT(value, frac_bits) ((value) << (frac_bits))
 #define FIXED_TO_INT(value, frac_bits) ((value) >> (frac_bits))
@@ -51,3 +53,8 @@
 
 #define FIXED_FROM_RATIO_T(type, numerator, denominator, frac_bits) \
     ((denominator) == 0 ? (type)0 : (type)(((int64_t)(numerator) << (frac_bits)) / (denominator)))
+
+int32_t fixed_sqrt(int32_t value, uint8_t frac_bits);
+int32_t fixed_inv_sqrt(int32_t value, uint8_t frac_bits);
+int32_t fixed_atan2(int32_t y, int32_t x, uint8_t frac_bits);
+int32_t fixed_asin(int32_t value, uint8_t frac_bits);

main/mahony_filter.c

@@ -0,0 +1,245 @@
+#include "mahony_filter.h"
+
+#include <stddef.h>
+
+#include "fixedpoint.h"
+
+static void mahony_filter_normalize_quaternion(mahony_filter_t *filter)
+{
+    const uint8_t frac_bits = filter->frac_bits;
+    const int32_t norm_sq =
+        FIXED_MUL(filter->q0, filter->q0, frac_bits) +
+        FIXED_MUL(filter->q1, filter->q1, frac_bits) +
+        FIXED_MUL(filter->q2, filter->q2, frac_bits) +
+        FIXED_MUL(filter->q3, filter->q3, frac_bits);
+    const int32_t inv_norm = fixed_inv_sqrt(norm_sq, frac_bits);
+
+    if (inv_norm == 0)
+    {
+        mahony_filter_reset(filter);
+        return;
+    }
+
+    filter->q0 = FIXED_MUL(filter->q0, inv_norm, frac_bits);
+    filter->q1 = FIXED_MUL(filter->q1, inv_norm, frac_bits);
+    filter->q2 = FIXED_MUL(filter->q2, inv_norm, frac_bits);
+    filter->q3 = FIXED_MUL(filter->q3, inv_norm, frac_bits);
+}
+
+void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits)
+{
+    if (filter == NULL)
+    {
+        return;
+    }
+
+    filter->frac_bits = frac_bits;
+    filter->kp = 0;
+    filter->ki = 0;
+    mahony_filter_reset(filter);
+}
+
+void mahony_filter_reset(mahony_filter_t *filter)
+{
+    if (filter == NULL)
+    {
+        return;
+    }
+
+    filter->integral_x = 0;
+    filter->integral_y = 0;
+    filter->integral_z = 0;
+    filter->q0 = FIXED_ONE(filter->frac_bits);
+    filter->q1 = 0;
+    filter->q2 = 0;
+    filter->q3 = 0;
+}
+
+void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki)
+{
+    if (filter == NULL)
+    {
+        return;
+    }
+
+    filter->kp = kp;
+    filter->ki = ki;
+}
+
+void mahony_filter_update_imu(
+    mahony_filter_t *filter,
+    int32_t gx,
+    int32_t gy,
+    int32_t gz,
+    int32_t ax,
+    int32_t ay,
+    int32_t az,
+    int32_t dt)
+{
+    uint8_t frac_bits;
+    int32_t half;
+    int32_t accel_norm_sq;
+    int32_t q0;
+    int32_t q1;
+    int32_t q2;
+    int32_t q3;
+
+    if (filter == NULL || dt <= 0)
+    {
+        return;
+    }
+
+    frac_bits = filter->frac_bits;
+    half = FIXED_HALF(frac_bits);
+    accel_norm_sq =
+        FIXED_MUL(ax, ax, frac_bits) +
+        FIXED_MUL(ay, ay, frac_bits) +
+        FIXED_MUL(az, az, frac_bits);
+
+    q0 = filter->q0;
+    q1 = filter->q1;
+    q2 = filter->q2;
+    q3 = filter->q3;
+
+    if (accel_norm_sq > 0)
+    {
+        const int32_t accel_inv_norm = fixed_inv_sqrt(accel_norm_sq, frac_bits);
+
+        if (accel_inv_norm != 0)
+        {
+            const int32_t ax_n = FIXED_MUL(ax, accel_inv_norm, frac_bits);
+            const int32_t ay_n = FIXED_MUL(ay, accel_inv_norm, frac_bits);
+            const int32_t az_n = FIXED_MUL(az, accel_inv_norm, frac_bits);
+            const int32_t vx = FIXED_MUL(FIXED_FROM_INT(2, frac_bits), (FIXED_MUL(q1, q3, frac_bits) - FIXED_MUL(q0, q2, frac_bits)), frac_bits);
+            const int32_t vy = FIXED_MUL(FIXED_FROM_INT(2, frac_bits), (FIXED_MUL(q0, q1, frac_bits) + FIXED_MUL(q2, q3, frac_bits)), frac_bits);
+            const int32_t vz =
+                FIXED_MUL(q0, q0, frac_bits) -
+                FIXED_MUL(q1, q1, frac_bits) -
+                FIXED_MUL(q2, q2, frac_bits) +
+                FIXED_MUL(q3, q3, frac_bits);
+            const int32_t ex = FIXED_MUL(ay_n, vz, frac_bits) - FIXED_MUL(az_n, vy, frac_bits);
+            const int32_t ey = FIXED_MUL(az_n, vx, frac_bits) - FIXED_MUL(ax_n, vz, frac_bits);
+            const int32_t ez = FIXED_MUL(ax_n, vy, frac_bits) - FIXED_MUL(ay_n, vx, frac_bits);
+
+            if (filter->ki != 0)
+            {
+                filter->integral_x += FIXED_MUL(FIXED_MUL(filter->ki, ex, frac_bits), dt, frac_bits);
+                filter->integral_y += FIXED_MUL(FIXED_MUL(filter->ki, ey, frac_bits), dt, frac_bits);
+                filter->integral_z += FIXED_MUL(FIXED_MUL(filter->ki, ez, frac_bits), dt, frac_bits);
+            }
+            else
+            {
+                filter->integral_x = 0;
+                filter->integral_y = 0;
+                filter->integral_z = 0;
+            }
+
+            gx += FIXED_MUL(filter->kp, ex, frac_bits) + filter->integral_x;
+            gy += FIXED_MUL(filter->kp, ey, frac_bits) + filter->integral_y;
+            gz += FIXED_MUL(filter->kp, ez, frac_bits) + filter->integral_z;
+        }
+    }
+
+    {
+        const int32_t half_dt = FIXED_MUL(half, dt, frac_bits);
+        const int32_t q_dot0 = -FIXED_MUL(q1, gx, frac_bits) - FIXED_MUL(q2, gy, frac_bits) - FIXED_MUL(q3, gz, frac_bits);
+        const int32_t q_dot1 = FIXED_MUL(q0, gx, frac_bits) + FIXED_MUL(q2, gz, frac_bits) - FIXED_MUL(q3, gy, frac_bits);
+        const int32_t q_dot2 = FIXED_MUL(q0, gy, frac_bits) - FIXED_MUL(q1, gz, frac_bits) + FIXED_MUL(q3, gx, frac_bits);
+        const int32_t q_dot3 = FIXED_MUL(q0, gz, frac_bits) + FIXED_MUL(q1, gy, frac_bits) - FIXED_MUL(q2, gx, frac_bits);
+
+        filter->q0 += FIXED_MUL(q_dot0, half_dt, frac_bits);
+        filter->q1 += FIXED_MUL(q_dot1, half_dt, frac_bits);
+        filter->q2 += FIXED_MUL(q_dot2, half_dt, frac_bits);
+        filter->q3 += FIXED_MUL(q_dot3, half_dt, frac_bits);
+    }
+
+    mahony_filter_normalize_quaternion(filter);
+}
+
+void mahony_filter_get_quaternion(
+    const mahony_filter_t *filter,
+    int32_t *q0,
+    int32_t *q1,
+    int32_t *q2,
+    int32_t *q3)
+{
+    if (filter == NULL)
+    {
+        return;
+    }
+
+    if (q0 != NULL)
+    {
+        *q0 = filter->q0;
+    }
+    if (q1 != NULL)
+    {
+        *q1 = filter->q1;
+    }
+    if (q2 != NULL)
+    {
+        *q2 = filter->q2;
+    }
+    if (q3 != NULL)
+    {
+        *q3 = filter->q3;
+    }
+}
+
+void mahony_filter_get_yaw_pitch_roll(
+    const mahony_filter_t *filter,
+    int32_t *yaw,
+    int32_t *pitch,
+    int32_t *roll)
+{
+    uint8_t frac_bits;
+    int32_t two;
+    int32_t q0q0;
+    int32_t q1q1;
+    int32_t q2q2;
+    int32_t q3q3;
+    int32_t yaw_num;
+    int32_t yaw_den;
+    int32_t pitch_arg;
+    int32_t roll_num;
+    int32_t roll_den;
+
+    if (filter == NULL)
+    {
+        return;
+    }
+
+    frac_bits = filter->frac_bits;
+    two = FIXED_FROM_INT(2, frac_bits);
+    q0q0 = FIXED_MUL(filter->q0, filter->q0, frac_bits);
+    q1q1 = FIXED_MUL(filter->q1, filter->q1, frac_bits);
+    q2q2 = FIXED_MUL(filter->q2, filter->q2, frac_bits);
+    q3q3 = FIXED_MUL(filter->q3, filter->q3, frac_bits);
+    yaw_num = FIXED_MUL(
+        two,
+        FIXED_MUL(filter->q0, filter->q3, frac_bits) + FIXED_MUL(filter->q1, filter->q2, frac_bits),
+        frac_bits);
+    yaw_den = q0q0 + q1q1 - q2q2 - q3q3;
+    pitch_arg = FIXED_MUL(
+        two,
+        FIXED_MUL(filter->q0, filter->q2, frac_bits) - FIXED_MUL(filter->q3, filter->q1, frac_bits),
+        frac_bits);
+    roll_num = FIXED_MUL(
+        two,
+        FIXED_MUL(filter->q0, filter->q1, frac_bits) + FIXED_MUL(filter->q2, filter->q3, frac_bits),
+        frac_bits);
+    roll_den = q0q0 - q1q1 - q2q2 + q3q3;
+
+    if (yaw != NULL)
+    {
+        *yaw = fixed_atan2(yaw_num, yaw_den, frac_bits);
+    }
+    if (pitch != NULL)
+    {
+        *pitch = fixed_asin(pitch_arg, frac_bits);
+    }
+    if (roll != NULL)
+    {
+        *roll = fixed_atan2(roll_num, roll_den, frac_bits);
+    }
+}

main/mahony_filter.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <stdint.h>
+
+typedef struct
+{
+    uint8_t frac_bits;
+    int32_t kp;
+    int32_t ki;
+    int32_t integral_x;
+    int32_t integral_y;
+    int32_t integral_z;
+    int32_t q0;
+    int32_t q1;
+    int32_t q2;
+    int32_t q3;
+} mahony_filter_t;
+
+void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits);
+void mahony_filter_reset(mahony_filter_t *filter);
+void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki);
+void mahony_filter_update_imu(
+    mahony_filter_t *filter,
+    int32_t gx,
+    int32_t gy,
+    int32_t gz,
+    int32_t ax,
+    int32_t ay,
+    int32_t az,
+    int32_t dt);
+void mahony_filter_get_quaternion(
+    const mahony_filter_t *filter,
+    int32_t *q0,
+    int32_t *q1,
+    int32_t *q2,
+    int32_t *q3);
+void mahony_filter_get_yaw_pitch_roll(
+    const mahony_filter_t *filter,
+    int32_t *yaw,
+    int32_t *pitch,
+    int32_t *roll);