Отказ от очереди

Source/BSP/Inc/imu.h

@@ -24,11 +24,11 @@
 #define ACCEL_FCHOICE_ON		1
 #define ACCEL_FCHOICE_OFF		0
 
-/*static volatile uint8_t i2c_busy = 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;*/
+static uint8_t i2c_addr = 0;
 
 typedef struct
 {
@@ -36,7 +36,7 @@ typedef struct
 	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
-typedef struct I2C_Request
+/*typedef struct I2C_Request
 {
     uint8_t addr;
     uint8_t reg;
@@ -49,9 +49,9 @@ typedef struct I2C_Request
 } I2C_Request;
 
 static I2C_Request* i2c_head = 0;
-static uint8_t i2c_busy = 0;
+static uint8_t i2c_busy = 0;*/
 
-//static void (*i2c_callback)(uint8_t* buf) = 0;
+static void (*i2c_callback)(uint8_t* buf) = 0;
 
 void imu_pow_init();
 
@@ -65,8 +65,9 @@ void imu_tim6_init(const uint16_t freq);
 
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
 
-void i2c_enqueue(I2C_Request* req);
-void i2c_start_next();
+/*void i2c_enqueue(I2C_Request* req);
+void i2c_start_next();*/
+
 void 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);

Source/BSP/Src/imu.c

@@ -1,8 +1,8 @@
 #include "imu.h"
 
-static I2C_Request* current_req = 0;
+/*static I2C_Request* current_req = 0;
 static uint8_t i2c_buf[16];
-static uint8_t i2c_index = 0;
+static uint8_t i2c_index = 0;*/
 
 void imu_pow_init()
 {
@@ -46,13 +46,6 @@ void i2c1_init()
 	I2C1->TIMINGR = 0x10802D9BUL;         // 400 kHz @ 16 MHz
 	
   I2C1->CR1 |= I2C_CR1_PE;
-  
-  I2C1->CR1 |= I2C_CR1_TXIE |
-             I2C_CR1_RXIE |
-             I2C_CR1_TCIE |
-             I2C_CR1_STOPIE;
-
-  NVIC_EnableIRQ(I2C1_EV_IRQn);
 }
 
 void imu_init()
@@ -114,7 +107,7 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
 	I2C1->ICR |= I2C_ICR_STOPCF;
 }
 
-void i2c_enqueue(I2C_Request* req)
+/*void i2c_enqueue(I2C_Request* req)
 {
     req->next = 0;
 
@@ -159,21 +152,30 @@ void i2c_start_next()
     I2C1->CR2 = (current_req->addr << 1) |
                 (1 << I2C_CR2_NBYTES_Pos) |
                 I2C_CR2_START;
-}
+}*/
 
 void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
 {
-    static I2C_Request req;
+  if (i2c_busy) return; // ❗ ВАЖНО
 
-    static uint8_t buf[16];
+  i2c_busy = 1;
 
-    req.addr = addr;
-    req.reg = reg;
-    req.buf = buf;
-    req.len = len;
-    req.callback = cb;
+  i2c_addr = addr;
+  i2c_reg = reg;
+  i2c_len = len;
+  i2c_callback = cb;
   
-    i2c_enqueue(&req);
+  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;
 }
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
@@ -197,44 +199,48 @@ void I2C1_EV_IRQHandler()
 {
   uint32_t isr = I2C1->ISR;
 
-    // --- TXIS ---
+  // TXIS — отправляем регистр
   if (isr & I2C_ISR_TXIS)
   {
-        I2C1->TXDR = current_req->reg;
+    I2C1->TXDR = i2c_reg;
   }
 
-    // --- TC ---
+  // TC — запускаем чтение
   else if (isr & I2C_ISR_TC)
   {
-        // запускаем чтение
-        I2C1->CR2 = (current_req->addr << 1) |
+    I2C1->CR2 = (i2c_addr << 1) |
                 I2C_CR2_RD_WRN |
-                    (current_req->len << I2C_CR2_NBYTES_Pos) |
+                (i2c_len << I2C_CR2_NBYTES_Pos) |
                 I2C_CR2_AUTOEND |
                 I2C_CR2_START;
   }
 
-    // --- RXNE ---
+  // RXNE — читаем байты
   else if (isr & I2C_ISR_RXNE)
   {
-        i2c_buf[i2c_index++] = I2C1->RXDR;
+    static uint8_t index = 0;
+    i2c_buf[index++] = I2C1->RXDR;
+
+    if (index >= i2c_len)
+        index = 0;
   }
 
-    // --- STOP ---
+  // STOP — завершение
   else if (isr & I2C_ISR_STOPF)
   {
     I2C1->ICR |= I2C_ICR_STOPCF;
 
-        // копируем данные
-        for (uint8_t i = 0; i < current_req->len; i++)
-            current_req->buf[i] = i2c_buf[i];
+    i2c_busy = 0;
     
-        // callback
-        if (current_req->callback)
-            current_req->callback(current_req->buf);
+    I2C1->CR1 |= I2C_CR1_TXIE |
+             I2C_CR1_RXIE |
+             I2C_CR1_TCIE |
+             I2C_CR1_STOPIE;
 
-        // следующий запрос
-        i2c_start_next();
+    NVIC_EnableIRQ(I2C1_EV_IRQn);
+
+    if (i2c_callback)
+        i2c_callback(i2c_buf);
   }
 }
 

Source/Core/Src/system_stm32g4xx.c

@@ -283,3 +283,7 @@ void SystemCoreClockUpdate(void)
   */
 
 
+
+
+
+

Source/INS/IRS.c

@@ -28,7 +28,7 @@ void IRS_update(IRS* irs, float dt)
   // /gyro update
 	
 	// accel update
-	//restoreQuat(irs);
+	// restoreQuat(irs);
 	
 	// /accel update
 }

Source/main.c

@@ -115,8 +115,6 @@ void imu_callback(uint8_t* buf)
   euler = QuatToEuler(&irs.q);
 }
 
-static uint8_t div = 0;
-
 void TIM6_DAC_IRQHandler()
 {
 	if (TIM6->SR & TIM_SR_UIF)