diff --git a/Source/BSP/Inc/imu.h b/Source/BSP/Inc/imu.h
index 0d6783c..fff886e 100644
--- a/Source/BSP/Inc/imu.h
+++ b/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,22 @@ typedef struct
 	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
-static void (*i2c_callback)(uint8_t* buf) = 0;
+typedef struct I2C_Request
+{
+    uint8_t addr;
+    uint8_t reg;
+    uint8_t *buf;
+    uint8_t len;
+
+    void (*callback)(uint8_t*);
+
+    struct I2C_Request* next;
+} I2C_Request;
+
+static I2C_Request* i2c_head = 0;
+static uint8_t i2c_busy = 0;
+
+//static void (*i2c_callback)(uint8_t* buf) = 0;
 
 void imu_pow_init();
 
@@ -50,7 +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);
 
-uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
+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);
 
diff --git a/Source/BSP/Src/imu.c b/Source/BSP/Src/imu.c
index 9353a5e..f00ab0a 100644
--- a/Source/BSP/Src/imu.c
+++ b/Source/BSP/Src/imu.c
@@ -1,5 +1,9 @@
 #include "imu.h"
 
+static I2C_Request* current_req = 0;
+static uint8_t i2c_buf[16];
+static uint8_t i2c_index = 0;
+
 void imu_pow_init()
 {
 	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
@@ -42,6 +46,13 @@ 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()
@@ -103,25 +114,66 @@ 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*))
+void i2c_enqueue(I2C_Request* req)
 {
-	if (i2c_busy) return 0;
+    req->next = 0;
 
-	i2c_busy = 1;
+    __disable_irq();
 
-	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);
+    if (!i2c_head)
+    {
+        i2c_head = req;
+    }
+    else
+    {
+        I2C_Request* cur = i2c_head;
+        while (cur->next) cur = cur->next;
+        cur->next = req;
+    }
 
-	// старт записи регистра
-	I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
+    __enable_irq();
 
-	return 1;
+    // если I2C свободен — стартуем
+    if (!i2c_busy)
+    {
+        NVIC_SetPendingIRQ(I2C1_EV_IRQn);
+    }
+}
+
+void i2c_start_next()
+{
+    if (!i2c_head)
+    {
+        i2c_busy = 0;
+        return;
+    }
+
+    i2c_busy = 1;
+
+    current_req = i2c_head;
+    i2c_head = i2c_head->next;
+
+    i2c_index = 0;
+
+    // сначала пишем регистр
+    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;
+
+    static uint8_t buf[16];
+
+    req.addr = addr;
+    req.reg = reg;
+    req.buf = buf;
+    req.len = len;
+    req.callback = cb;
+
+    i2c_enqueue(&req);
 }
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
@@ -143,47 +195,47 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
 
 void I2C1_EV_IRQHandler()
 {
-	static uint8_t index = 0;
-	static uint8_t stage = 0; // 0 = send reg, 1 = read
+    uint32_t isr = I2C1->ISR;
 
-	// TX — отправляем регистр
-	if (I2C1->ISR & I2C_ISR_TXIS)
-	{
-		I2C1->TXDR = i2c_reg;
-	}
+    // --- TXIS ---
+    if (isr & I2C_ISR_TXIS)
+    {
+        I2C1->TXDR = current_req->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;
+    // --- TC ---
+    else if (isr & I2C_ISR_TC)
+    {
+        // запускаем чтение
+        I2C1->CR2 = (current_req->addr << 1) |
+                    I2C_CR2_RD_WRN |
+                    (current_req->len << I2C_CR2_NBYTES_Pos) |
+                    I2C_CR2_AUTOEND |
+                    I2C_CR2_START;
+    }
 
-		index = 0;
-		stage = 1;
-	}
+    // --- RXNE ---
+    else if (isr & I2C_ISR_RXNE)
+    {
+        i2c_buf[i2c_index++] = I2C1->RXDR;
+    }
 
-	// чтение данных
-	if (I2C1->ISR & I2C_ISR_RXNE)
-	{
-		i2c_buf[index++] = I2C1->RXDR;
-	}
+    // --- STOP ---
+    else if (isr & I2C_ISR_STOPF)
+    {
+        I2C1->ICR |= I2C_ICR_STOPCF;
 
-	// завершение
-	if (I2C1->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;
-    
-    // отключаем IRQ
-    I2C1->CR1 &= ~(I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE);
+        // callback
+        if (current_req->callback)
+            current_req->callback(current_req->buf);
 
-		if (i2c_callback)
-			i2c_callback(i2c_buf);
-	}
+        // следующий запрос
+        i2c_start_next();
+    }
 }
 
 void imu_read_raw(imu_raw_t* data)