Некоторая реализация логики перенесена в 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

@@ -1,8 +1,11 @@
 #include "irs.h"
 #include <math.h>
 
-#define MAHONY_KP 2.5f
+#define MAHONY_KP 		2.5f
-#define MAHONY_KI 0.01f
+#define MAHONY_KI 		0.01f
+
+#define ACC_ALPHA		0.01f
+#define ACC_MAX_ERROR	0.2f
 
 static Vector3 gyro_bias = {0};
 
@@ -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 = 
-
+	{
-    Vector3 g;
+		2 * (q->x*q->z - q->w*q->y),
-    g.x = 2 * (q->x*q->z - q->w*q->y);
+		2 * (q->w*q->x + q->y*q->z),
-    g.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
-    g.z = q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z;
+	};
 
     return g;
 }
@@ -29,54 +32,72 @@ 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};
 
-    // --- нормализация акселя
+    // gyro intergate
-    float norm = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
+	Quaternion q = irs->q;
 	
-    if (norm > 1e-6f)
+	Quaternion omega = 
-    {
+	{
-        accel.x /= norm;
+		gyro.x * DEG2RAD,
-        accel.y /= norm;
+		gyro.y * DEG2RAD,
-        accel.z /= norm;
+		gyro.z * DEG2RAD,
+		0
+	};
 	
-        Vector3 g = IRS_getGravity(irs);
+	Quaternion dq = QuatProd(&q, &omega);
+	dq = QuatConstProd(&dq, 0.5f * dt);
 	
-        Vector3 error =
+	q = QuatSum(&q, &dq);
-        {
+	q = QuatNormalize(&q, 1.0f);
-            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;
+	// accel correction
-        gyro_bias.y += MAHONY_KI * error.y * dt;
+	float acc_len = sqrtf(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
-        gyro_bias.z += MAHONY_KI * error.z * dt;
 	
-        gyro.x += MAHONY_KP * error.x + gyro_bias.x;
+	if (acc_len > 1e-6f)
-        gyro.y += MAHONY_KP * error.y + gyro_bias.y;
+	{
-        gyro.z += MAHONY_KP * error.z + gyro_bias.z;
+		Vector3 acc_norm =
-    }
+		{
+			accel.x / acc_len,
+			accel.y / acc_len,
+			accel.z / acc_len
+		};
 		
-    irs->gyro = gyro;
+		float dyn = fabsf(acc_len - 1.0f);
-    irs->accel = accel;
 		
-    // --- интеграция кватерниона
+		float trust;
-    Quaternion q = irs->q;
+		if (dyn > ACC_MAX_ERROR)
+			trust = 0.0f;
+		else
+			trust = 1.0f - (dyn / ACC_MAX_ERROR);
 		
-    Quaternion omega =
+		float gain = ACC_ALPHA * trust;
-    {
-        gyro.x * DEG2RAD,
-        gyro.y * DEG2RAD,
-        gyro.z * DEG2RAD,
-        0
-    };
 		
-    Quaternion dq = QuatProd(&q, &omega);
+		if (gain > 1e-5f)
-    dq = QuatConstProd(&dq, 0.5f * dt);
+		{
+			Vector3 est = IRS_getGravity(&q);
 			
-    q = QuatSum(&q, &dq);
+			Vector3 error = 
-    q = QuatNormalize(&q, 1.0f);
+			{
+				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
+			};
 			
-    irs->q = q;
+			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,47 +52,59 @@ int main(void)
 	motors_init();
 	
 	while (1)
-{
+	{
-    receiver_update(&rx_chs_raw);
+	    receiver_update(&rx_chs_raw);
-    rx_chs_normalized = normalize_channels(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);
+			imu_read_scaled(&imu);
 		
-        Vector3 gyro = {imu.gx, imu.gy, imu.gz};
+		    Vector3 gyro = {imu.gx, imu.gy, imu.gz};
-        Vector3 accel = {imu.ax, imu.ay, imu.az};
+		    Vector3 accel = {imu.ax, imu.ay, imu.az};
 		
-        IRS_update(&irs, &gyro, &accel, IMU_DT);
+		    IRS_update(&irs, &gyro, &accel, IMU_DT);
-    }
+		}
 		
-    if (pid_update_flag)
+		euler = QuatToEuler(&irs.q);
-    {
-        pid_update_flag = 0;
 		
-        attitude_controller_update(
+		if (control_update_flag)
-            &ctrl_chs,
+		{
-            &rx_chs_normalized,
+		    control_update_flag = 0;
-            &irs.q,
-            &irs.gyro
-        );
 		
-        if (rx_chs_normalized.rc_armed)
+		    attitude_controller_update(
-        {
+		        &ctrl_chs,
-            motors_set_throttle_mix(
+		        &rx_chs_normalized,
-                rx_chs_normalized.rc_throttle,
+		        &irs.q,
-                &ctrl_chs,
+		        &irs.gyro
-                rx_chs_normalized.rc_armed
+		    );
-            );
+		}
-        }
+		
-        else
+		if (rx_chs_normalized.rc_armed)
-        {
+	    {
-            motors_turn_off();
+	        motors_set_throttle_mix(
-        }
+	            rx_chs_normalized.rc_throttle,
-    }
+	            &ctrl_chs,
+	            rx_chs_normalized.rc_armed
+	        );
+	    }
+	    else
+		{
+			motors_turn_off();
+		}
+	}
 }
+
+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)