diff --git a/Core/Application/MAIN_MainApplication.c b/Core/Application/MAIN_MainApplication.c
index 2a35e5d..f550b75 100644
--- a/Core/Application/MAIN_MainApplication.c
+++ b/Core/Application/MAIN_MainApplication.c
@@ -19,7 +19,7 @@
 //
 //-------------------------------------------------------------------------------------------------
 //
-// Description:   TEC - Peltier Controller
+// Description:   TEC - Peltier Controller Communication handler
 //
 //=================================================================================================
 
@@ -64,10 +64,6 @@
 
 #define EVENT_FLAGS_ALL             ( EVENT_NEW_MESSAGE | EVENT_TIMER_UPDATE )
 
-#define MESSAGE_TYPE_READ           0
-#define MESSAGE_TYPE_WRITE          1
-#define MESSAGE_TYPE_COMMAND        2
-
 #define COMMAND_NONE                0
 #define COMMAND_ON                  1
 #define COMMAND_OFF                 2
@@ -226,13 +222,11 @@ PRIVATE VOID vTask( PVOID arg )
 		
 		if( u32Flags & EVENT_NEW_MESSAGE ) {
 
-      // TODO: implement new protocol
-
 			CAND_Message stMessage;
       osMessageQueueGet( m_pstCANRxMsgQueueID, &stMessage, NULL, 0 );
 
       U8 u8Register = stMessage.au8Data[0];
-      U8 u8Typ = stMessage.au8Data[1];
+      U8 u8Typ = stMessage.u8Typ;
 
       if( u8Typ == MESSAGE_TYPE_READ ){
         VARH_UVariable uVariable = VARH_uGetVariableData(u8Register);
@@ -244,7 +238,7 @@ PRIVATE VOID vTask( PVOID arg )
         CAND_boSendMessage( au8Buffer, 6 );
       } else if( u8Typ == MESSAGE_TYPE_WRITE ){
         VARH_UVariable uData;
-        UTIL_vMemCopy(&stMessage.au8Data[2], &uData, 4);
+        UTIL_vMemCopy(&stMessage.au8Data[1], &uData, 4);
 
         VARH_vSetVariableData(u8Register, uData);
       } else if( u8Typ == MESSAGE_TYPE_COMMAND ){
@@ -258,7 +252,7 @@ PRIVATE VOID vTask( PVOID arg )
         case COMMAND_OFF:
           PECO_Enable( FALSE );
           break;
-        
+          // TODO: Implement watchdog
         default:
           break;
         }
diff --git a/Core/Drivers/CAND_CanDriver.c b/Core/Drivers/CAND_CanDriver.c
index dd5b2fa..cedf0f8 100644
--- a/Core/Drivers/CAND_CanDriver.c
+++ b/Core/Drivers/CAND_CanDriver.c
@@ -50,12 +50,11 @@
 //=================================================================================================
 
 #define CAND_SHIFT 0x0 // Shift address space (0-3)
-#define CAND_BOARD_GROUP 0x03 // Board group (0-127 / 0x00-0x7F)
 
 #define CAND_PRIVATE 1
 #define CAND_PUBLIC 0
 
-#define MASTER_ID ((CAND_SHIFT<<6) | 0x0000)
+#define MASTER_ID ((CAND_SHIFT<<8) | 0x0000)
 
 //=================================================================================================
 // Section:       MACROS
@@ -82,14 +81,27 @@
 //=================================================================================================
 CAND_pfnRxCallback m_pfnRxCallback = NULL;
 
-CAN_FilterTypeDef stFilter = {
-	(CAND_SHIFT<<6)|(CAND_PUBLIC<<5)|(CAND_BOARD_GROUP<<1), // FilterIdHigh
-	0x00, // FilterIdLow
-	(CAND_SHIFT<<6)|(CAND_PRIVATE<<5)|(CAND_BOARD_GROUP<<1), // FilterMaskIdHigh
-	0x00, // FilterMaskIdLow
+CAN_FilterTypeDef stFilter_public = {
+	(CAND_SHIFT<<6)|(CAND_PUBLIC<<5), // FilterIdHigh
+	0x0000, // FilterIdLow
+	0xC000, // FilterMaskIdHigh
+	0x0000, // FilterMaskIdLow
 	CAN_FILTER_FIFO0, // FilterFIFOAssignment
 	0x00, // FilterBank
-	CAN_FILTERMODE_IDLIST, // FilterMode
+	CAN_FILTERMODE_IDMASK, // FilterMode
+	CAN_FILTERSCALE_32BIT, // FilterScale
+	CAN_FILTER_ENABLE, // FilterActivation
+	0x00 // SlaveStartFilterBank
+};
+
+CAN_FilterTypeDef stFilter_private = {
+	(CAND_SHIFT<<6)|(CAND_PRIVATE<<5), // FilterIdHigh
+	0x0000, // FilterIdLow
+	0xC1C0, // FilterMaskIdHigh
+	0x0000, // FilterMaskIdLow
+	CAN_FILTER_FIFO0, // FilterFIFOAssignment
+	0x01, // FilterBank
+	CAN_FILTERMODE_IDMASK, // FilterMode
 	CAN_FILTERSCALE_32BIT, // FilterScale
 	CAN_FILTER_ENABLE, // FilterActivation
 	0x00 // SlaveStartFilterBank
@@ -140,12 +152,13 @@ BOOL CAND_boInitializeModule( VOID )
 	u8BoardId |= DIPO_boGetInput(DIPO_eADR0) << 0;
 	u8BoardId |= DIPO_boGetInput(DIPO_eADR1) << 1;
 	u8BoardId |= DIPO_boGetInput(DIPO_eADR2) << 2;
-	//u8BoardId |= DIPO_boGetInput(DIPO_eADR0) << 3; // TODO: New HW
+	u8BoardId |= DIPO_boGetInput(DIPO_eADR3) << 3;
 
-	stFilter.FilterIdHigh |= u8BoardId >> 3;
-	stFilter.FilterIdLow |= u8BoardId << 5;
+	stFilter_private.FilterIdHigh |= u8BoardId >> 3;
+	stFilter_private.FilterIdLow |= u8BoardId << 5;
 
-	boOK &= ( HAL_CAN_ConfigFilter(&hcan1, &stFilter) == HAL_OK ) ? TRUE : FALSE;
+	boOK &= ( HAL_CAN_ConfigFilter(&hcan1, &stFilter_public) == HAL_OK ) ? TRUE : FALSE;
+	boOK &= ( HAL_CAN_ConfigFilter(&hcan1, &stFilter_private) == HAL_OK ) ? TRUE : FALSE;
 	
 	boOK &= ( HAL_CAN_Start(&hcan1) == HAL_OK ) ? TRUE : FALSE;
 	
@@ -215,7 +228,8 @@ void HAL_CAN_RxFifo0MsgPendingCallback( CAN_HandleTypeDef *hcan ){
 	HAL_CAN_GetRxMessage( hcan, CAN_RX_FIFO0, &header, au8Data );
 	
 	CAND_Message stMessage;
-	
+
+	stMessage.u8Typ = (header.Id & 0x030) >> 4;	
 	stMessage.u8Len = header.DLC;
 	UTIL_vMemCopy( au8Data,stMessage.au8Data,stMessage.u8Len );
 	
diff --git a/Core/Drivers/CAND_CanDriver.h b/Core/Drivers/CAND_CanDriver.h
index 24219be..68dfda1 100644
--- a/Core/Drivers/CAND_CanDriver.h
+++ b/Core/Drivers/CAND_CanDriver.h
@@ -40,7 +40,9 @@ extern "C" {
 // Description:   Definition of global constants (visible by all modules).
 //=================================================================================================
 
-
+#define MESSAGE_TYPE_WRITE          2
+#define MESSAGE_TYPE_READ           1
+#define MESSAGE_TYPE_COMMAND        0
 
 //=================================================================================================
 // Section:       MACROS
@@ -64,6 +66,7 @@ extern "C" {
 typedef struct {
 	U8 au8Data[8];
 	U8 u8Len;
+	U8 u8Type;
 } CAND_Message;
 
 // callback functions
diff --git a/Core/Drivers/DIPO_DigitalPorts.c b/Core/Drivers/DIPO_DigitalPorts.c
index a4496b9..b3fc983 100644
--- a/Core/Drivers/DIPO_DigitalPorts.c
+++ b/Core/Drivers/DIPO_DigitalPorts.c
@@ -99,7 +99,7 @@ LOCAL CONST StDigitalIO m_astInputs[DIPO_eInNumberOfInputs] =
 {
 	
 	{ GPIOB, { GPIO_PIN_7,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },   // 00 DIPO_ePG
-  { GPIOC, { GPIO_PIN_15,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },   // 01 DIPO_eADR2
+  { GPIOC, { GPIO_PIN_15,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },   // 01 DIPO_eADR3
 	{ GPIOB, { GPIO_PIN_4,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },   // 01 DIPO_eADR2
 	{ GPIOB, { GPIO_PIN_3,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },   // 02 DIPO_eADR1
 	{ GPIOA, { GPIO_PIN_15,  GPIO_MODE_INPUT, GPIO_NOPULL, GPIO_SPEED_FREQ_MEDIUM, 0 }, FALSE, TRUE },  // 03 DIPO_eADR0
diff --git a/Core/Drivers/USFL_UserFlash.c b/Core/Drivers/USFL_UserFlash.c
index 239e0cc..2a59249 100644
--- a/Core/Drivers/USFL_UserFlash.c
+++ b/Core/Drivers/USFL_UserFlash.c
@@ -173,7 +173,8 @@ U32 vFindNextFreePointer( void ){
     }
 
     if( u32Pointer >= USERFLASHSIZE ){
-      
+      u32Pointer = 1;
+      break;
     }
   }
   return u32Pointer;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 0b30fe4..6d9f5d0 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -331,7 +331,7 @@ static void MX_CAN1_Init(void)
   hcan1.Init.TimeTriggeredMode = DISABLE;
   hcan1.Init.AutoBusOff = DISABLE;
   hcan1.Init.AutoWakeUp = DISABLE;
-  hcan1.Init.AutoRetransmission = DISABLE;
+  hcan1.Init.AutoRetransmission = ENABLE;
   hcan1.Init.ReceiveFifoLocked = DISABLE;
   hcan1.Init.TransmitFifoPriority = DISABLE;
   if (HAL_CAN_Init(&hcan1) != HAL_OK)
@@ -340,23 +340,6 @@ static void MX_CAN1_Init(void)
   }
   /* USER CODE BEGIN CAN1_Init 2 */
 
-  CAN_FilterTypeDef canfilterconfig;
-
-  canfilterconfig.FilterActivation = CAN_FILTER_ENABLE;
-  canfilterconfig.FilterBank = 10;
-  canfilterconfig.FilterFIFOAssignment = CAN_RX_FIFO0;
-  canfilterconfig.FilterIdHigh = 0x0000;
-  canfilterconfig.FilterIdLow = 0x0000;
-  canfilterconfig.FilterMaskIdHigh = 0x0000;
-  canfilterconfig.FilterMaskIdLow = 0x0000;
-  canfilterconfig.FilterMode = CAN_FILTERMODE_IDMASK;
-  canfilterconfig.FilterScale = CAN_FILTERSCALE_32BIT;
-
-  if (HAL_CAN_ConfigFilter(&hcan1, &canfilterconfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-
   /* USER CODE END CAN1_Init 2 */
 
 }
diff --git a/Makefile b/Makefile
index 8d50331..15cf238 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 ##########################################################################################################################
-# File automatically-generated by tool: [projectgenerator] version: [3.15.2] date: [Fri Jun 03 15:43:56 CEST 2022] 
+# File automatically-generated by tool: [projectgenerator] version: [3.15.2] date: [Tue Jun 21 10:57:30 CEST 2022] 
 ##########################################################################################################################
 
 # ------------------------------------------------
diff --git a/PeltierControllerV3.ioc b/PeltierControllerV3.ioc
index 57763ce..3602458 100644
--- a/PeltierControllerV3.ioc
+++ b/PeltierControllerV3.ioc
@@ -29,7 +29,8 @@ CAN1.BS1=CAN_BS1_3TQ
 CAN1.CalculateBaudRate=1000000
 CAN1.CalculateTimeBit=1000
 CAN1.CalculateTimeQuantum=200.0
-CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,Prescaler
+CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,Prescaler,NART
+CAN1.NART=ENABLE
 CAN1.Prescaler=16
 Dma.ADC1.2.Direction=DMA_PERIPH_TO_MEMORY
 Dma.ADC1.2.Instance=DMA2_Channel3
@@ -237,7 +238,7 @@ ProjectManager.StackSize=0x400
 ProjectManager.TargetToolchain=Makefile
 ProjectManager.ToolChainLocation=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_DAC1_Init-DAC1-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true
+ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-SystemClock_Config-RCC-false-HAL-false,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_DAC1_Init-DAC1-false-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true,8-MX_I2C1_Init-I2C1-false-HAL-true
 RCC.ADCCLockSelection=RCC_ADCCLKSOURCE_SYSCLK
 RCC.ADCFreq_Value=80000000
 RCC.AHBFreq_Value=80000000