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

Source/BSP/Inc/imu.h

@@ -10,7 +10,8 @@
 #define REG_BANK_SEL			  0x7F
 #define REG_GYRO_CONFIG_1		0x01
 #define REG_ACCEL_CONFIG		0x14
-#define IMU_DT					    0.002f   // 2 ms
+#define FREQUENCY           100.0f
+#define IMU_DT					    1.0f / FREQUENCY
 
 #define GYRO_FS_SEL_2000		3 << 1
 #define GYRO_DLPFCFG_73			3 << 3
@@ -23,12 +24,20 @@
 #define ACCEL_FCHOICE_ON		1
 #define ACCEL_FCHOICE_OFF		0
 
+static volatile uint8_t i2c_busy = 0;
+static uint8_t i2c_buf[16];
+static uint8_t i2c_len = 0;
+static uint8_t i2c_reg = 0;
+static uint8_t i2c_addr = 0;
+
 typedef struct
 {
 	int16_t ax, ay, az; // lsb
 	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
+static void (*i2c_callback)(uint8_t* buf) = 0;
+
 void imu_pow_init();
 
 void i2c_gpio_init();
@@ -37,10 +46,12 @@ void i2c1_init();
 
 void imu_init();
 
-void imu_tim6_init();
+void imu_tim6_init(const uint16_t freq);
 
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
 
+uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
+
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);
 
 void imu_read_raw(imu_raw_t* data);

Source/BSP/Inc/imu_processing.h

@@ -17,11 +17,12 @@ typedef struct
 
 void imu_processing_init();
 
+void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib);
 void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib);
 void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax);
 uint16_t Rev16(uint16_t v);
 
-void imu_calibrate();
+void imu_accel_calibrate();
 
 float normalize(float value, float scale, float min, float max);
 long absl(long value);

Source/BSP/Src/imu.c

@@ -36,15 +36,12 @@ void i2c_gpio_init()
 
 void i2c1_init()
 {
-	// enable I2C1
-	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;
 	
-  // 0x00303D5B - 100 kHz
+	RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN; // enable I2C1
-  
-	// 400 kHz @ 16 MHz
-	I2C1->TIMINGR = 0x10802D9BUL;
 	
-	I2C1->CR1 |= I2C_CR1_PE;
+	I2C1->TIMINGR = 0x10802D9BUL;         // 400 kHz @ 16 MHz
+	
+  I2C1->CR1 |= I2C_CR1_PE;
 }
 
 void imu_init()
@@ -68,13 +65,14 @@ void imu_init()
 	i2c_write(ICM_ADDR, REG_BANK_SEL, ~(3 << 4));
 }
 
-void imu_tim6_init()
+void imu_tim6_init(const uint16_t freq)
 {
 	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
 	
-	TIM6->PSC = 16000 - 1;        // 16 MHz / 16000 = 1000 Hz (1 ms)
+  TIM6->CR1 = 0;
-	TIM6->ARR = 2 - 1;            // 2 ms
+  TIM6->ARR = 1000 - 1;
-	
+	TIM6->PSC = (SystemCoreClock / 1000 / freq) - 1;
+		
 	TIM6->DIER |= TIM_DIER_UIE;   // interrupt enable
 	TIM6->CR1 |= TIM_CR1_CEN;     // counter enable
 	
@@ -105,6 +103,27 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
 	I2C1->ICR |= I2C_ICR_STOPCF;
 }
 
+uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
+{
+	if (i2c_busy) return 0;
+
+	i2c_busy = 1;
+
+	i2c_addr = addr;
+	i2c_reg = reg;
+	i2c_len = len;
+	i2c_callback = cb;
+  
+  // включаем прерывания
+	I2C1->CR1 |= I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE;
+  NVIC_EnableIRQ(I2C1_EV_IRQn);
+
+	// старт записи регистра
+	I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
+
+	return 1;
+}
+
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
 {
 	i2c_wait_idle(I2C1);
@@ -122,6 +141,51 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
 	I2C1->CR2 |= I2C_CR2_STOP;
 }
 
+void I2C1_EV_IRQHandler()
+{
+	static uint8_t index = 0;
+	static uint8_t stage = 0; // 0 = send reg, 1 = read
+
+	// TX — отправляем регистр
+	if (I2C1->ISR & I2C_ISR_TXIS)
+	{
+		I2C1->TXDR = i2c_reg;
+	}
+
+	// переход к чтению
+	if (I2C1->ISR & I2C_ISR_TC)
+	{
+		I2C1->CR2 = (i2c_addr << 1) |
+		            I2C_CR2_RD_WRN |
+		            (i2c_len << I2C_CR2_NBYTES_Pos) |
+		            I2C_CR2_AUTOEND |
+		            I2C_CR2_START;
+
+		index = 0;
+		stage = 1;
+	}
+
+	// чтение данных
+	if (I2C1->ISR & I2C_ISR_RXNE)
+	{
+		i2c_buf[index++] = I2C1->RXDR;
+	}
+
+	// завершение
+	if (I2C1->ISR & I2C_ISR_STOPF)
+	{
+		I2C1->ICR |= I2C_ICR_STOPCF;
+
+		i2c_busy = 0;
+    
+    // отключаем IRQ
+    I2C1->CR1 &= ~(I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE);
+
+		if (i2c_callback)
+			i2c_callback(i2c_buf);
+	}
+}
+
 void imu_read_raw(imu_raw_t* data)
 {
 	uint8_t buf[12];
@@ -139,7 +203,17 @@ void imu_read_raw(imu_raw_t* data)
 
 static void i2c_wait_idle(I2C_TypeDef* i2c)
 {
-	while (i2c->ISR & I2C_ISR_BUSY);
+  int timeout = 100000;
+  while ((i2c->ISR & I2C_ISR_BUSY) && --timeout);
+
+  if (timeout == 0)
+  {
+    // сброс I2C
+    i2c->CR1 &= ~I2C_CR1_PE;
+    i2c->CR1 |= I2C_CR1_PE;
+  }
+  
+	// while (i2c->ISR & I2C_ISR_BUSY);
 	
 	i2c->ICR = I2C_ICR_STOPCF |
            I2C_ICR_NACKCF |

Source/BSP/Src/imu_processing.c

@@ -2,15 +2,8 @@
 #include "biquad_filter.h"
 #include "math.h"
 
-
-
-/*static float gyro_bias_x = 0.0f;
-static float gyro_bias_y = 0.0f;
-static float gyro_bias_z = 0.0f;
-
 static float accel_bias_x = 0.0f;
 static float accel_bias_y = 0.0f;
-static float accel_bias_z = 0.0f;*/
 
 static biquad_t accel_x_lpf;
 static biquad_t accel_y_lpf;
@@ -45,6 +38,9 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
 	static imu_raw_t raw;
 	
 	imu_read_raw(&raw);
+  
+  raw.ax -= accel_bias_x;
+  raw.ay -= accel_bias_y;
 	
 	raw.ax = (uint16_t) biquad_apply(&accel_x_lpf, raw.ax);
 	raw.ay = (uint16_t) biquad_apply(&accel_y_lpf, raw.ay);
@@ -63,40 +59,35 @@ void imu_read_scaled(imu_scaled_t* out, const uint8_t* allowed_calib)
 	out->gx = (raw.gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
 	out->gy = (raw.gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
 	out->gz = (raw.gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
-	
+}
-	////////////////////////////////////////////////////////////
+
-  
+void imu_process_raw(imu_scaled_t* out, imu_raw_t* raw, const uint8_t* allowed_calib)
-  /*raw.ax = Rev16(raw.ax);
+{
-  raw.ay = Rev16(raw.ay);
+	// bias
-  raw.az = Rev16(raw.az);
+	raw->ax -= accel_bias_x;
-  
+	raw->ay -= accel_bias_y;
-  raw.gx = Rev16(raw.gx);
+
-  raw.gy = Rev16(raw.gy);
+	// фильтры
-  raw.gz = Rev16(raw.gz);
+	raw->ax = (uint16_t) biquad_apply(&accel_x_lpf, raw->ax);
-  
+	raw->ay = (uint16_t) biquad_apply(&accel_y_lpf, raw->ay);
-	out->ax = raw.ax - accel_bias_x;
+	raw->az = (uint16_t) biquad_apply(&accel_z_lpf, raw->az);
-	out->ay = raw.ay - accel_bias_y;
+
-	out->az = raw.az - accel_bias_z;
+	raw->gx = (uint16_t) biquad_apply(&gyro_x_lpf, raw->gx);
-	
+	raw->gy = (uint16_t) biquad_apply(&gyro_y_lpf, raw->gy);
-	out->gx = raw.gx - gyro_bias_x;
+	raw->gz = (uint16_t) biquad_apply(&gyro_z_lpf, raw->gz);
-	out->gy = raw.gy - gyro_bias_y;
+
-	out->gz = raw.gz - gyro_bias_z;
+	// калибровка
-  
+	if (allowed_calib)
-  out->ax = biquad_apply(&accel_x_lpf, out->ax);
+		imu_calib(raw, 50, 4096, 500);
-	out->ay = biquad_apply(&accel_y_lpf, out->ay);
+
-	out->az = biquad_apply(&accel_z_lpf, out->az);
+	// масштабирование
-	
+	out->ax = normalize(raw->ax, 1.0f, accel_min, accel_max);
-	out->gx = biquad_apply(&gyro_x_lpf, out->gx);
+	out->ay = normalize(raw->ay, 1.0f, accel_min, accel_max);
-	out->gy = biquad_apply(&gyro_y_lpf, out->gy);
+	out->az = normalize(raw->az, 1.0f, accel_min, accel_max);
-	out->gz = biquad_apply(&gyro_z_lpf, out->gz);
+
-  
+	out->gx = (raw->gx - gyro_shift[0]) / GYRO_SENS_SCALE_FACTOR;
-  out->ax /= ACCEL_SENS_SCALE_FACTOR;
+	out->gy = (raw->gy - gyro_shift[1]) / GYRO_SENS_SCALE_FACTOR;
-  out->ay /= ACCEL_SENS_SCALE_FACTOR;
+	out->gz = (raw->gz - gyro_shift[2]) / GYRO_SENS_SCALE_FACTOR;
-  out->az /= ACCEL_SENS_SCALE_FACTOR;
-  
-  out->gx /= GYRO_SENS_SCALE_FACTOR;
-  out->gy /= GYRO_SENS_SCALE_FACTOR;
-  out->gz /= GYRO_SENS_SCALE_FACTOR;*/
 }
 
 void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
@@ -104,8 +95,8 @@ void imu_calib(imu_raw_t* imu, long gyrLim, long accZero, long accMax)
 	long len = imu->gx*imu->gx + imu->gy*imu->gy + imu->gz*imu->gz;
 	if (len > gyrLim*gyrLim) return;
 	
-	x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
 	const float alpha = 0.001f;
+  x_calib = imu->gx, y_calib = imu->gy, z_calib = imu->gz;
 	
 	x_calib = x_calib * (1.0f - alpha) + imu->gx * alpha;
 	y_calib = y_calib * (1.0f - alpha) + imu->gy * alpha;
@@ -131,17 +122,12 @@ uint16_t Rev16(uint16_t v)
   return v;
 }
 
-/*void imu_calibrate()
+void imu_accel_calibrate()
 {
 	const int samples = 1000;
 	
 	float sum_ax = 0;
 	float sum_ay = 0;
-	float sum_az = 0;
-	
-	float sum_gx = 0;
-	float sum_gy = 0;
-	float sum_gz = 0;
 	
 	imu_raw_t imu;
 	
@@ -151,23 +137,13 @@ uint16_t Rev16(uint16_t v)
 		
 		sum_ax += imu.ax;
 		sum_ay += imu.ay;
-		sum_az += imu.az;
 		
-		sum_gx += imu.gx;
+		for (volatile uint16_t i = 0; i < 5000; ++i) { asm volatile("NOP"); };
-		sum_gy += imu.gy;
-		sum_gz += imu.gz;
-		
-		for (volatile uint16_t d = 0; d < 5000; ++d);
 	}
 	
 	accel_bias_x = sum_ax / samples;
 	accel_bias_y = sum_ay / samples;
-	accel_bias_z = sum_az / samples;
+}
-	
-	gyro_bias_x = sum_gx / samples;
-	gyro_bias_y = sum_gy / samples;
-	gyro_bias_z = sum_gz / samples;
-}*/
 
 float normalize(float value, float scale, float min, float max)
 {

Source/INS/IRS.c

@@ -11,26 +11,24 @@ void IRS_init(IRS* irs)
 
 void IRS_update(IRS* irs, float dt)
 {
-	Quaternion qConjugate = QuatConjugate(&irs->q);
-	
 	// gyro update
 	Vector3 gyro = 
 	{
-		irs->gyro.x * DEG2RAD,
+		irs->gyro.x * dt * DEG2RAD,
-		irs->gyro.y * DEG2RAD,
+		irs->gyro.y * dt * DEG2RAD,
-		irs->gyro.z * DEG2RAD
+		irs->gyro.z * dt * DEG2RAD
 	};
 	
 	Quaternion g = {gyro.x, gyro.y, gyro.z, 0};
 	g = QuatProd(&irs->q, &g);
-	g = QuatConstProd(&g, dt * 0.5f);
+	g = QuatConstProd(&g, 0.5f);
 	irs->q = QuatSum(&irs->q, &g);
 	irs->q = QuatNormalize(&irs->q, 1.0f);
 	
   // /gyro update
 	
 	// accel update
-	restoreQuat(irs);
+	//restoreQuat(irs);
 	
 	// /accel update
 }
@@ -100,14 +98,14 @@ void setAccelShift(IRS* irs, const float roll, const float pitch, const float ya
 
 Vector3 IRS_getGravity(const Quaternion* q)
 {
-    Vector3 g = 
+  Vector3 g = 
 	{
 		2.0f * (q->x*q->z - q->w*q->y),
 		2.0f * (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;
+  return g;
 }
 
 

Source/INS/IRS.h

@@ -9,10 +9,10 @@
 
 typedef struct
 {
-    Quaternion q;   			// ориентация
+  Quaternion q;   			// ориентация
 	Vector3 oriPRT; 			// orientation pitch roll tilts
-    Vector3 gyro;   			// deg/s
+  Vector3 gyro;   			// deg/s
-    Vector3 accel;  			// g
+  Vector3 accel;  			// g
 	
 	Quaternion shiftAccelPRY; 	// смещение акселерометра
 

Source/INS/geometry/vector.c

@@ -10,7 +10,7 @@ Vector2 normalizeV2(const Vector2* v, float gain)
 
 Vector3 normalizeV3(const Vector3* v, float gain)
 {
-	Vector3 res = {0};
+	Vector3 res = {0.0f, 0.0f, 0.0f};
 	float n = lengthV3(v);
 	
 	if (n > 1e-12f)
@@ -133,7 +133,7 @@ Vector2 constProdV2(const Vector2* v, float value)
 
 Vector3 constProdV3(const Vector3* v, float value)
 {
-	Vector3 res = {.x = v->x * value, .y = v->y * value, .z = v->z * value};
+	Vector3 res = {v->x * value, v->y * value, v->z * value};
 	return res;
 }
 

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);
+	  receiver_update(&rx_chs_raw);
-	    rx_chs_normalized = normalize_channels(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;
 	}
 }