diff --git a/Software/Core/Inc/main.h b/Software/Core/Inc/main.h
index f529088..f473388 100644
--- a/Software/Core/Inc/main.h
+++ b/Software/Core/Inc/main.h
@@ -47,6 +47,7 @@ extern "C" {
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 
+void Error_Handler_Led(uint8_t err);
 /* USER CODE END EM */
 
 /* Exported functions prototypes ---------------------------------------------*/
diff --git a/Software/Core/Src/main.c b/Software/Core/Src/main.c
index a76b745..e882e71 100644
--- a/Software/Core/Src/main.c
+++ b/Software/Core/Src/main.c
@@ -132,9 +132,11 @@ void loop_1kHz() {
 
       }
 
-      if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK)
-        Error_Handler();
-
+      if (HAL_FDCAN_AddMessageToTxFifoQ(hMainCAN, &txHeader, (uint8_t*) &txData) != HAL_OK) {
+        if(HAL_FDCAN_GetError(hMainCAN) != HAL_FDCAN_ERROR_FIFO_FULL) {
+          Error_Handler_Led(1);
+        }
+      }
     }
 
     if (mscounter >= 500) {
@@ -195,10 +197,10 @@ int main(void)
   hPeriCAN = &hfdcan2;
 
   if (HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(2);
 
   if (HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_values, NUM_ADC_PINS) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(2);
 
   HAL_TIM_Base_Start_IT(&htim6);
 
@@ -215,11 +217,11 @@ int main(void)
   txHeader.DataLength = 8;
 
   if (HAL_FDCAN_ActivateNotification(hMainCAN, FDCAN_IT_RX_FIFO0_NEW_MESSAGE, 0) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(3);
 
   if (HAL_FDCAN_ConfigGlobalFilter(hMainCAN, FDCAN_REJECT, FDCAN_REJECT,
                                     FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(3);
 
   FDCAN_FilterTypeDef filter;
   filter.IdType = FDCAN_STANDARD_ID;
@@ -230,10 +232,10 @@ int main(void)
   filter.FilterID2 = CAN_PWM_FILTER_MASK;
 
   if (HAL_FDCAN_ConfigFilter(hMainCAN, &filter) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(5);
 
   if (HAL_FDCAN_Start(hMainCAN) != HAL_OK)
-    Error_Handler();
+    Error_Handler_Led(5);
 
   // Init all channels as stopped
   memset(pwm_ch_active, 0, 8);
@@ -299,7 +301,7 @@ void SystemClock_Config(void)
   RCC_OscInitStruct.PLL.PLLFRACN = 0;
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
   {
-    Error_Handler();
+    Error_Handler_Led(4);
   }
 
   /** Initializes the CPU, AHB and APB buses clocks
@@ -317,7 +319,7 @@ void SystemClock_Config(void)
 
   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
   {
-    Error_Handler();
+    Error_Handler_Led(4);
   }
 }
 
@@ -458,6 +460,49 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
   HAL_ResumeTick();
 }
 
+void Error_Handler_Led(uint8_t err) {
+  HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_RESET);
+  HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_RESET);
+  switch (err)
+  {
+  case 0: // red: general error
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    break;
+
+  case 1: // red blue: in 1 kHz loop
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    break;
+
+  case 2: // blue: ADC calibration or DMA start
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    break;
+
+  case 3: // green blue: CAN notify and config gloabl filter
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+  case 4: //red green: clock
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+  case 5: // red green blue: CAN config filter and start
+    HAL_GPIO_WritePin(STATUS_R_GPIO_Port, STATUS_R_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_B_GPIO_Port, STATUS_B_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(STATUS_G_GPIO_Port, STATUS_G_Pin, GPIO_PIN_SET);
+    break;
+    
+    default:
+    break;
+  }
+
+  __disable_irq();
+  while (1);
+}
+
 /* USER CODE END 4 */
 
 /**