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

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);
-    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();
-        }
-    }
+	{
+	    receiver_update(&rx_chs_raw);
+	    rx_chs_normalized = normalize_channels(rx_chs_raw);
+		
+		if (irs_update_flag)
+		{
+		    irs_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);
+		}
+		
+		euler = QuatToEuler(&irs.q);
+		
+		if (control_update_flag)
+		{
+		    control_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 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)