diff --git a/Core/Src/canlog.c b/Core/Src/canlog.c
index cfbf7e8..0d5ddcb 100644
--- a/Core/Src/canlog.c
+++ b/Core/Src/canlog.c
@@ -121,17 +121,11 @@ void vLED_Heartbeat(void *argument)
 {
   /* CODE BEGIN */
   LED_Config *led = (LED_Config*)argument;
+  HAL_GPIO_WritePin(led->port, led->pin, GPIO_RESET);
 
   for(;;)
   {
-    //HAL_GPIO_WritePin(led->port, led->pin, GPIO_PIN_RESET);
-    // if semaphore for canrx, do the rest
-
-
-    HAL_GPIO_WritePin(led->port, led->pin, GPIO_PIN_SET);
-    osDelay(100);
-    HAL_GPIO_WritePin(led->port, led->pin, GPIO_PIN_RESET);
-    osDelay(100);
+    HAL_GPIO_TogglePin(led->port, led->pin);
   }
   /* CODE END */
 }
diff --git a/Core/Src/main.c b/Core/Src/main.c
index bd7b2a3..1bb0aa7 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -56,6 +56,9 @@ DMA_HandleTypeDef hdma_sdio_tx;
 LED_Config led_can1 = {GPIOB, GPIO_PIN_2};
 LED_Config led_can2 = {GPIOB, GPIO_PIN_5};
 LED_Config led_error = {GPIOB, GPIO_PIN_3};
+
+osTimerId_t heartbeat_timer_can1;
+osTimerId_t heartbeat_timer_can2;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -127,7 +130,11 @@ int main(void)
   /* USER CODE END RTOS_SEMAPHORES */
 
   /* USER CODE BEGIN RTOS_TIMERS */
-  /* start timers, add new ones, ... */
+  heartbeat_timer_can1 = osTimerNew(vLED_Heartbeat, osTimerPeriodic, &led_can1, NULL);
+  if (heartbeat_timer_can1 != NULL) osTimerStart(heartbeat_timer_can1, 250U);
+
+  heartbeat_timer_can2 = osTimerNew(vLED_Heartbeat, osTimerPeriodic, &led_can2, NULL);
+  if (heartbeat_timer_can2 != NULL) osTimerStart(heartbeat_timer_can2, 250U);
   /* USER CODE END RTOS_TIMERS */
 
   /* USER CODE BEGIN RTOS_QUEUES */
@@ -446,10 +453,11 @@ void Error_Handler(void)
 
   //TODO: check if it is necessary to stop FreeRTOS
 
-  uint32_t error_code;
+  uint32_t error_code_can1;
+  uint32_t error_code_can2;
 
-  if (HAL_CAN_GetState(&hcan1) != HAL_CAN_STATE_READY) error_code = HAL_CAN_GetError(&hcan1);
-  if (HAL_CAN_GetState(&hcan2) != HAL_CAN_STATE_READY) error_code = HAL_CAN_GetError(&hcan2);
+  if (HAL_CAN_GetState(&hcan1) != HAL_CAN_STATE_READY) error_code_can1 = HAL_CAN_GetError(&hcan1);
+  if (HAL_CAN_GetState(&hcan2) != HAL_CAN_STATE_READY) error_code_can2 = HAL_CAN_GetError(&hcan2);
 
   // TODO: write error to SD and/or serial