diff --git a/Source/BSP/Inc/lidar.h b/Source/BSP/Inc/lidar.h
index 9777017..b7ffb15 100644
--- a/Source/BSP/Inc/lidar.h
+++ b/Source/BSP/Inc/lidar.h
@@ -5,6 +5,9 @@
 
 #include "stm32g431xx.h"
 
+#define USART3_START_BYTE	0x59
+#define USART3_FRAME_SIZE	9
+
 typedef struct
 {
 	uint8_t header1;		// 0x59
@@ -16,7 +19,7 @@ typedef struct
  	uint8_t temp_l;
  	uint8_t temp_h;
  	uint8_t checksum;
-} lidar_data;
+} lidar_data_buf;
 
 void lidar_init();
 void lidar_update();
diff --git a/Source/BSP/Src/lidar.c b/Source/BSP/Src/lidar.c
index d50b85b..4a9242a 100644
--- a/Source/BSP/Src/lidar.c
+++ b/Source/BSP/Src/lidar.c
@@ -1,5 +1,11 @@
 #include "lidar.h"
 
+volatile uint8_t usart3_index = 0;
+volatile uint8_t usart3_checksum = 0;
+volatile uint8_t usart3_frame_ready = 0;
+static lidar_data_buf buffer;
+static uint8_t* buff_data = (uint8_t*)&buffer;
+
 void lidar_init()
 {
 	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
@@ -51,4 +57,35 @@ void lidar_init()
 	
 	// Interrupt enable
 	NVIC_EnableIRQ(USART3_IRQn);
+}
+
+void USART3_IRQHandler()
+{
+	if (USART3->ISR & USART_ISR_RXNE)
+	{
+		uint8_t b = USART3->RDR;
+		
+		if (usart3_index < 2)
+		{
+			if (b == USART3_START_BYTE)
+				buff_data[usart3_index++] = b;
+		}
+		else if (usart3_index < USART3_FRAME_SIZE)
+			buff_data[usart3_index++] = b;
+		
+		if (usart3_index == USART3_FRAME_SIZE)
+		{
+			for (uint8_t i = 0; i < USART3_FRAME_SIZE; ++i) usart3_checksum += buff_data[i];
+			if (buffer.checksum == usart3_checksum)
+			{
+				usart3_index = 0;
+				usart3_frame_ready = 1;
+			}
+			else
+			{
+				usart3_index = 0;
+				usart3_frame_ready = 0;
+			}
+		}
+	}
 }
\ No newline at end of file