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