diff --git a/Core/Src/canlog.c b/Core/Src/canlog.c
index cfbf7e8..71d1ab4 100644
--- a/Core/Src/canlog.c
+++ b/Core/Src/canlog.c
@@ -43,22 +43,6 @@ const osThreadAttr_t vCAN2_rx_attributes = {
   .stack_size = 128 * 4,
   .priority = (osPriority_t) osPriorityRealtime,
 };
-
-/* Definitions for CAN1 LED heartbeat */
-osThreadId_t vLED_CAN1_Heartbeat;
-const osThreadAttr_t vLED_CAN1_Heartbeat_attributes = {
-  .name = "vLED_CAN1_HeartbeatTask",
-  .stack_size = 128 * 4,
-  .priority = (osPriority_t) osPriorityVeryLow1,
-};
-
-/* Definitions for CAN2 LED heartbeat */
-osThreadId_t vLED_CAN2_Heartbeat;
-const osThreadAttr_t vLED_CAN2_Heartbeat_attributes = {
-  .name = "vLED_CAN2_HeartbeatTask",
-  .stack_size = 128 * 4,
-  .priority = (osPriority_t) osPriorityVeryLow,
-};
 /* USER END RTOS_TASKS */
 
 
@@ -121,18 +105,7 @@ void vLED_Heartbeat(void *argument)
 {
   /* CODE BEGIN */
   LED_Config *led = (LED_Config*)argument;
-
-  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 */
 }
 /* END vLED_HeartbeatOnCanRx */
diff --git a/Core/Src/main.c b/Core/Src/main.c
index bd7b2a3..e424652 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 */
@@ -137,8 +144,6 @@ int main(void)
   /* USER CODE BEGIN RTOS_THREADS */
   vCAN1_rx = osThreadNew(vCAN_Logger_Listen, &hcan1, &vCAN1_rx_attributes);
   vCAN2_rx = osThreadNew(vCAN_Logger_Listen, &hcan2, &vCAN2_rx_attributes);
-  vLED_CAN1_Heartbeat = osThreadNew(vLED_Heartbeat, &led_can1, &vLED_CAN1_Heartbeat_attributes);
-  vLED_CAN2_Heartbeat = osThreadNew(vLED_Heartbeat, &led_can2, &vLED_CAN2_Heartbeat_attributes);
   /* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */
@@ -446,10 +451,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