diff --git a/Core/Src/cansend.c b/Core/Src/cansend.c
index 79327f2..dbeab88 100644
--- a/Core/Src/cansend.c
+++ b/Core/Src/cansend.c
@@ -92,11 +92,6 @@ static int format_can_message(char *buf, uint8_t source, const CanMessage_t *mes
   */
 void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
 {
-  if (huart->Instance == USART1)
-  {
-    huart->gState = HAL_UART_STATE_READY;
-    osSemaphoreRelease(xUARTDMASemaphore);
-  }
 }
 /* END HAL_UART_TxCpltCallback */
 
@@ -142,22 +137,12 @@ void vUARTLogger(void *argument)
         HAL_UART_Init(&huart1);
       }
 
-      if (osSemaphoreAcquire(xUARTDMASemaphore, queue_timeout) == osOK)
-      {
-        int len = format_can_message(tx_buffer, message.source, &message);
-        if (HAL_UART_Transmit_DMA(&huart1, (uint8_t*)tx_buffer, len) != HAL_OK)
-        {
-          osSemaphoreRelease(xUARTDMASemaphore);
-          DEBUG_PRINT("HAL error, CAN frame NOT sent!\r\n");
-        }
-      }
-      else
-      {
-        DEBUG_PRINT("ERROR: Semaphore timeout! UART might be stuck in BUSY state.\r\n");
-        HAL_UART_Abort(&huart1);
-        HAL_UART_Init(&huart1);
-        osSemaphoreRelease(xUARTDMASemaphore);
-      }
+      osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever);
+
+      int len = format_can_message(tx_buffer, message.source, &message);
+      HAL_UART_Transmit(&huart1, (uint8_t*)tx_buffer, len, 100);
+
+      osSemaphoreRelease(xUARTDMASemaphore);
     }
     else
     {
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 18b83a0..b8d9821 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -87,22 +87,17 @@ int _write(int file, char *ptr, int len)
   return len;
 }
 
-#ifndef DEBUG_ITM
+#ifndef DEBUG_ITW
 #include <stdio.h>
 #include <stdarg.h>
 
 extern osSemaphoreId_t xUARTDMASemaphore;
-extern osMutexId_t debugPrintMutex;
 
 void uart_debug_print(const char *fmt, ...)
 {
   if (huart1.gState == HAL_UART_STATE_RESET) return;
 
-  if (osKernelGetState() == osKernelRunning)
-  {
-    osMutexAcquire(debugPrintMutex, osWaitForever);
-    osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever);
-  }
+  if (osKernelGetState() == osKernelRunning) osSemaphoreAcquire(xUARTDMASemaphore, osWaitForever);
 
   char buf[128];
   va_list args;
@@ -113,11 +108,7 @@ void uart_debug_print(const char *fmt, ...)
 
   if (len > 0) HAL_UART_Transmit(&huart1, (uint8_t*)buf, len, 100);
 
-  if (osKernelGetState() == osKernelRunning)
-  {
-    osSemaphoreRelease(xUARTDMASemaphore);
-    osMutexRelease(debugPrintMutex);
-  }
+  if (osKernelGetState() == osKernelRunning) osSemaphoreRelease(xUARTDMASemaphore);
 }
 #endif
 /* USER CODE END PF */