Обновлена архитектура обработки IMU

Source/INS/IRS.c

@@ -1 +1,82 @@
-#include "IRS.h"
+#include "irs.h"
+#include <math.h>
+
+#define MAHONY_KP 2.5f
+#define MAHONY_KI 0.01f
+
+static Vector3 gyro_bias = {0};
+
+void IRS_init(IRS* irs)
+{
+    irs->q.w = 1.0f;
+    irs->q.x = 0.0f;
+    irs->q.y = 0.0f;
+    irs->q.z = 0.0f;
+}
+
+Vector3 IRS_getGravity(const IRS* irs)
+{
+    const Quaternion* q = &irs->q;
+
+    Vector3 g;
+    g.x = 2 * (q->x*q->z - q->w*q->y);
+    g.y = 2 * (q->w*q->x + q->y*q->z);
+    g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z;
+
+    return g;
+}
+
+void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
+{
+    Vector3 gyro = *gyro_in;
+    Vector3 accel = *accel_in;
+
+    // --- нормализация акселя
+    float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
+
+    if (norm > 1e-6f)
+    {
+        accel.x /= norm;
+        accel.y /= norm;
+        accel.z /= norm;
+
+        Vector3 g = IRS_getGravity(irs);
+
+        Vector3 error =
+        {
+            accel.y*g.z - accel.z*g.y,
+            accel.z*g.x - accel.x*g.z,
+            accel.x*g.y - accel.y*g.x
+        };
+
+        gyro_bias.x += MAHONY_KI * error.x * dt;
+        gyro_bias.y += MAHONY_KI * error.y * dt;
+        gyro_bias.z += MAHONY_KI * error.z * dt;
+
+        gyro.x += MAHONY_KP * error.x + gyro_bias.x;
+        gyro.y += MAHONY_KP * error.y + gyro_bias.y;
+        gyro.z += MAHONY_KP * error.z + gyro_bias.z;
+    }
+
+    irs->gyro = gyro;
+    irs->accel = accel;
+
+    // --- интеграция кватерниона
+    Quaternion q = irs->q;
+
+    Quaternion omega =
+    {
+        gyro.x * DEG2RAD,
+        gyro.y * DEG2RAD,
+        gyro.z * DEG2RAD,
+        0
+    };
+
+    Quaternion dq = QuatProd(&q, &omega);
+    dq = QuatConstProd(&dq, 0.5f * dt);
+
+    q = QuatSum(&q, &dq);
+    q = QuatNormalize(&q, 1.0f);
+
+    irs->q = q;
+}