Обновлена архитектура обработки 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;
+}

Source/INS/IRS.h

@@ -1,40 +1,24 @@
-#pragma once
-
 #ifndef IRS_H
 #define IRS_H
 
 #include "quaternion.h"
 #include "vector.h"
 
+#define PI			3.14159265359f
+#define DEG2RAD		PI / 180.0f
+
 typedef struct
 {
-	Quaternion orientationQ;			// главный кватерион ориентации
-	// Vector3 tilts;					// локально ориентированные наклоны sinX sinY cosZ
-	
-	Vector3 gyro, accel;				// последние значения датчиков
-	
-	float roll, pitch, yaw;
+    Quaternion q;   // ориентация
+    Vector3 gyro;   // deg/s
+    Vector3 accel;  // g
 
-	// Vector3 accel, spd, pos;			// инерциальные значения движения					
 } IRS;
 
-void updateIRS(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
-void IRSgetEuler(const IRS* irs);
+void IRS_init(IRS* irs);
 
-void updateGyro(Vector3* gyro, const Vector3* newGyro);
-void updateAccel(Vector3* accel, const Vector3* newAccel);
+void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
 
-// void setShiftAlpha(const Vector3* pos, const Vector3* spd, const float* qua);	// коэффициент восстановления позиции и скорости в секунду
-
-// void updatePosSpeed();
-void updateQuat();
-
-// void restoreAllShift();
-
-// void setAccelShift();
-
-// void getSinXYCosZ();
-// void getRollPitchYaw();
-// void getInertial();
+Vector3 IRS_getGravity(const IRS* irs);
 
 #endif