diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index 950de2d..ea22fd6 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -46,7 +46,7 @@ extern "C" {
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 //#define DEBUG_ITM
-//#define DEBUG_DUMMY_FRAME
+#define DEBUG_DUMMY_FRAME
 
 #ifdef DEBUG_ITM
 #include <stdio.h>
diff --git a/Core/Src/cansend.c b/Core/Src/cansend.c
index b5e50ec..36dc143 100644
--- a/Core/Src/cansend.c
+++ b/Core/Src/cansend.c
@@ -52,8 +52,8 @@ void uart_printf(const char *fmt, ...)
     {
       if (osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever) == osOK)
       {
-        if (HAL_UART_Transmit_DMA(&huart1, (uint8_t*)buf, len) != HAL_OK) osSemaphoreRelease(xUARTDMASemaphore);
-        else osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever);
+        HAL_UART_Transmit(&huart1, (uint8_t*)buf, len, HAL_MAX_DELAY);
+        osSemaphoreRelease(xUARTDMASemaphore);
       }
     }
     else HAL_UART_Transmit(&huart1, (uint8_t*)buf, len, HAL_MAX_DELAY);
@@ -128,7 +128,19 @@ void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
 }
 /* END HAL_UART_TxCpltCallback */
 
-/* BEGIN vUARTLoggerListen */
+/* BEGIN HAL_UART_ErrorCallback */
+/**
+  * @brief  Safely release UART semaphore inside ISR and yield task if possible.
+  * @param  argument: UART handle
+  * @retval None
+  */
+void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
+{
+  if (huart->Instance == USART1) osSemaphoreRelease(xUARTDMASemaphore);
+}
+/* END HAL_UART_ErrorCallback */
+
+/* BEGIN vUARTLogger */
 /**
   * @brief  Send CAN bus traffic saved in FIFO buffer to USART1
   * @param  argument: Not used
@@ -139,43 +151,23 @@ void vUARTLogger(void *argument)
   CanMessage_t message;
   char tx_buffer[45];
 
-  #ifdef DEBUG_DUMMY_FRAME
-  CanMessage_t dummy_frame = {
-    .id = 0x0CF00400,       // J1939 EEC1 ID
-    .dlc = 8,               // 8 bytes of data
-    .isExtended = 1,        // 29-bit Extended ID
-    .source = 1,            // CAN1
-    .payload = {0xAA, 0xBB, 0xCC, 0xDD, 0xEE, 0xFF, 0x11, 0x22}
-  };
-  #endif
-
   for (;;)
   {
     DEBUG_PRINT("[D] Waiting for CAN traffic...\r\n");
 
-    #ifdef DEBUG_DUMMY_FRAME
-    uint32_t queue_timeout = 1000U;
-    #else
-    uint32_t queue_timeout = osWaitForever;
-    #endif
-
-    #ifdef DEBUG_DUMMY_FRAME
-    osMessageQueuePut(xUARTQueue, &dummy_frame, 0U, 0U);
-    osDelay(1000);
-    #endif
-
-   	if (osMessageQueueGet(xUARTQueue, &message, NULL, queue_timeout) == osOK)
-	{
-      if (osSemaphoreAcquire(xUARTDMASemaphore, queue_timeout) == osOK)
+    if (osMessageQueueGet(xUARTQueue, &message, NULL, osWaitForever) == osOK)
+    {
+      if (osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever) == osOK)
       {
         int len = format_can_message(tx_buffer, message.source, &message);
 
-        if (HAL_UART_Transmit_DMA(&huart1, (uint8_t*)tx_buffer, len) != HAL_OK) Error_Handler();
-        else DEBUG_PRINT("[E] HAL error, CAN frame NOT sent!\r\n");
-        osSemaphoreRelease(xUARTDMASemaphore);
+        if (HAL_UART_Transmit_DMA(&huart1, (uint8_t*)tx_buffer, len) != HAL_OK)
+        {
+          osSemaphoreRelease(xUARTDMASemaphore);
+          DEBUG_PRINT("[E] HAL error, CAN frame NOT sent!\r\n");
+        }
       }
-      else DEBUG_PRINT("[E] Semaphore timeout! UART might be stuck in BUSY state.\r\n");
-	}
+    }
   }
 }
-/* END vUARTLoggerListen */
+/* END vUARTLogger */
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 1465aae..f442984 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -76,6 +76,8 @@ static void MX_CAN2_Init(void);
 static void MX_SDIO_SD_Init(void);
 static void MX_USART1_UART_Init(void);
 
+void uart_printf(const char *fmt, ...);
+
 /* USER CODE BEGIN PFP */
 int _write(int file, char *ptr, int len)
 {