diff --git a/Source/BSP/Inc/lidar.h b/Source/BSP/Inc/lidar.h
index 9930b77..081d9d8 100644
--- a/Source/BSP/Inc/lidar.h
+++ b/Source/BSP/Inc/lidar.h
@@ -9,6 +9,7 @@
 #define USART3_FRAME_SIZE	9
 #define LIDAR_MIN_DIST		0.01f
 #define LIDAR_MAX_DIST		40.0f
+#define TF02_I2C_ADDR		0x10
 
 typedef struct
 {
@@ -35,6 +36,9 @@ void USART3_IRQHandler();
 void lidar_update(lidar_data* lidar);
 
 void lidar_i2c2_init();
-void lidar_i2c2_update();
+static void i2c2_wait_txis();
+static void i2c2_wait_stop();
+static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size);
+void tf02_force_uart();
 
 #endif
\ No newline at end of file
diff --git a/Source/BSP/Src/lidar.c b/Source/BSP/Src/lidar.c
index 9c54be4..b7c9eea 100644
--- a/Source/BSP/Src/lidar.c
+++ b/Source/BSP/Src/lidar.c
@@ -120,6 +120,65 @@ void lidar_i2c2_init()
 	I2C2->CR1 |= I2C_CR1_PE;
 }
 
+static void i2c2_wait_txis()
+{
+    while (!(I2C2->ISR & I2C_ISR_TXIS));
+}
+
+static void i2c2_wait_stop()
+{
+    while (!(I2C2->ISR & I2C_ISR_STOPF));
+    I2C2->ICR |= I2C_ICR_STOPCF;
+}
+
+static int i2c2_write(uint8_t addr, uint8_t *data, uint8_t size)
+{
+    while (I2C2->ISR & I2C_ISR_BUSY);
+
+    I2C2->CR2 = 0;
+    I2C2->CR2 |= (addr << 1);                 // 7-bit addr
+    I2C2->CR2 |= (size << 16);                // bite count
+    I2C2->CR2 |= I2C_CR2_AUTOEND;             // auto stop
+    I2C2->CR2 |= I2C_CR2_START;               // start
+
+    for (uint8_t i = 0; i < size; i++)
+    {
+        i2c2_wait_txis();
+        I2C2->TXDR = data[i];
+    }
+
+    i2c2_wait_stop();
+
+    // check NACK
+    if (I2C2->ISR & I2C_ISR_NACKF)
+    {
+        I2C2->ICR |= I2C_ICR_NACKCF;
+        return 0;
+    }
+
+    return 1;
+}
+
+void tf02_force_uart()
+{
+    uint8_t cmd_uart[] = {0x5A, 0x05, 0x0A, 0x00, 0x69};
+    uint8_t cmd_save[] = {0x5A, 0x04, 0x11, 0x6F};
+
+    // force UART command
+    if (!i2c2_write(TF02_I2C_ADDR, cmd_uart, sizeof(cmd_uart)))
+    {
+        // no ACK — lidar is not on i2c
+        return;
+    }
+
+    for (volatile int i = 0; i < 100000; i++);
+
+	// save command
+    i2c2_write(TF02_I2C_ADDR, cmd_save, sizeof(cmd_save));
+
+    for (volatile int i = 0; i < 200000; i++);
+}
+
 
 
 
diff --git a/Source/main.c b/Source/main.c
index fa18086..b2b9412 100644
--- a/Source/main.c
+++ b/Source/main.c
@@ -19,7 +19,7 @@ void delay_ms(uint32_t ms);
 
 int main(void)
 {
-	__enable_irq();
+	/*__enable_irq();
 	
 	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
 	NVIC_SetPriority(USART3_IRQn, 1);
@@ -36,11 +36,14 @@ int main(void)
 	
 	receiver_init();
 	
-	motors_init();
+	motors_init();*/
 	
-	lidar_init();
+	//lidar_init();
 	
-	while (1)
+	lidar_i2c2_init();
+	tf02_force_uart();
+	
+	/*while (1)
 	{
 		attitude_update(&attitude, &imu);
 		
@@ -59,7 +62,7 @@ int main(void)
 		{
 			motors_turn_off();
 		}
-	}
+	}*/
 }
 
 void delay_ms(uint32_t ms)