Реализовано асинхронное чтение IMU

Source/main.c

@@ -10,24 +10,42 @@
 
 void TIM6_DAC_IRQHandler();
 
-static uint8_t irs_update_flag = 0;
+//static uint8_t irs_update_flag = 0;
 static uint8_t control_update_flag = 0;
 static uint8_t allowed_calib = 0;
 
+imu_raw_t imu_raw;
 imu_scaled_t imu;
 IRS irs;
 attitude_t attitude;
 rc_channels rx_chs_raw;
 rc_channels rx_chs_normalized;
 control_channels_t ctrl_chs;
-lidar_data lidar;
+//lidar_data lidar;
 
 Vector3 euler;
 
+volatile uint32_t us_ticks = 0;
+
+void SysTick_Handler()
+{
+	us_ticks++;
+}
+
+float micros()
+{
+	return (float)us_ticks;
+}
+
+static float last_time = 0;
+
+void imu_callback(uint8_t* buf);
 void delay_ms(uint32_t ms);
 
 int main(void)
 {
+  last_time = micros();
+  
 	__enable_irq();
 	
 	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
@@ -40,9 +58,9 @@ int main(void)
 	imu_init();
 	
 	imu_processing_init();
-	//imu_calibrate();
+	//imu_accel_calibrate();
 	
-	imu_tim6_init();
+	imu_tim6_init(500);
 	
 	IRS_init(&irs);
 	
@@ -54,10 +72,10 @@ int main(void)
 	
 	while (1)
 	{
-	    receiver_update(&rx_chs_raw);
-	    rx_chs_normalized = normalize_channels(rx_chs_raw);
+	  receiver_update(&rx_chs_raw);
+	  rx_chs_normalized = normalize_channels(rx_chs_raw);
 		
-		if (irs_update_flag)
+		/*if (irs_update_flag)
 		{
 		  irs_update_flag = 0;
 			
@@ -72,9 +90,9 @@ int main(void)
 			irs.accel.z = imu.az;
 		
 		  IRS_update(&irs, IMU_DT);
-		}
+		}*/
+		
 		
-		euler = QuatToEuler(&irs.q);
 		
 		if (control_update_flag)
 		{
@@ -90,7 +108,7 @@ int main(void)
 		
 		if (rx_chs_normalized.rc_armed)
 	  {
-      allowed_calib = 1;
+      allowed_calib = 0;
       
       motors_set_throttle_mix(
         rx_chs_normalized.rc_throttle,
@@ -101,17 +119,47 @@ int main(void)
 	  else
 		{
 			motors_turn_off();
-			allowed_calib = 0;
+			allowed_calib = 1;
 		}
 	}
 }
 
+void imu_callback(uint8_t* buf)
+{
+	imu_raw.ax = (buf[0] << 8) | buf[1];
+	imu_raw.ay = (buf[2] << 8) | buf[3];
+	imu_raw.az = (buf[4] << 8) | buf[5];
+
+	imu_raw.gx = (buf[6] << 8)  | buf[7];
+	imu_raw.gy = (buf[8] << 8)  | buf[9];
+	imu_raw.gz = (buf[10] << 8) | buf[11];
+
+	imu_process_raw(&imu, &imu_raw, &allowed_calib);
+
+	float now = micros();
+	float dt = (now - last_time) * 1e-6f;
+  if (dt <= 0.0f || dt > 0.1f) dt = IMU_DT;
+	last_time = now;
+
+	irs.gyro.x = imu.gx;
+	irs.gyro.y = imu.gy;
+	irs.gyro.z = imu.gz;
+
+	irs.accel.x = imu.ax;
+	irs.accel.y = imu.ay;
+	irs.accel.z = imu.az;
+
+	IRS_update(&irs, IMU_DT);
+  
+  euler = QuatToEuler(&irs.q);
+}
+
 void TIM6_DAC_IRQHandler()
 {
 	if (TIM6->SR & TIM_SR_UIF)
 	{
 		TIM6->SR &= ~TIM_SR_UIF;
-		irs_update_flag = 1;
+    i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
 		control_update_flag = 1;
 	}
 }