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

Source/main.c

@@ -1,6 +1,7 @@
 #include "stm32g431xx.h"
 #include "imu.h"
 #include "imu_processing.h"
+#include "IRS.h"
 #include "attitude.h"
 #include "radio_receiver.h"
 #include "motors.h"
@@ -9,6 +10,7 @@
 
 
 imu_scaled_t imu;
+IRS irs;
 attitude_t attitude;
 rc_channels rx_chs_raw;
 rc_channels rx_chs_normalized;
@@ -34,6 +36,8 @@ int main(void)
 	
 	imu_calibrate();
 	
+	IRS_init(&irs);
+	
 	attitude_init(&attitude);
 	
 	receiver_init();
@@ -41,22 +45,47 @@ int main(void)
 	motors_init();
 	
 	while (1)
-	{
-		receiver_update(&rx_chs_raw);
-		rx_chs_normalized = normalize_channels(rx_chs_raw);
-		
-		attitude_update(&attitude, &imu);
-		attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude);
-		
-		if (rx_chs_normalized.rc_armed)
-		{
-			motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);
-		}
-		else
-		{
-			motors_turn_off();
-		}
-	}
+{
+    receiver_update(&rx_chs_raw);
+    rx_chs_normalized = normalize_channels(rx_chs_raw);
+
+    if (imu_update_flag)
+    {
+        imu_update_flag = 0;
+
+        imu_read_scaled(&imu);
+
+        Vector3 gyro = {imu.gx, imu.gy, imu.gz};
+        Vector3 accel = {imu.ax, imu.ay, imu.az};
+
+        IRS_update(&irs, &gyro, &accel, IMU_DT);
+    }
+
+    if (pid_update_flag)
+    {
+        pid_update_flag = 0;
+
+        attitude_controller_update(
+            &ctrl_chs,
+            &rx_chs_normalized,
+            &irs.q,
+            &irs.gyro
+        );
+
+        if (rx_chs_normalized.rc_armed)
+        {
+            motors_set_throttle_mix(
+                rx_chs_normalized.rc_throttle,
+                &ctrl_chs,
+                rx_chs_normalized.rc_armed
+            );
+        }
+        else
+        {
+            motors_turn_off();
+        }
+    }
+}
 }
 
 void delay_ms(uint32_t ms)