diff --git a/firmware/.cproject b/firmware/.cproject
index 4681c93..47fc747 100644
--- a/firmware/.cproject
+++ b/firmware/.cproject
@@ -52,6 +52,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/BMS_v3/Middlewares/Third_Party/FatFs/src}&quot;"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.1889788482" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
@@ -81,6 +83,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols.739392240" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -121,6 +125,7 @@
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Libs"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -178,6 +183,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Middlewares/Third_Party/FatFs/src}&quot;"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols.390483859" name="Define symbols (-D)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.definedsymbols" useByScannerDiscovery="false" valueType="definedSymbols">
@@ -206,6 +213,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags.1890454030" name="Other flags" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.cpp.compiler.option.otherflags" useByScannerDiscovery="true" valueType="stringList"/>
 							</tool>
@@ -245,6 +254,7 @@
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="Libs"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/firmware/.cproject_org b/firmware/.cproject_org
index 09afd6e..58bf7bb 100644
--- a/firmware/.cproject_org
+++ b/firmware/.cproject_org
@@ -46,6 +46,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option id="gnu.c.compiler.option.preprocessor.def.symbols.903935306" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -69,6 +71,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option id="gnu.cpp.compiler.option.preprocessor.def.593685131" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -114,6 +118,7 @@
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Libs"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -164,6 +169,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option id="gnu.c.compiler.option.preprocessor.def.symbols.903935306" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -186,6 +193,8 @@
 									<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32F1xx/Include"/>
 									<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
+									<listOptionValue builtIn="false" value="../Libs/"/>
+									<listOptionValue builtIn="false" value="../Libs/CAN/"/>
 								</option>
 								<option id="gnu.cpp.compiler.option.preprocessor.def.symbols.1054182349" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def.symbols" useByScannerDiscovery="false" valueType="definedSymbols">
 									<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
@@ -231,6 +240,7 @@
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
 						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
+						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Libs"/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/firmware/.idea/misc.xml b/firmware/.idea/misc.xml
index b06c61e..3c45264 100644
--- a/firmware/.idea/misc.xml
+++ b/firmware/.idea/misc.xml
@@ -5,6 +5,7 @@
     <sourceRoots>
       <file path="$PROJECT_DIR$/Core" />
       <file path="$PROJECT_DIR$/Drivers" />
+      <file path="$PROJECT_DIR$/Libs" />
       <file path="$PROJECT_DIR$/Middlewares" />
       <file path="$PROJECT_DIR$/USB_DEVICE" />
       <file path="$PROJECT_DIR$/USB_Device_Library" />
diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt
index 024c9b1..377b75a 100644
--- a/firmware/CMakeLists.txt
+++ b/firmware/CMakeLists.txt
@@ -1,7 +1,7 @@
 #THIS FILE IS AUTO GENERATED FROM THE TEMPLATE! DO NOT CHANGE!
 set(CMAKE_SYSTEM_NAME Generic)
 set(CMAKE_SYSTEM_VERSION 1)
-cmake_minimum_required(VERSION 3.24)
+cmake_minimum_required(VERSION 3.25)
 
 # specify cross-compilers and tools
 set(CMAKE_C_COMPILER arm-none-eabi-gcc)
@@ -63,6 +63,9 @@ include_directories(
         Drivers/STM32F1xx_HAL_Driver/Inc
         Drivers/STM32F1xx_HAL_Driver/Inc/Legacy
 
+        Libs/
+        Libs/CAN
+
         Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
         Middlewares/ST/STM32_USB_Device_Library/Core/Inc
 
@@ -76,7 +79,7 @@ include_directories(
 add_definitions(-DDEBUG -DUSE_HAL_DRIVER -DSTM32F107xC)
 
 
-file(GLOB_RECURSE SOURCES "startup/*.*" "USB_DEVICE/*.*" "Middlewares/*.*" "Drivers/*.*" "Core/*.*")
+file(GLOB_RECURSE SOURCES "startup/*.*" "USB_DEVICE/*.*" "Middlewares/*.*" "Drivers/*.*" "Core/*.*" "Libs/*.*")
 
 #set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/)
 
diff --git a/firmware/CMakeLists_template.txt b/firmware/CMakeLists_template.txt
index a6f9d32..ea32ce3 100644
--- a/firmware/CMakeLists_template.txt
+++ b/firmware/CMakeLists_template.txt
@@ -62,6 +62,9 @@ include_directories(
         Drivers/STM32F1xx_HAL_Driver/Inc
         Drivers/STM32F1xx_HAL_Driver/Inc/Legacy
 
+        Libs/
+        Libs/CAN
+
         Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc
         Middlewares/ST/STM32_USB_Device_Library/Core/Inc
 
diff --git a/firmware/Core/Src/driverSWUART2.c b/firmware/Core/Src/driverSWUART2.c
index 0ba2f4c..a69daf7 100644
--- a/firmware/Core/Src/driverSWUART2.c
+++ b/firmware/Core/Src/driverSWUART2.c
@@ -25,9 +25,9 @@ char driverSWUART2PutCharInOutputBuffer(char character, FILE *stream) {
 bool driverSWUART2Task(void) {
 	char outputChar;
 	
-	if(!driverSWUART2OutputBuffer->isEmpty(driverSWUART2OutputBuffer)){						// Check if there is data in the ouput buffer
+	if(!driverSWUART2OutputBuffer->isEmpty(driverSWUART2OutputBuffer)){					// Check if there is data in the ouput buffer
 		driverSWUART2OutputBuffer->pull(driverSWUART2OutputBuffer,&outputChar);			// Pull the data from ouput buffer
-		driverHWUART2SendChar(outputChar);																					// And send it to the uart
+		driverHWUART2SendChar(outputChar);										// And send it to the uart
 	}
 	
 	return !driverSWUART2OutputBuffer->isEmpty(driverSWUART2OutputBuffer);
diff --git a/firmware/Core/Src/main.c b/firmware/Core/Src/main.c
index 1a77f95..75cedbf 100644
--- a/firmware/Core/Src/main.c
+++ b/firmware/Core/Src/main.c
@@ -38,7 +38,9 @@
 #include "DPC_Timeout.h"
 #include "GSM.h"
 #include "time.h"
-#include "can.h"
+
+#include "../../Libs/CAN/can.h"
+#include "../../Libs/CAN/can_messenger.h"
 
 #include "SD_Card.h"
 
@@ -279,14 +281,6 @@ uint64_t output_control_clock = 0;
 bool need_shunt_charging_contractor = false;
 uint64_t shunt_charging_contractor_clock = 0;
 
-uint64_t can1_transmit_clock = 0;
-uint64_t can2_transmit_clock = 0;
-
-uint8_t can1_transmit_queue[500];
-uint8_t can2_transmit_queue[100];
-
-uint16_t can1_transmit_queue_size = 0;
-uint16_t can2_transmit_queue_size = 0;
 
 
 #define __MA_WIN_SIZE (500)
@@ -315,8 +309,6 @@ void Save_data_to_Backup();
 void Restore_shutdown_data();
 void updateLimitsFromMem();
 
-void addCanPacketToQueue(uint16_t id, uint8_t* data, uint8_t* queue, uint16_t* queueSize);
-void transmitCanPacketFromQueue(uint8_t* queue, uint16_t* queueSize, CAN_HandleTypeDef* hcan);
 
 void modem_init();
 
@@ -372,7 +364,6 @@ void usbTIM(uint8_t OnOff) {
 	}
 }
 
-u8_t TestData[8] = {1,2,3,4,5,6,7,8};
 
 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 {
@@ -385,15 +376,23 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 	{
 		if (TIM3_Clock % 10 == 0)
 		{
-			CAN_TransmitRTR(0x500, 8, &hcan2);
+			CAN_TransmitRTR(CAN_BMS_HEARTBEAT, 8, &hcan2);
 		}
-		else if (TIM3_Clock % 3 == 0)
+
+        if (TIM3_Clock % 4 == 0)
         {
-        	transmitCanPacketFromQueue(can1_transmit_queue, &can1_transmit_queue_size, &hcan1);
+//        	transmitCanPacketFromQueue(can1_transmit_queue, &can1_transmit_queue_size, &hcan1);
+        	transmitCanPacketFromQueueCan1();
         }
-        else if (TIM3_Clock % 3 == 1)
+#include "../../Libs/Utils/micros.h"
+        if (TIM3_Clock % 3 == 0)
         {
-        	transmitCanPacketFromQueue(can2_transmit_queue, &can2_transmit_queue_size, &hcan2);
+            volatile u32_t measure = 0;
+//            StartCodeMeasure();
+//        	transmitCanPacketFromQueue(can2_transmit_queue, &can2_transmit_queue_size, &hcan2);
+        	transmitCanPacketFromQueueCan2();
+//            StopCodeMeasure(&measure);
+//            __NOP();
         }
 
 		TIM3_Clock++;
@@ -426,8 +425,6 @@ void load_switch(uint8_t state) {
 /* USER CODE END 0 */
 
 
-
-
 uint16_t MA_Filter(uint16_t input, uint8_t array)
 {
   uint16_t Result = 0;
@@ -1103,269 +1100,6 @@ void outputControl()
 	output_control_clock = TIM_Clock;
 }
 
-void transmitCan1Packet()
-{
-	if ((TIM_Clock - can1_transmit_clock) < 1000)
-	{
-		return;
-	}
-
-
-	uint8_t data1[8] = {0x45};
-	CAN_Transmit(300, data1, 8, &hcan1);
-	can1_transmit_clock = TIM_Clock;
-	return;
-
-	CAN_TxHeaderTypeDef header;
-	header.IDE = CAN_ID_STD;
-	header.RTR = CAN_RTR_DATA;
-	header.StdId = 0x444;
-	header.DLC = 1;
-
-	uint8_t data[32] = {};
-	uint32_t mailbox = 0;
-
-	// sending SOC, SOH and number of cells
-	uint16_t id = 0x101;
-	memcpy(data, &id, sizeof(id));
-	memcpy(data + 2, &packState.SoC, sizeof(packState.SoC));
-	memcpy(data + 6, &packState.SoCCapacityAh, sizeof(packState.SoCCapacityAh));
-	memcpy(data + 10, &generalConfig->noOfCellsSeries, sizeof(generalConfig->noOfCellsSeries));
-	header.DLC = 11;
-	HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
-
-	// sending charge current, discharge current // TODO
-	id = 0x102;
-	memcpy(data, &id, sizeof(id));
-	memcpy(data + 2, &packState.packCurrent, sizeof(packState.packCurrent));
-	memcpy(data + 6, &packState.loCurrentLoadCurrent, sizeof(packState.loCurrentLoadCurrent));
-	header.DLC = 10;
-	HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
-
-	// sending BMS state, input state, output state // TODO
-	id = 0x103;
-	memcpy(data, &id, sizeof(id));
-	memcpy(data + 2, &packState.cellVoltageLow, sizeof(packState.cellVoltageLow));
-	memcpy(data + 6, &packState.cellVoltageAverage, sizeof(packState.cellVoltageAverage));
-	memcpy(data + 10, &packState.cellVoltageHigh, sizeof(packState.cellVoltageHigh));
-	header.DLC = 12;
-	HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
-
-	// sending cell voltages
-	id = 0x200;
-	for (int cellPointer = 0; cellPointer < generalConfig->noOfCellsSeries * generalConfig->noOfParallelModules; ++cellPointer)
-	{
-		++id;
-		memcpy(data, &id, sizeof(id));
-		float voltage = 0;
-		if (packState.cellVoltagesIndividual[cellPointer].cellBleedActive)
-			voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * -1000;
-		else
-			voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * 1000;
-		memcpy(data + 2, &voltage, sizeof(voltage));
-		header.DLC = 6;
-		HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
-	}
-
-	// sending temperatures
-	id = 0x300;
-	for (int sensorPointer = 0; sensorPointer < NoOfTempSensors; ++sensorPointer)
-	{
-		++id;
-		memcpy(data, &id, sizeof(id));
-		float temperature = packState.temperatures[sensorPointer];
-		memcpy(data + 2, &temperature, sizeof(temperature));
-		header.DLC = 6;
-		HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
-	}
-
-	can1_transmit_clock = TIM_Clock;
-}
-
-int getIndexBySoc()
-{
-	const float soc = packState.SoC;
-	if (soc <= 0)
-	{
-		return 0;
-	}
-	else if (soc <= 10 && soc > 0)
-	{
-		return 1;
-	}
-	else if (soc <= 20 && soc > 10)
-	{
-		return 2;
-	}
-	else if (soc <= 30 && soc > 20)
-	{
-		return 3;
-	}
-	else if (soc <= 40 && soc > 30)
-	{
-		return 4;
-	}
-	else if (soc <= 50 && soc > 40)
-	{
-		return 5;
-	}
-	else if (soc <= 60 && soc > 50)
-	{
-		return 6;
-	}
-	else if (soc <= 70 && soc > 60)
-	{
-		return 7;
-	}
-	else if (soc <= 80 && soc > 70)
-	{
-		return 8;
-	}
-	else if (soc <= 95 && soc > 80)
-	{
-		return 9;
-	}
-	else if (soc > 95)
-	{
-		return 10;
-	}
-
-	return -1;
-}
-
-int getIndexByTemperature()
-{
-	const float temp = packState.tempBatteryAverage;
-	if (temp < 0)
-	{
-		return 0;
-	}
-	else if (temp < 2 && temp >= 0)
-	{
-		return 1;
-	}
-	else if (temp < 7 && temp >= 2)
-	{
-		return 2;
-	}
-	else if (temp < 15 && temp >= 7)
-	{
-		return 3;
-	}
-	else if (temp < 20 && temp >= 15)
-	{
-		return 4;
-	}
-	else if (temp < 45 && temp >= 20)
-	{
-		return 5;
-	}
-	else if (temp < 50 && temp >= 45)
-	{
-		return 6;
-	}
-	else if (temp < 55 && temp >= 50)
-	{
-		return 7;
-	}
-	else if (temp >= 55)
-	{
-		return 8;
-	}
-
-	return -1;
-}
-
-void transmitCan2Packet()
-{
-	if ((TIM_Clock - can2_transmit_clock) < 500)
-	{
-		return;
-	}
-
-	can2_transmit_clock = TIM_Clock;
-
-	uint8_t buffer[8] = {0};
-
-	// sending common current and voltage command
-	memcpy(buffer, &packState.packCurrent, sizeof(packState.packCurrent));
-	memcpy(buffer + 4, &packState.packVoltage, sizeof(packState.packVoltage));
-	addCanPacketToQueue(0x501, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-
-	// sending charge voltages (start, end) command
-	const float startVoltage = generalConfig->cellHardUnderVoltage * generalConfig->noOfCellsSeries;
-	const float endVoltage = generalConfig->cellHardOverVoltage * generalConfig->noOfCellsSeries;
-	memcpy(buffer, &startVoltage, sizeof(startVoltage));
-	memcpy(buffer + 4, &endVoltage, sizeof(endVoltage));
-	addCanPacketToQueue(0x502, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-
-	// sending fault voltages (under, over) command
-	const float startFaultVoltage = generalConfig->cellLCSoftUnderVoltage * generalConfig->noOfCellsSeries;
-	const float endFaultVoltage = generalConfig->cellSoftOverVoltage * generalConfig->noOfCellsSeries;
-	memcpy(buffer, &startFaultVoltage, sizeof(startFaultVoltage));
-	memcpy(buffer + 4, &endFaultVoltage, sizeof(endFaultVoltage));
-	addCanPacketToQueue(0x503, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-
-	// sending charge start current
-	float chargeStartingCurrent = 15; // TODO move to generalConfig
-    uint32_t chargeStartingCurrentInterval = 20; // in seconds TODO move to generalConfig
-	memcpy(buffer, &chargeStartingCurrent, sizeof(chargeStartingCurrent));
-	memcpy(buffer + 4, &chargeStartingCurrentInterval, sizeof(chargeStartingCurrentInterval));
-    addCanPacketToQueue(0x504, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-
-    // sending table current and end-of-charge current command
-	const int socIndex = getIndexBySoc();
-	const int temperatureIndex = getIndexByTemperature();
-	float tableCurrent = 0;
-
-	if (socIndex != -1 && temperatureIndex != -1)
-	{
-		float tableValue = generalConfig->externalChargeCurrentTable[temperatureIndex][socIndex];
-		float pureCurrent = generalConfig->externalChargeUnitTable[temperatureIndex][socIndex];
-		tableCurrent = pureCurrent ? tableValue : generalConfig->batteryCapacity * tableValue;
-	}
-
-    float chargeEndingCurrent = 5; // TODO move to generalConfig
-    memcpy(buffer + 0, &tableCurrent, sizeof(tableCurrent));
-    memcpy(buffer + 4, &chargeEndingCurrent, sizeof(chargeEndingCurrent));
-    addCanPacketToQueue(0x505, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-
-    // sending charge permission, charge ending, charge/discharge state command
-    const uint8_t chargeAllowed = packState.chargeAllowed ? 0xFF : 0x00;
-    const uint8_t chargeEnding = packState.SoC > 95 ? 0xFF : 0x00;
-    const uint8_t chargeSwitchState = charge_switch_state ? 0xFF : 0x00;
-	memset(buffer, '\0', sizeof(buffer));
-	memcpy(buffer + 0, &chargeAllowed, sizeof(chargeAllowed));
-	memcpy(buffer + 1, &chargeEnding, sizeof(chargeEnding));
-	memcpy(buffer + 2, &chargeSwitchState, sizeof(chargeSwitchState));
-	addCanPacketToQueue(0x506, buffer, can2_transmit_queue, &can2_transmit_queue_size);
-}
-
-void addCanPacketToQueue(uint16_t id, uint8_t* data, uint8_t* queue, uint16_t* queueSize)
-{
-	memcpy(queue + *queueSize, &id, 2);
-	memcpy(queue + *queueSize + 2, data, 8);
-
-	*queueSize += 10;
-}
-
-void transmitCanPacketFromQueue(uint8_t* queue, uint16_t* queueSize, CAN_HandleTypeDef* hcan)
-{
-	if (*queueSize < 10)
-		return;
-
-	uint16_t id = 0;
-	uint8_t data[8] = {};
-
-	memcpy(&id, queue, 2);
-	memcpy(data, queue + 2, 8);
-
-	*queueSize -= 10;
-	memmove(queue, queue + 10, *queueSize);
-
-	CAN_Transmit(id, data, 8, hcan);
-}
-
 void modem_init(){
 
 	  SIM800_Var_Init(&SIM800_Struct);
@@ -2236,7 +1970,7 @@ int main(void)
 
 	//MX_CAN1_Init();
 	//MX_CAN2_Init();
-
+    can_msgr_init();
 	CAN_SetSpeed(CAN_SPD_100, &hcan1);
 	CAN_SetSpeed(CAN_SPD_100, &hcan2);
 
diff --git a/firmware/Core/Src/can.c b/firmware/Libs/CAN/can.c
similarity index 99%
rename from firmware/Core/Src/can.c
rename to firmware/Libs/CAN/can.c
index edb67ca..a583163 100644
--- a/firmware/Core/Src/can.c
+++ b/firmware/Libs/CAN/can.c
@@ -12,7 +12,7 @@
 #define CAN_BASIC_MODE CAN_MODE_NORMAL
 #endif
 
-//#include "can_messenger.h"
+#include "can_messenger.h"
 
 extern CAN_HandleTypeDef hcan1;
 extern CAN_HandleTypeDef hcan2;
@@ -324,6 +324,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
 
 void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
     if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &can2RX, can2Data) == HAL_OK) {
+        can2_receive_from_irq(&can2RX, can2Data);
         //can_irq_receive(&can2RX, can2Data, 2);
     }
 }
diff --git a/firmware/Core/Inc/can.h b/firmware/Libs/CAN/can.h
similarity index 100%
rename from firmware/Core/Inc/can.h
rename to firmware/Libs/CAN/can.h
diff --git a/firmware/Libs/CAN/can_messenger.c b/firmware/Libs/CAN/can_messenger.c
new file mode 100644
index 0000000..058f082
--- /dev/null
+++ b/firmware/Libs/CAN/can_messenger.c
@@ -0,0 +1,354 @@
+//
+// Created by enik on 03.04.23.
+//
+
+#include "can_messenger.h"
+#include "can.h"
+//#include "modCommands.h"
+#include "modPowerElectronics.h"
+
+#include <string.h>
+
+#include "../RingBuffer/RingBuffer.h"
+
+extern uint64_t TIM_Clock;
+
+extern CAN_HandleTypeDef hcan1;
+extern CAN_HandleTypeDef hcan2;
+
+uint64_t can1_transmit_clock = 0;
+uint64_t can2_transmit_clock = 0;
+
+typedef struct CAN_MSG_t {
+    u16_t id;
+    u8_t len;
+    u8_t msg[8];
+} CAN_MSG_t;
+
+static volatile CAN_MSG_t can1_transmit_q_msg[45];  // 540 bytes
+static volatile CAN_MSG_t can2_transmit_q_msg[10];  // 120 bytes
+static volatile RINGBUF_t can1_ring;
+static volatile RINGBUF_t can2_ring;
+
+static volatile CAN_MSG_t can1_buf = {0,};      // Buf for 1 packet
+static volatile CAN_MSG_t can2_buf = {0,};      // Buf for 1 packet
+
+static volatile CAN_MSG_t can1_tx_buf = {0,};      // Buf for 1 TX packet
+static volatile CAN_MSG_t can2_tx_buf = {0,};      // Buf for 1 TX packet
+
+extern modPowerElectronicsPackStateTypedef packState;
+extern modConfigGeneralConfigStructTypedef *generalConfig;
+
+static volatile bool threshold_request = 0;
+static volatile bool active_request = 0;
+
+void can_msgr_init() {
+    RingBuf_Init((void *) &can1_transmit_q_msg, 45, sizeof(CAN_MSG_t), (RINGBUF_t *) &can1_ring);
+    RingBuf_Init((void *) &can2_transmit_q_msg, 10, sizeof(CAN_MSG_t), (RINGBUF_t *) &can2_ring);
+}
+
+void transmitCan1Packet() {
+    if ((TIM_Clock - can1_transmit_clock) < 1000) {
+        return;
+    }
+
+    uint8_t data1[8] = {0x45};
+    CAN_Transmit(300, data1, 8, &hcan1);
+    can1_transmit_clock = TIM_Clock;
+    return;
+
+    CAN_TxHeaderTypeDef header;
+    header.IDE = CAN_ID_STD;
+    header.RTR = CAN_RTR_DATA;
+    header.StdId = 0x444;
+    header.DLC = 1;
+
+    uint8_t data[32] = {};
+    uint32_t mailbox = 0;
+
+    // sending SOC, SOH and number of cells
+    uint16_t id = 0x101;
+    memcpy(data, &id, sizeof(id));
+    memcpy(data + 2, &packState.SoC, sizeof(packState.SoC));
+    memcpy(data + 6, &packState.SoCCapacityAh, sizeof(packState.SoCCapacityAh));
+    memcpy(data + 10, &generalConfig->noOfCellsSeries, sizeof(generalConfig->noOfCellsSeries));
+    header.DLC = 11;
+    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
+
+    // sending charge current, discharge current // TODO
+    id = 0x102;
+    memcpy(data, &id, sizeof(id));
+    memcpy(data + 2, &packState.packCurrent, sizeof(packState.packCurrent));
+    memcpy(data + 6, &packState.loCurrentLoadCurrent, sizeof(packState.loCurrentLoadCurrent));
+    header.DLC = 10;
+    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
+
+    // sending BMS state, input state, output state // TODO
+    id = 0x103;
+    memcpy(data, &id, sizeof(id));
+    memcpy(data + 2, &packState.cellVoltageLow, sizeof(packState.cellVoltageLow));
+    memcpy(data + 6, &packState.cellVoltageAverage, sizeof(packState.cellVoltageAverage));
+    memcpy(data + 10, &packState.cellVoltageHigh, sizeof(packState.cellVoltageHigh));
+    header.DLC = 12;
+    HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
+
+    // sending cell voltages
+    id = 0x200;
+    for (int cellPointer = 0;
+         cellPointer < generalConfig->noOfCellsSeries * generalConfig->noOfParallelModules; ++cellPointer) {
+        ++id;
+        memcpy(data, &id, sizeof(id));
+        float voltage = 0;
+        if (packState.cellVoltagesIndividual[cellPointer].cellBleedActive)
+            voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * -1000;
+        else
+            voltage = packState.cellVoltagesIndividual[cellPointer].cellVoltage * 1000;
+        memcpy(data + 2, &voltage, sizeof(voltage));
+        header.DLC = 6;
+        HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
+    }
+
+    // sending temperatures
+    id = 0x300;
+    for (int sensorPointer = 0; sensorPointer < NoOfTempSensors; ++sensorPointer) {
+        ++id;
+        memcpy(data, &id, sizeof(id));
+        float temperature = packState.temperatures[sensorPointer];
+        memcpy(data + 2, &temperature, sizeof(temperature));
+        header.DLC = 6;
+        HAL_CAN_AddTxMessage(&hcan1, &header, data, &mailbox);
+    }
+
+    can1_transmit_clock = TIM_Clock;
+}
+
+int getIndexBySoc() {
+    const float soc = packState.SoC;
+    if (soc <= 0) {
+        return 0;
+    } else if (soc <= 10 && soc > 0) {
+        return 1;
+    } else if (soc <= 20 && soc > 10) {
+        return 2;
+    } else if (soc <= 30 && soc > 20) {
+        return 3;
+    } else if (soc <= 40 && soc > 30) {
+        return 4;
+    } else if (soc <= 50 && soc > 40) {
+        return 5;
+    } else if (soc <= 60 && soc > 50) {
+        return 6;
+    } else if (soc <= 70 && soc > 60) {
+        return 7;
+    } else if (soc <= 80 && soc > 70) {
+        return 8;
+    } else if (soc <= 95 && soc > 80) {
+        return 9;
+    } else if (soc > 95) {
+        return 10;
+    }
+
+    return -1;
+}
+
+int getIndexByTemperature() {
+    const float temp = packState.tempBatteryAverage;
+    if (temp < 0) {
+        return 0;
+    } else if (temp < 2 && temp >= 0) {
+        return 1;
+    } else if (temp < 7 && temp >= 2) {
+        return 2;
+    } else if (temp < 15 && temp >= 7) {
+        return 3;
+    } else if (temp < 20 && temp >= 15) {
+        return 4;
+    } else if (temp < 45 && temp >= 20) {
+        return 5;
+    } else if (temp < 50 && temp >= 45) {
+        return 6;
+    } else if (temp < 55 && temp >= 50) {
+        return 7;
+    } else if (temp >= 55) {
+        return 8;
+    }
+
+    return -1;
+}
+
+u16_t get_table_current() {
+    // sending table current and end-of-charge current command
+    const int socIndex = getIndexBySoc();
+    const int temperatureIndex = getIndexByTemperature();
+    float tableCurrent = 0;
+    if (socIndex != -1 && temperatureIndex != -1) {
+        float tableValue = generalConfig->externalChargeCurrentTable[temperatureIndex][socIndex];
+        float pureCurrent = generalConfig->externalChargeUnitTable[temperatureIndex][socIndex];
+        return (u16_t) roundf(pureCurrent ? tableValue : generalConfig->batteryCapacity * tableValue);
+    }
+    return 0;
+}
+
+void can2_send_status() {
+    // id - CAN_BMS_STATUS
+    // len - 7
+    // msg[0:0] = 0: permission, 1: end_charge, 2: charging
+    // msg[1:2] = set_curr
+    // msg[3:6] = end_voltage
+
+    u8_t b0 = 0;
+    b0 |= (packState.chargeAllowed & 0b1) << 0; // bit 0
+    b0 |= ((packState.SoC > 95) & 0b1) << 1;    // bit 1
+    b0 |= (charge_switch_state & 0b1) << 2;     // bit 2
+
+    u16_t curr = get_table_current();
+    const float endVoltage = generalConfig->cellHardOverVoltage * (float) generalConfig->noOfCellsSeries;
+
+    can2_buf.id = CAN_BMS_STATUS;
+    can2_buf.len = 7;
+    can2_buf.msg[0] = b0;   // set msg[0]
+    memcpy((void *) can2_buf.msg + sizeof(b0), &curr, sizeof(curr));               // set msg[1]
+    memcpy((void *) can2_buf.msg + sizeof(b0) + sizeof(curr), &endVoltage, sizeof(endVoltage)); // set msg[3]
+    RingBuf_CellPut(&can2_buf, &can2_ring);
+}
+
+void can2_send_volt_threshold() {
+    // id - CAN_BMS_VOLT_THRESH
+    // len - 8
+    // msg[0:3] = start_voltage
+    // msg[4:7] = end_voltage
+
+    // TODO: Check it
+    const float startVoltage = generalConfig->cellHardUnderVoltage * (float) generalConfig->noOfCellsSeries;
+    const float endVoltage = generalConfig->cellHardOverVoltage * (float) generalConfig->noOfCellsSeries;
+
+    can2_buf.id = CAN_BMS_VOLT_THRESH;
+    can2_buf.len = 8;
+    memcpy((void *) can2_buf.msg, &startVoltage, sizeof(startVoltage));
+    memcpy((void *) can2_buf.msg + sizeof(startVoltage), &endVoltage, sizeof(endVoltage));
+    RingBuf_CellPut(&can2_buf, &can2_ring);
+}
+
+void can2_send_volt_alarm() {
+    // id - CAN_BMS_VOLT_ALARM
+    // len - 8
+    // msg[0:3] = volt_alarm_lo
+    // msg[4:7] = volt_alarm_up
+
+    // TODO: Check it
+    const float startFaultVoltage = generalConfig->cellLCSoftUnderVoltage * (float) generalConfig->noOfCellsSeries;
+    const float endFaultVoltage = generalConfig->cellSoftOverVoltage * (float) generalConfig->noOfCellsSeries;
+
+    can2_buf.id = CAN_BMS_VOLT_ALARM;
+    can2_buf.len = 8;
+    memcpy((void *) can2_buf.msg, &startFaultVoltage, sizeof(startFaultVoltage));
+    memcpy((void *) can2_buf.msg + sizeof(startFaultVoltage), &endFaultVoltage, sizeof(endFaultVoltage));
+    RingBuf_CellPut(&can2_buf, &can2_ring);
+}
+
+void can2_send_current() {
+    // id - CAN_BMS_CURRENT
+    // len - 8
+    // msg[0:1] = [u16] start_current
+    // msg[2:3] = [u16] start_current_interval
+    // msg[4:5] = [u16] set_current
+    // msg[6:7] = [u16] end_current
+
+    u16_t chargeStartingCurrent = 15; // TODO move to generalConfig
+    u16_t chargeStartingCurrentInterval = 20; // in seconds TODO move to generalConfig
+    u16_t curr = get_table_current();
+    u16_t chargeEndingCurrent = 5; // TODO move to generalConfig
+
+    can2_buf.id = CAN_BMS_CURRENT;
+    can2_buf.len = 8;
+    memcpy((void *) can2_buf.msg, &chargeStartingCurrent, sizeof(chargeStartingCurrent));
+    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent),
+           &chargeStartingCurrentInterval,
+           sizeof(chargeStartingCurrentInterval));
+    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent) + sizeof(chargeStartingCurrentInterval),
+           &curr,
+           sizeof(curr));
+    memcpy((void *) can2_buf.msg + sizeof(chargeStartingCurrent) + sizeof(chargeStartingCurrentInterval) + sizeof(curr),
+           &chargeEndingCurrent,
+           sizeof(chargeEndingCurrent));
+
+    RingBuf_CellPut(&can2_buf, &can2_ring);
+}
+
+void can2_send_active() {
+    // id - CAN_BMS_ACT_VAL
+    // len - 6
+    // msg[0:3] = [float] sum_voltage
+    // msg[4:5] = [u16] sum_current
+
+    can2_buf.id = CAN_BMS_ACT_VAL;
+    can2_buf.len = 6;
+    memcpy((void *) can2_buf.msg, &packState.packVoltage, sizeof(packState.packVoltage));
+    memcpy((void *) can2_buf.msg + sizeof(packState.packVoltage),
+           &packState.packCurrent, sizeof(packState.packCurrent));
+    RingBuf_CellPut(&can2_buf, &can2_ring);
+}
+
+
+void transmitCan2Packet() {
+    if (threshold_request) {
+        // Send 501 - status, 503 - volt. threshold, 504 - volt. alarm, 505 - current
+        can2_send_status();         // 501
+        can2_send_volt_threshold(); // 503
+        can2_send_volt_alarm();     // 504
+        can2_send_current();        // 505
+
+        threshold_request = 0;
+    }
+    if (active_request){
+        // send 507 - active values
+        can2_send_active();
+        active_request = 0;
+    }
+
+    if ((TIM_Clock - can2_transmit_clock) < 10) {
+        return;
+    }
+    can2_transmit_clock = TIM_Clock;
+
+    can2_send_status();
+}
+
+void transmitCanPacketFromQueueCan1(void) {
+    if (!HAL_CAN_GetTxMailboxesFreeLevel(&hcan1)) return; // if no free mailboxes
+    u16_t len = 0;
+    RingBuf_Available(&len, &can1_ring);
+    if (!len) return;
+
+    RingBuf_CellRead(&can1_tx_buf, &can1_ring);
+    CAN_Transmit(can1_tx_buf.id, can1_tx_buf.msg, can1_tx_buf.len, &hcan1);
+}
+
+u32_t ctr;
+
+void transmitCanPacketFromQueueCan2(void) {
+    if (!HAL_CAN_GetTxMailboxesFreeLevel(&hcan2)) return; // if no free mailboxes
+    u16_t len = 0;
+    RingBuf_Available(&len, &can2_ring);
+    if (!len) return;
+
+//    volatile u32_t delta = HAL_GetTick() - ctr;
+//    ctr = HAL_GetTick();
+//
+//    __NOP();
+
+    RingBuf_CellRead(&can2_tx_buf, &can2_ring);
+    CAN_Transmit(can2_tx_buf.id, can2_tx_buf.msg, can2_tx_buf.len, &hcan2);
+}
+
+
+void can2_receive_from_irq(CAN_RxHeaderTypeDef *hdr, uint8_t *data) {
+    // If remote frame with STD ID
+    if (hdr->RTR == CAN_RTR_REMOTE && hdr->IDE == CAN_ID_STD) {
+        if (hdr->StdId == CAN_BMS_THRESH_REQ) {  // if Threshold request
+            threshold_request = 1;
+        } else if (hdr->StdId == CAN_BMS_ACT_REQ) {  // if Active request
+            active_request = 1;
+        }
+    }
+}
\ No newline at end of file
diff --git a/firmware/Libs/CAN/can_messenger.h b/firmware/Libs/CAN/can_messenger.h
new file mode 100644
index 0000000..7d24eb8
--- /dev/null
+++ b/firmware/Libs/CAN/can_messenger.h
@@ -0,0 +1,38 @@
+//
+// Created by enik on 03.04.23.
+//
+
+#ifndef BMS_V3_CAN_MESSENGER_H
+#define BMS_V3_CAN_MESSENGER_H
+
+#include "../Core/Inc/main.h"
+
+/**
+ * @brief CAN BMS ID description enum
+ */
+typedef enum CAN_BMS_ID {
+    CAN_BMS_HEARTBEAT = 0x500U,     ///< [BMS->Charger][RTR] Heartbeat
+    CAN_BMS_STATUS  = 0x501U,       ///< [BMS->Charger][L:7] Status packet
+    CAN_BMS_THRESH_REQ = 0x502U,    ///< [Charger->BMS][RTR] Threshold request
+    CAN_BMS_VOLT_THRESH = 0x503U,   ///< [BMS->Charger][L:8] Voltage threshold
+    CAN_BMS_VOLT_ALARM = 0x504U,    ///< [BMS->Charger][L:8] Voltage alarm
+    CAN_BMS_CURRENT = 0x505U,       ///< [BMS->Charger][L:6] Current control
+    CAN_BMS_ACT_REQ = 0x506U,       ///< [Charger->BMS][RTR] Active values request
+    CAN_BMS_ACT_VAL = 0x507U,       ///< [BMS->Charger][L:6] Active values
+} CAN_BMS_ID;
+
+void can_msgr_init();
+
+void transmitCan1Packet();
+int getIndexBySoc();
+int getIndexByTemperature();
+void transmitCan2Packet();
+void addCanPacketToQueue(uint16_t id, uint8_t len, uint8_t *data, uint8_t *queue, uint16_t *queueSize);
+//void transmitCanPacketFromQueue(uint8_t* queue, uint16_t* queueSize, CAN_HandleTypeDef* hcan);
+
+void transmitCanPacketFromQueueCan1(void);
+void transmitCanPacketFromQueueCan2(void);
+
+void can2_receive_from_irq(CAN_RxHeaderTypeDef *hdr, uint8_t* data);
+
+#endif //BMS_V3_CAN_MESSENGER_H
diff --git a/firmware/Libs/RingBuffer/RingBuffer.c b/firmware/Libs/RingBuffer/RingBuffer.c
new file mode 100644
index 0000000..4a588ff
--- /dev/null
+++ b/firmware/Libs/RingBuffer/RingBuffer.c
@@ -0,0 +1,236 @@
+/**
+ *******************************************
+ * @file    RingBuffer.c
+ * @author  Dmitriy Semenov / Crazy_Geeks
+ * @version 1.2
+ * @date	05-March-2022
+ * @brief   Source file for RingBuffer lib
+ * @note    https://crazygeeks.ru/c-ringbuffer/
+ *******************************************
+ */
+
+#include "RingBuffer.h"
+
+/**
+ * @addtogroup RING_BUF
+ * @{
+ */
+
+/**
+ * @brief Init ring buffer
+ *
+ * @param[in] buf Pointer to the allocated buffer
+ * @param[in] size Size of buffer
+ * @param[in] cellsize Size of 1 cell [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_Init(void *buf, u16_t size, size_t cellsize, RINGBUF_t *rb) {
+    rb->size = size; // size of array
+    rb->cell_size = cellsize; // size of 1 cell of array
+    rb->buf = buf;      // set pointer to buffer
+    RingBuf_Clear(rb); // clear all
+    return rb->buf ? RINGBUF_OK : RINGBUF_PARAM_ERR;
+}
+
+/**
+ * @brief Clear ring buffer
+ * @note Disable interrupts while clearing
+ *
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_Clear(RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    rb->head = rb->tail = 0;
+    return RINGBUF_OK;
+}
+
+/**
+ * @brief Check available size to read
+ *
+ * @param[out] len Size to read [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_Available(u16_t *len, RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    if (rb->head < rb->tail)
+        *len = rb->size - rb->tail + rb->head;
+    else
+        *len = rb->head - rb->tail;
+    return RINGBUF_OK;
+}
+
+/**
+ * @brief Put byte to the buffer
+ *
+ * @param[in] data Data byte to be put [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_BytePut(const u8_t data, RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    rb->buf[rb->head++] = data; // put byte in cell and increment data
+    if (rb->head >= rb->size) // if overflow
+        rb->head = 0;   // set to start
+    return RINGBUF_OK;
+}
+
+/**
+ * @brief Put 1 cell to the buffer
+ * @param[in] data Pointer to data
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_CellPut(const void *data, RINGBUF_t *rb) {
+    return RingBuf_DataPut(data, 1, rb);
+}
+
+/**
+ * @brief Put some data to the buffer
+ *
+ * @param[in] data Data to be put
+ * @param[in] len Length of data to be written [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_DataPut(const void *data, u16_t len, RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    if (len > rb->size)
+        return RINGBUF_OVERFLOW;
+    const char *input = data; // recast pointer
+    // INPUT data index start address
+    size_t s_addr = 0;
+    // available space in the end of buffer
+    size_t space = rb->size - rb->head;
+    if (len > space) { // if len > available space
+        // copy data to available space
+        memcpy(&rb->buf[rb->head*rb->cell_size], &input[s_addr * rb->cell_size], space * rb->cell_size);
+        // next writing will start from 0
+        rb->head = 0;
+        // new start address = space length
+        s_addr = space;
+        // new length = len-space
+        len -= space;
+    }
+    // copy all the data to the buf storage
+    memcpy(&rb->buf[rb->head*rb->cell_size], &input[s_addr * rb->cell_size], len * rb->cell_size);
+    // shift to the next head
+    rb->head += len;
+    if (rb->head >= rb->size)
+        rb->head = 0;
+    return RINGBUF_OK;
+}
+
+/**
+ * @brief Read next byte from the buffer
+ *
+ * @param[out] data Data byte from the buffer
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_ByteRead(u8_t *data, RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    RINGBUF_STATUS st = RingBuf_ByteWatch(data, rb);
+    if (st != RINGBUF_OK)
+        return st;
+    rb->tail++;
+    if (rb->tail >= rb->size)
+        rb->tail = 0;
+    return st;
+}
+
+/**
+ * @brief Read 1 cell from buf
+ * @param[out] data Data cell from the buffer
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_CellRead(void *data, RINGBUF_t *rb) {
+    return RingBuf_DataRead(data, 1, rb);
+}
+
+/**
+ * @brief Read some next data from the buffer
+ *
+ * @param[out] data Data from the buffer
+ * @param[in] len Length of data to be read [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_DataRead(void *data, u16_t len, RINGBUF_t *rb) {
+    if (rb->buf == NULL) return RINGBUF_PARAM_ERR;
+    // read data
+    RINGBUF_STATUS st = RingBuf_DataWatch(data, len, rb);
+    if (st != RINGBUF_OK)
+        return st;
+    // shift to the next head
+    rb->tail += len;
+    if (rb->tail >= rb->size)
+        rb->tail = 0;
+    return st;
+}
+
+/**
+ * @brief Watch current byte in buf
+ * @note Reads data without shifting in the buffer
+ *
+ * @param[out] data Pointer to data byte got from buffer
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_ByteWatch(u8_t *data, RINGBUF_t *rb) {
+    if (data == NULL) return RINGBUF_PARAM_ERR;
+    *data = rb->buf[rb->tail];
+    return RINGBUF_OK;
+}
+
+/**
+ * @brief Watch 1 cell from buf
+ * @note Reads data without shifting in the buffer
+ *
+ * @param[out] data Pointer to data cell got from buffer
+ * @param[in] #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_CellWatch(void *data, RINGBUF_t *rb) {
+    return RingBuf_DataWatch(data, 1, rb);
+}
+
+/**
+ * @brief Watch current data in the buf
+ * @note Reads data without shifting in the buffer
+ *
+ * @param[out] data Data from buffer
+ * @param[in] len Length of data to be read [bytes]
+ * @param[in] rb #RINGBUF_t structure instance
+ * @return #RINGBUF_STATUS enum
+ */
+RINGBUF_STATUS RingBuf_DataWatch(void *data, u16_t len, RINGBUF_t *rb) {
+    if (data == NULL)
+        return RINGBUF_PARAM_ERR;
+    if (len > rb->size)
+        return RINGBUF_OVERFLOW;
+    // OUTPUT data index start address
+    u16_t s_addr = 0;
+    // available space in the end of buffer
+    u16_t space = rb->size - rb->tail;
+    u16_t loc_tail = rb->tail;
+    if (len > space) { // if len > available space
+        // recast pointer to u8_t
+        // copy data from available space
+        memcpy(&data[s_addr * rb->cell_size], &rb->buf[loc_tail * rb->cell_size], space * rb->cell_size);
+        // next reading will start from 0
+        loc_tail = 0;
+        // new start address - space length
+        s_addr = space;
+        // new length - len-space
+        len -= space;
+    }
+    // copy all the data from the buf storage
+    memcpy(&data[s_addr * rb->cell_size], &rb->buf[loc_tail * rb->cell_size], len * rb->cell_size);
+    return RINGBUF_OK;
+}
+
+/// @} RING_BUF Group
diff --git a/firmware/Libs/RingBuffer/RingBuffer.h b/firmware/Libs/RingBuffer/RingBuffer.h
new file mode 100644
index 0000000..71ab69f
--- /dev/null
+++ b/firmware/Libs/RingBuffer/RingBuffer.h
@@ -0,0 +1,77 @@
+/**
+ *******************************************
+ * @file    RingBuffer.h
+ * @author  Dmitriy Semenov / Crazy_Geeks
+ * @version 1.2
+ * @date	05-March-2022
+ * @brief   Header file for RingBuffer lib
+ * @note    https://crazygeeks.ru/c-ringbuffer/
+ *******************************************
+ */
+#ifndef RING_BUF_H_
+#define RING_BUF_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "libs.h"
+
+/**
+ * @addtogroup RING_BUF
+ * @brief Ring buffer implementation
+ * @{
+ */
+
+/**
+ * @struct RINGBUF_t
+ * @brief Ring buffer unit
+ */
+typedef struct RINGBUF_t{
+    u8_t *buf;			 ///< Storage of the buffer
+    volatile size_t tail; ///< Place of read point [cells]
+    volatile size_t head; ///< Place of write point [cells]
+    volatile size_t size; ///< Size of buffer [cells]
+    volatile size_t cell_size; ///< Size of one cell [bytes]
+} RINGBUF_t;
+
+/**
+ * @enum RINGBUF_STATUS
+ * @brief Ring buf status enum
+ *
+ * RINGBUF_X
+ * X: OK, ERR, PARAM_ERR, OVERFLOW
+ */
+typedef enum RINGBUF_STATUS{
+    RINGBUF_OK,		  ///< Success status
+    RINGBUF_ERR,      ///< Error
+    RINGBUF_PARAM_ERR, ///< Parameter error
+    RINGBUF_OVERFLOW, ///< Buffer overflow
+} RINGBUF_STATUS;
+
+RINGBUF_STATUS RingBuf_Init(void *buf, u16_t size, size_t cellsize, RINGBUF_t *rb); // Init buf
+RINGBUF_STATUS RingBuf_Clear(RINGBUF_t *rb);			 	 // Clear buf
+RINGBUF_STATUS RingBuf_Available(u16_t *len, RINGBUF_t *rb); // Data available
+
+// Put: add data to buffer
+RINGBUF_STATUS RingBuf_BytePut(u8_t data, RINGBUF_t *rb); // Put byte to the buf
+RINGBUF_STATUS RingBuf_CellPut(const void *data, RINGBUF_t *rb); // Put 1 cell to the buf
+RINGBUF_STATUS RingBuf_DataPut(const void *data, u16_t len, RINGBUF_t *rb); // Put data to the buf
+
+// Read: Get data & flush it
+RINGBUF_STATUS RingBuf_ByteRead(u8_t *data, RINGBUF_t *rb); // Read byte from buf
+RINGBUF_STATUS RingBuf_CellRead(void *data, RINGBUF_t *rb); // Read 1 cell from buf
+RINGBUF_STATUS RingBuf_DataRead(void *data, u16_t len, RINGBUF_t *rb); // Read data from buf
+
+// Watch: Get data without flushing
+RINGBUF_STATUS RingBuf_ByteWatch(u8_t *data, RINGBUF_t *rb); // Watch byte from buf
+RINGBUF_STATUS RingBuf_CellWatch(void *data, RINGBUF_t *rb); // Watch 1 cell from buf
+RINGBUF_STATUS RingBuf_DataWatch(void *data, u16_t len, RINGBUF_t *rb); // Watch data form buf
+
+/// @} RING_BUF Group
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* RING_BUF_H_ */
diff --git a/firmware/Libs/Utils/micros.c b/firmware/Libs/Utils/micros.c
new file mode 100755
index 0000000..5970dff
--- /dev/null
+++ b/firmware/Libs/Utils/micros.c
@@ -0,0 +1,49 @@
+//
+// Created by enik on 18.02.2022.
+//
+
+#include "micros.h"
+
+#define    DWT_CYCCNT    *(volatile uint32_t*)0xE0001004
+#define    DWT_CONTROL   *(volatile uint32_t*)0xE0001000
+#define    SCB_DEMCR     *(volatile uint32_t*)0xE000EDFC
+
+static volatile bool dwt_enabled = 0;
+
+/**
+ * @brief Enable DWT Counter
+ */
+void EnableDWT(void){
+    SCB_DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; // allow to use DWT
+    DWT_CONTROL |= DWT_CTRL_CYCCNTENA_Msk;   // Start DWT counter
+    dwt_enabled = 1;
+}
+
+/**
+ * @brief Delay Microseconds
+ * @param[in] val Value in microseconds to delay
+ */
+void DelayMicros(u32_t val){
+    if (!dwt_enabled) EnableDWT();
+    u32_t us_count_tic =  val * (SystemCoreClock / 1000000); // get delay value in ticks
+    DWT->CYCCNT = 0U; // zero counter value
+    while(DWT->CYCCNT < us_count_tic);
+}
+
+/**
+ * @brief Start code measure from this point
+ */
+void StartCodeMeasure(void){
+    if (!dwt_enabled) EnableDWT();
+    DWT_CYCCNT = 0; // zero counter value
+}
+
+/**
+ * @brief Stop code measure in this point
+ * @param[out] micros Value in microseconds
+ */
+void StopCodeMeasure(u32_t *micros){
+    if (!dwt_enabled){ *micros = 0; return; }
+    //*micros = DWT_CYCCNT / SystemCoreClock; // get counter value
+    *micros = DWT_CYCCNT; // get counter value
+}
\ No newline at end of file
diff --git a/firmware/Libs/Utils/micros.h b/firmware/Libs/Utils/micros.h
new file mode 100755
index 0000000..37af2db
--- /dev/null
+++ b/firmware/Libs/Utils/micros.h
@@ -0,0 +1,21 @@
+//
+// Created by enik on 07.03.2022.
+//
+
+#ifndef _MICROS_H
+#define _MICROS_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "libs.h"
+
+void EnableDWT(void);
+void DelayMicros(u32_t val);
+void StartCodeMeasure(void);
+void StopCodeMeasure(u32_t *micros);
+
+#ifdef __cplusplus
+}
+#endif
+#endif // _MICROS_H
diff --git a/firmware/Libs/libs.h b/firmware/Libs/libs.h
new file mode 100755
index 0000000..28c3c59
--- /dev/null
+++ b/firmware/Libs/libs.h
@@ -0,0 +1,29 @@
+/**
+ *******************************************
+ * @file    libs.h
+ * @author  Dmitriy Semenov / Crazy_Geeks
+ * @brief   Internal header for adding sys libs and defines
+ *******************************************
+*/
+
+#ifndef LIBS_H_
+#define LIBS_H_
+
+#include "main.h"    ///< Main project file
+#include <stdlib.h>  ///< Standard library
+#include <stdint.h>  ///< Std types
+#include <stdbool.h> ///< _Bool to bool
+#include <string.h>  ///< Lib for memcpy, strlen, etc
+#include <stdio.h>   ///< Lib for sprintf, printf, etc
+
+typedef uint8_t u8_t; 	///< 8-bit unsigned
+typedef int8_t i8_t;	///< 8-bit signed
+typedef uint16_t u16_t; ///< 16-bit unsigned
+typedef int16_t i16_t;	///< 16-bit signed
+typedef uint32_t u32_t; ///< 32-bit unsigned
+typedef int32_t i32_t;	///< 32-bit signed
+typedef float fl_t;	///< float type
+
+#define __USR_BKPT() __asm__ __volatile__("BKPT")
+
+#endif /* LIBS_H_ */