Add mutex for debug print collision on UART

Core/Src/main.c

@@ -55,6 +55,8 @@ LED_Config led_can1 = {GPIOB, GPIO_PIN_5};
 LED_Config led_can2 = {GPIOB, GPIO_PIN_6};
 LED_Config led_error = {GPIOB, GPIO_PIN_7};
 
+osMutexId_t debugPrintMutex;
+
 osThreadId_t xCAN1rxTask;
 osThreadId_t xCAN2rxTask;
 osThreadId_t xCAN1LedTask;
@@ -89,10 +91,14 @@ int _write(int file, char *ptr, int len)
 #include <stdio.h>
 #include <stdarg.h>
 
+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);
+
   char buf[128];
   va_list args;
 
@@ -105,6 +111,8 @@ void uart_debug_print(const char *fmt, ...)
     while (huart1.gState != HAL_UART_STATE_READY);
     HAL_UART_Transmit(&huart1, (uint8_t*)buf, len, 100);
   }
+
+  if (osKernelGetState() == osKernelRunning) osMutexRelease(debugPrintMutex);
 }
 #endif
 /* USER CODE END PF */
@@ -194,6 +202,8 @@ int main(void)
   /* USER CODE END RTOS_TASKS */
 
   /* USER CODE BEGIN RTOS_MUTEX */
+  debugPrintMutex = osMutexNew(NULL);
+  if (debugPrintMutex == NULL) Error_Handler();
   /* USER CODE END RTOS_MUTEX */
 
   /* USER CODE BEGIN RTOS_SEMAPHORES */