diff --git a/Core/Inc/canlog.h b/Core/Inc/canlog.h
index db865b1..0ba9cb4 100644
--- a/Core/Inc/canlog.h
+++ b/Core/Inc/canlog.h
@@ -23,28 +23,25 @@
 #define CANLOG_H
 
 #include "main.h"
+#include "cmsis_os.h"
 
-/* Decladerations for queues */
 extern osMessageQueueId_t xCAN1RxQueue;
 extern osMessageQueueId_t xCAN2RxQueue;
+extern osSemaphoreId_t xLEDSemaphoreCAN1;
+extern osSemaphoreId_t xLEDSemaphoreCAN2;
 
-/* Decladerations for thread handles and attributes */
-extern osThreadId_t vCAN1_rx;
-extern osThreadId_t vCAN2_rx;
-extern osThreadId_t vLED_CAN1_Heartbeat;
-extern osThreadId_t vLED_CAN2_Heartbeat;
-
-extern const osThreadAttr_t vCAN1_rx_attributes;
-extern const osThreadAttr_t vCAN2_rx_attributes;
-extern const osThreadAttr_t vLED_CAN1_Heartbeat_attributes;
-extern const osThreadAttr_t vLED_CAN2_Heartbeat_attributes;
+typedef struct {
+    LED_Config *led;
+    osSemaphoreId_t semaphore;
+    osTimerId_t timer;
+} LEDContext;
 
 /**
   * @brief  Initializes the CAN logger modules, OS threads, queues, and hardware.
   * @param  hcan1 Pointer to the CAN1 handle
   * @param  hcan2 Pointer to the CAN2 handle
   */
-void vCANLoggerInit(CAN_HandleTypeDef *hcan1, CAN_HandleTypeDef *hcan2);
+void CAN_Logger_Init(CAN_HandleTypeDef *hcan1, CAN_HandleTypeDef *hcan2);
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan);
 void vCANLoggerListen(void *argument);
 void vLEDHeartbeat(void *argument);
diff --git a/Core/Src/canlog.c b/Core/Src/canlog.c
index 85a5a6f..509a9b2 100644
--- a/Core/Src/canlog.c
+++ b/Core/Src/canlog.c
@@ -82,12 +82,12 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
     memcpy(message.payload, data, rxHeader.DLC);
 
     osMessageQueueId_t queue = (hcan->Instance == CAN1) ? xCAN1RxQueue : xCAN2RxQueue;
-    if (osMessageQueuePut(queue, &message, 0U, 0U) != osOK) Error_Handler();
-    // TODO: better handling: flash error_led on osTimeout, call Error_Handler() when other errors
-
-    // Issue #1: set an event flag for led task
-    // osEventFlagsSet(xCanEventFlags, (hcan->Instance == CAN1) ? 0x01 : 0x02);
+    if (osMessageQueuePut(queue, &message, 0U, 0U) != osOK)
+    {
+        // TODO: better handling: flash error_led on osTimeout, call Error_Handler() when other errors
 
+        // send errors like queue full via UART
+    }
     /* CODE END */
 }
 /* END HAL_CAN_RxFifo0MsgPendingCallback */
@@ -109,11 +109,13 @@ void vCANLoggerListen(void *argument)
 
     for (;;)
     {
-        if (osMessageQueueGet(queue, &msg, NULL, osWaitForever) == osOK)
+        if (osMessageQueueGet(queue, &message, NULL, osWaitForever) == osOK)
         {
             // TODO: parse message send message to serial/uart
-            // TODO: add rtos semaphore to blink the LED with the timer already set
+
+            osSemaphoreRelease( (hcan->Instance == CAN1) ? xLEDSemaphoreCAN1 : xLEDSemaphoreCAN2 );
         }
+
     }
     /* CODE END */
 }
@@ -128,8 +130,17 @@ void vCANLoggerListen(void *argument)
 void vLEDHeartbeat(void *argument)
 {
   /* CODE BEGIN */
-  LED_Config *led = (LED_Config*)argument;
-  HAL_GPIO_TogglePin(led->port, led->pin);
+  LEDContext *ctx = (LEDContext*)argument;
+
+  if (osSemaphoreAcquire(ctx->sem, 0U) == osOK)
+  {
+      HAL_GPIO_WritePin(ctx->led->port, ctx->led->pin, GPIO_PIN_SET);
+      osTimerStart(ctx->timer, 25U);
+  }
+  else
+  {
+      HAL_GPIO_WritePin(ctx->led->port, ctx->led->pin, GPIO_PIN_RESET);
+  }
   /* CODE END */
 }
 /* END vLEDHeartbeat */
diff --git a/Core/Src/main.c b/Core/Src/main.c
index ce25a02..82611d8 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -23,7 +23,6 @@
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "canlog.h"
-#include <system_error>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -66,8 +65,17 @@ LED_Config led_can1 = {GPIOB, GPIO_PIN_2};
 LED_Config led_can2 = {GPIOB, GPIO_PIN_5};
 LED_Config led_error = {GPIOB, GPIO_PIN_3};
 
+LEDContext ledContextCAN1;
+LEDContext ledContextCAN2;
+
+osSemaphoreId_t xLEDSemaphoreCAN1;
+osSemaphoreId_t xLEDSemaphoreCAN2;
+
 osTimerId_t xHeartbeatTimerCAN1;
 osTimerId_t xHeartbeatTimerCAN2;
+
+osMessageQueueId_t xCAN1RxQueue;
+osMessageQueueId_t xCAN2RxQueue;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -147,24 +155,32 @@ int main(void)
   /* USER CODE END RTOS_MUTEX */
 
   /* USER CODE BEGIN RTOS_SEMAPHORES */
-  /* add semaphores, ... */
+  xLEDSemaphoreCAN1 = osSemaphoreNew(10, 0, NULL);
+  xLEDSemaphoreCAN2 = osSemaphoreNew(10, 0, NULL);
+
+  if (xLEDSemaphoreCAN1 == NULL || xLEDSemaphoreCAN2 == NULL) Error_Handler();
+
+  ledContextCAN1.led = &led_can1;
+  ledContextCAN1.semaphore = xLEDSemaphoreCAN1;
+  ledContextCAN2.led = &led_can2;
+  ledContextCAN2.semaphore = xLEDSemaphoreCAN2;
   /* USER CODE END RTOS_SEMAPHORES */
 
   /* USER CODE BEGIN RTOS_TIMERS */
-  xHeartbeatTimerCAN1 = osTimerNew(vLEDHeartbeat, osTimerPeriodic, &led_can1, NULL);
-  osTimerStart(xHeartbeatTimerCAN1, 250U);
+  xHeartbeatTimerCAN1 = osTimerNew(vLEDHeartbeat, osTimerOnce, &ledContextCAN1, NULL);
+  xHeartbeatTimerCAN2 = osTimerNew(vLEDHeartbeat, osTimerOnce, &ledContextCAN2, NULL);
 
-  xHeartbeatTimerCAN2 = osTimerNew(vLEDHeartbeat, osTimerPeriodic, &led_can2, NULL);
-  osTimerStart(xHeartbeatTimerCAN2, 250U);
+  osTimerStart(xHeartbeatTimerCAN1, 25U);
+  osTimerStart(xHeartbeatTimerCAN2, 25U);
 
   if (xHeartbeatTimerCAN1 == NULL || xHeartbeatTimerCAN2 == NULL) Error_Handler();
+
+  ledContextCAN1.timer = xHeartbeatTimerCAN1;
+  ledContextCAN2.timer = xHeartbeatTimerCAN2;
   /* USER CODE END RTOS_TIMERS */
 
   /* USER CODE BEGIN RTOS_QUEUES */
-  osMessageQueueId_t xCAN1RxQueue;
   xCAN1RxQueue = osMessageQueueNew(32, sizeof(CanMessage_t), NULL);
-
-  osMessageQueueId_t xCAN2RxQueue;
   xCAN2RxQueue = osMessageQueueNew(32, sizeof(CanMessage_t), NULL);
 
   if (xCAN1RxQueue == NULL || xCAN2RxQueue == NULL) Error_Handler();
@@ -179,6 +195,16 @@ int main(void)
   /* add events, ... */
   /* USER CODE END RTOS_EVENTS */
 
+  /* USER CODE BEGIN 3 */
+  ledContextCAN1.led = &led_can1;
+  ledContextCAN1.semaphore = xLEDSemaphoreCAN1;
+  ledContextCAN1.timer = xHeartbeatTimerCAN1;
+
+  ledContextCAN2.led = &led_can2;
+  ledContextCAN2.semaphore = xLEDSemaphoreCAN2;
+  ledContextCAN2.timer = xHeartbeatTimerCAN2;
+  /* USER CODE END 3 */
+
   /* Start scheduler */
   osKernelStart();
 
@@ -190,9 +216,9 @@ int main(void)
   {
     /* USER CODE END WHILE */
 
-    /* USER CODE BEGIN 3 */
+    /* USER CODE BEGIN 4 */
   }
-  /* USER CODE END 3 */
+  /* USER CODE END 4 */
 }
 
 /**
@@ -442,9 +468,9 @@ static void MX_GPIO_Init(void)
   /* USER CODE END MX_GPIO_Init_2 */
 }
 
-/* USER CODE BEGIN 4 */
+/* USER CODE BEGIN 5 */
 
-/* USER CODE END 4 */
+/* USER CODE END 5 */
 
 /**
   * @brief  Period elapsed callback in non blocking mode