temp mahony filter

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));
     }
 }