temp mahony filter

2026-03-20 17:17:33 +01:00
parent 1b6bd537ad
commit f6de696148
9 changed files with 662 additions and 34 deletions
--- a/main/CMakeLists.txt
+++ b/main/CMakeLists.txt
@@ -2,6 +2,8 @@ idf_component_register(
    SRCS
        "bbot.c"
        "bmi160.c"
+        "fixedpoint.c"
+        "mahony_filter.c"
        "motors.c"
    INCLUDE_DIRS 
        "."
--- a/main/bbot.c
+++ b/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));
    }
 }
--- a/main/bmi160.c
+++ b/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));
--- a/main/fixedpoint.c
+++ b/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);
+}
--- a/main/fixedpoint.h
+++ b/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);
--- a/main/mahony_filter.c
+++ b/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);
+    }
+}
--- a/main/mahony_filter.h
+++ b/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);
--- a/plot.py
+++ b/plot.py
@@ -238,6 +238,32 @@ def draw_grid(
    surface.blit(font.render(title, True, TEXT), (rect.left + 8, rect.top + 8))


+def draw_graph_info(
+    surface: pygame.Surface,
+    rect: pygame.Rect,
+    font: pygame.font.Font,
+    graph_streams: list[dict],
+    latest_values: dict[str, float | None],
+) -> None:
+    line_height = font.get_height() + 6
+    y = rect.top + 8
+    for stream in graph_streams:
+        color = color_tuple(stream["color"])
+        label = stream.get("label", stream["key"])
+        latest_value = latest_values.get(stream["key"])
+        value_text = "--" if latest_value is None else f"{latest_value:.3f}"
+        pygame.draw.line(
+            surface,
+            color,
+            (rect.left, y + 8),
+            (rect.left + 24, y + 8),
+            3,
+        )
+        text = font.render(f"{label}: {value_text}", True, TEXT)
+        surface.blit(text, (rect.left + 32, y))
+        y += line_height
+
+
 def draw_trace(
    surface: pygame.Surface,
    rect: pygame.Rect,
@@ -348,16 +374,20 @@ def main() -> int:
        width, height = screen.get_size()
        now = time.monotonic()
        graph_count = len(graphs)
-        plot_width = max(100, width - 100)
+        left_margin = 70
+        right_margin = 20
+        info_width = min(280, max(180, width // 4))
+        plot_width = max(100, width - left_margin - right_margin - info_width - 16)
        available_height = max(100, height - 120 - (graph_count - 1) * 30)
        panel_height = max(100, available_height // graph_count)

        screen.fill(BACKGROUND)

-        latest_values: dict[str, int | None] = {}
+        latest_values: dict[str, float | None] = {}
        top = 40
        for graph in graphs:
-            rect = pygame.Rect(70, top, plot_width, panel_height)
+            rect = pygame.Rect(left_margin, top, plot_width, panel_height)
+            info_rect = pygame.Rect(rect.right + 16, top, info_width, panel_height)
            graph_streams = graph["streams"]
            stream_names = [stream["key"] for stream in graph_streams]
            y_min, y_max = visible_range(sample_sets, stream_names, now, view_span)
@@ -381,39 +411,16 @@ def main() -> int:
                    y_max,
                    color_tuple(stream["color"]),
                )
+            draw_graph_info(screen, info_rect, small_font, graph_streams, latest_values)
            top = rect.bottom + 30

        header = f"port={args.port} baud={args.baudrate} zoom={view_span:.1f}s"
-        for graph in graphs:
-            graph_parts = []
-            for stream in graph["streams"]:
-                latest_value = latest_values.get(stream["key"])
-                if latest_value is not None:
-                    graph_parts.append(f"{stream.get('label', stream['key'])}={latest_value:.3f}")
-            if graph_parts:
-                header += " | " + " ".join(graph_parts)
        screen.blit(font.render(header, True, TEXT), (10, 8))
        screen.blit(
            small_font.render("mouse wheel: zoom horizontal axis", True, AXIS),
            (10, height - 24),
        )

-        legend_x = 10
-        legend_y = 34
-        for graph in graphs:
-            for stream in graph["streams"]:
-                color = color_tuple(stream["color"])
-                label = stream.get("label", stream["key"])
-                pygame.draw.line(
-                    screen,
-                    color,
-                    (legend_x, legend_y + 9),
-                    (legend_x + 24, legend_y + 9),
-                    3,
-                )
-                screen.blit(small_font.render(label, True, TEXT), (legend_x + 30, legend_y))
-                legend_x += max(90, 42 + len(label) * 10)
-
        if error_message:
            error_surface = font.render(error_message, True, ERROR)
            screen.blit(error_surface, (10, height - 52))
--- a/plot_config.json
+++ b/plot_config.json
@@ -4,7 +4,22 @@
  "sources": [
    {
      "tag": "IMU",
-      "fields": ["time", "acc_x", "acc_y", "acc_z", "gyr_x", "gyr_y", "gyr_z"]
+      "fields": [
+        "time",
+        "acc_x",
+        "acc_y",
+        "acc_z",
+        "gyr_x",
+        "gyr_y",
+        "gyr_z",
+        "q0",
+        "q1",
+        "q2",
+        "q3",
+        "yaw",
+        "pitch",
+        "roll"
+      ]
    }
  ],
  "graphs": [
@@ -23,6 +38,23 @@
        { "key": "gyr_y", "label": "GYR Y", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 },
        { "key": "gyr_z", "label": "GYR Z", "color": [225, 200, 60], "interpreter": "fixed", "frac_bits": 12 }
      ]
+    },
+    {
+      "title": "Quaternion",
+      "streams": [
+        { "key": "q0", "label": "Q0", "color": [230, 90, 90], "interpreter": "fixed", "frac_bits": 12 },
+        { "key": "q1", "label": "Q1", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 },
+        { "key": "q2", "label": "Q2", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 },
+        { "key": "q3", "label": "Q3", "color": [255, 200, 60], "interpreter": "fixed", "frac_bits": 12 }
+      ]
+    },
+    {
+      "title": "Yaw Pitch Roll",
+      "streams": [
+        { "key": "yaw", "label": "Yaw", "color": [230, 90, 90], "interpreter": "fixed", "frac_bits": 12 },
+        { "key": "pitch", "label": "Pitch", "color": [90, 170, 255], "interpreter": "fixed", "frac_bits": 12 },
+        { "key": "roll", "label": "Roll", "color": [80, 220, 140], "interpreter": "fixed", "frac_bits": 12 }
+      ]
    }
  ]
 }