Некоторая реализация логики перенесена в main

Source/Control/Inc/attitude.h

@@ -5,15 +5,14 @@
 #include "vector.h"
 #include "pid.h"
 #include "radio_receiver.h"
+#include "imu_processing.h"
+#include "IRS.h"
 
 typedef struct
 {
     Vector3 gyro;
 } attitude_t;
 
-static uint8_t imu_update_flag = 0;
-static uint8_t pid_update_flag = 0;
-
 void attitude_init(attitude_t* att);
 
 void attitude_controller_update(control_channels_t* control,
@@ -23,6 +22,5 @@ void attitude_controller_update(control_channels_t* control,
 
 Quaternion rx_to_quaternion(const rc_channels* rx);
 float constrain(float x, float min, float max);
-void TIM6_DAC_IRQHandler();
 
 #endif

Source/Control/Src/attitude.c

@@ -67,13 +67,3 @@ float constrain(float x, float min, float max)
 	if (x < min) x = min; else if (x > max) x = max;
 	return x;
 }
-
-void TIM6_DAC_IRQHandler()
-{
-	if (TIM6->SR & TIM_SR_UIF)
-	{
-		TIM6->SR &= ~TIM_SR_UIF;
-		imu_update_flag = 1;
-		pid_update_flag = 1;
-	}
-}

Source/INS/IRS.c

@@ -4,6 +4,9 @@
 #define MAHONY_KP 		2.5f
 #define MAHONY_KI 		0.01f
 
+#define ACC_ALPHA		0.01f
+#define ACC_MAX_ERROR	0.2f
+
 static Vector3 gyro_bias = {0};
 
 void IRS_init(IRS* irs)
@@ -14,14 +17,14 @@ void IRS_init(IRS* irs)
     irs->q.z = 0.0f;
 }
 
-Vector3 IRS_getGravity(const IRS* irs)
+Vector3 IRS_getGravity(const Quaternion* q)
 {
-    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;
+    Vector3 g = 
+	{
+		2 * (q->x*q->z - q->w*q->y),
+		2 * (q->w*q->x + q->y*q->z),
+		q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
+	};
 
     return g;
 }
@@ -29,39 +32,9 @@ Vector3 IRS_getGravity(const IRS* irs)
 void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float dt)
 {
     Vector3 gyro = *gyro_in;
-    Vector3 accel = *accel_in;
+	Vector3 accel = {accel_in->x, accel_in->y, -accel_in->z};
 
-    // --- нормализация акселя
-    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;
-
-    // --- интеграция кватерниона
+    // gyro intergate
 	Quaternion q = irs->q;
 	
 	Quaternion omega = 
@@ -78,5 +51,53 @@ void IRS_update(IRS* irs, const Vector3* gyro_in, const Vector3* accel_in, float
 	q = QuatSum(&q, &dq);
 	q = QuatNormalize(&q, 1.0f);
 	
+	// accel correction
+	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
+	
+	if (acc_len > 1e-6f)
+	{
+		Vector3 acc_norm =
+		{
+			accel.x / acc_len,
+			accel.y / acc_len,
+			accel.z / acc_len
+		};
+		
+		float dyn = fabsf(acc_len - 1.0f);
+		
+		float trust;
+		if (dyn > ACC_MAX_ERROR)
+			trust = 0.0f;
+		else
+			trust = 1.0f - (dyn / ACC_MAX_ERROR);
+		
+		float gain = ACC_ALPHA * trust;
+		
+		if (gain > 1e-5f)
+		{
+			Vector3 est = IRS_getGravity(&q);
+			
+			Vector3 error = 
+			{
+				acc_norm.y*est.z - acc_norm.z*est.y,
+				acc_norm.z*est.x - acc_norm.x*est.z,
+				acc_norm.x*est.y - acc_norm.y*est.x
+			};
+			
+			Quaternion corretrion = 
+			{
+				error.x * gain * 0.5f,
+				error.y * gain * 0.5f,
+				error.z * gain * 0.5f,
+				1.0f
+			};
+			
+			q = QuatProd(&corretrion, &q);
+			q = QuatNormalize(&q, 1.0f);
+		}
+	}
+	
 	irs->q = q;
+	irs->gyro = gyro;
+	irs->accel = accel;
 }

Source/INS/IRS.h

@@ -19,6 +19,6 @@ void IRS_init(IRS* irs);
 
 void IRS_update(IRS* irs, const Vector3* gyro, const Vector3* accel, float dt);
 
-Vector3 IRS_getGravity(const IRS* irs);
+Vector3 IRS_getGravity(const Quaternion* q);
 
 #endif

Source/INS/geometry/quaternion.c

@@ -1,6 +1,8 @@
 #include "quaternion.h"
 #include <math.h>
 
+#define PI		3.14159265359f
+
 Quaternion QuatNormalize(const Quaternion* q, const float gain)
 {
   Quaternion res = {};
@@ -134,7 +136,24 @@ Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, boo
 	return error;
 }
 
-
+Vector3 QuatToEuler(const Quaternion* q)
+{
+	Vector3 e;
+
+    e.x = atan2f(2*(q->w*q->x + q->y*q->z),
+                 1 - 2*(q->x*q->x + q->y*q->y));
+
+    e.y = asinf(2*(q->w*q->y - q->z*q->x));
+
+    e.z = atan2f(2*(q->w*q->z + q->x*q->y),
+                 1 - 2*(q->y*q->y + q->z*q->z));
+
+    e.x *= 180.0f / PI;
+    e.y *= 180.0f / PI;
+    e.z *= 180.0f / PI;
+
+    return e;
+}
 
 
 

Source/INS/geometry/quaternion.h

@@ -25,6 +25,7 @@ Vector3 QuatRotateAroundZ(const Quaternion* q, const Vector3* vec, bool CCW);
 Quaternion QuatCreateFromEuler(const Vector3* eulerAngels);
 Quaternion QuatGetError(const Quaternion* current, const Quaternion* target, bool fastWay);
 
+Vector3 QuatToEuler(const Quaternion* q);
 
 #endif
 

Source/main.c

@@ -8,6 +8,10 @@
 #include "pid.h"
 #include "lidar.h"
 
+void TIM6_DAC_IRQHandler();
+
+static uint8_t irs_update_flag = 0;
+static uint8_t control_update_flag = 0;
 
 imu_scaled_t imu;
 IRS irs;
@@ -17,6 +21,8 @@ rc_channels rx_chs_normalized;
 control_channels_t ctrl_chs;
 lidar_data lidar;
 
+Vector3 euler;
+
 void delay_ms(uint32_t ms);
 
 int main(void)
@@ -31,11 +37,12 @@ int main(void)
 	i2c_gpio_init();
 	i2c1_init();
 	imu_init();
-	imu_tim6_init();
-	imu_processing_init();
 	
+	imu_processing_init();
 	imu_calibrate();
 	
+	imu_tim6_init();
+	
 	IRS_init(&irs);
 	
 	attitude_init(&attitude);
@@ -45,13 +52,13 @@ int main(void)
 	motors_init();
 	
 	while (1)
-{
+	{
 	    receiver_update(&rx_chs_raw);
 	    rx_chs_normalized = normalize_channels(rx_chs_raw);
 		
-    if (imu_update_flag)
+		if (irs_update_flag)
 		{
-        imu_update_flag = 0;
+		    irs_update_flag = 0;
 			
 			imu_read_scaled(&imu);
 		
@@ -61,9 +68,11 @@ int main(void)
 		    IRS_update(&irs, &gyro, &accel, IMU_DT);
 		}
 		
-    if (pid_update_flag)
+		euler = QuatToEuler(&irs.q);
+		
+		if (control_update_flag)
 		{
-        pid_update_flag = 0;
+		    control_update_flag = 0;
 		
 		    attitude_controller_update(
 		        &ctrl_chs,
@@ -71,6 +80,7 @@ int main(void)
 		        &irs.q,
 		        &irs.gyro
 		    );
+		}
 		
 		if (rx_chs_normalized.rc_armed)
 	    {
@@ -86,6 +96,15 @@ int main(void)
 		}
 	}
 }
+
+void TIM6_DAC_IRQHandler()
+{
+	if (TIM6->SR & TIM_SR_UIF)
+	{
+		TIM6->SR &= ~TIM_SR_UIF;
+		irs_update_flag = 1;
+		control_update_flag = 1;
+	}
 }
 
 void delay_ms(uint32_t ms)