diff --git a/Core/Src/canlog.c b/Core/Src/canlog.c
index c209540..94fc414 100644
--- a/Core/Src/canlog.c
+++ b/Core/Src/canlog.c
@@ -98,13 +98,20 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
 void vCANLoggerListen(void *argument)
 {
   /* CODE BEGIN */
-  LEDContext *context = (LEDContext*)argument;
+	CAN_HandleTypeDef *hcan = (CAN_HandleTypeDef*)argument;
+	osMessageQueueId_t queue;
+    CanMessage_t message;
 
-  for (;;)
-  {
-    if (osSemaphoreAcquire(context->semaphore, 25U) == osOK) HAL_GPIO_WritePin(context->led->port, context->led->pin, GPIO_PIN_SET);
-    else HAL_GPIO_WritePin(context->led->port, context->led->pin, GPIO_PIN_RESET);
-    /* CODE END */
+	queue = (hcan->Instance == CAN1) ? xCAN1RxQueue : xCAN2RxQueue;
+
+	for (;;)
+	{
+	  if (osMessageQueueGet(queue, &message, NULL, osWaitForever) == osOK)
+	    {
+	      osSemaphoreRelease( (hcan->Instance == CAN1) ? xSemaphoreCAN1 : xSemaphoreCAN2 );
+	    }
+	}
+	/* CODE END */
 }
 /* END vCANLoggerListen */