add serial debug dma support

src/app/inc/debug_task.h

@@ -1,6 +1,8 @@
 #ifndef DEBUG_TASK_H
 #define DEBUG_TASK_H
 
+#define DEBUG_PERIOD (1)
+
 void Debug_Task_Init(void);
 void Debug_Task_Loop(void);
 

src/app/inc/robot_param.h

@@ -1,6 +1,8 @@
 #ifndef ROBOT_PARAM_H
 #define ROBOT_PARAM_H
 
+#define DEBUG
+
 /* Motor ID */
 #define LEFT_FRONT_ID 0x00
 #define RIGHT_FRONT_ID 0x01

src/app/inc/robot_tasks.h

@@ -43,7 +43,7 @@ void Robot_Tasks_Start()
     osThreadDef(ui_task, Robot_Tasks_UI, osPriorityAboveNormal, 0, 256);
     ui_task_handle = osThreadCreate(osThread(ui_task), NULL);
 
-    osThreadDef(debug_task, Robot_Tasks_Debug, osPriorityAboveNormal, 0, 256);
+    osThreadDef(debug_task, Robot_Tasks_Debug, osPriorityIdle, 0, 256);
     debug_task_handle = osThreadCreate(osThread(debug_task), NULL);
 
     osThreadDef(jetson_orin_task, Robot_Tasks_Jetson_Orin, osPriorityAboveNormal, 0, 256);
@@ -98,7 +98,7 @@ void Robot_Tasks_Debug(void const *argument)
 {
     portTickType xLastWakeTime;
     xLastWakeTime = xTaskGetTickCount();
-    const TickType_t TimeIncrement = pdMS_TO_TICKS(40);
+    const TickType_t TimeIncrement = pdMS_TO_TICKS(DEBUG_PERIOD);
     while (1)
     {
         Debug_Task_Loop();

src/app/src/debug_task.c

@@ -19,5 +19,11 @@ extern Launch_Target_t g_launch_target;
 extern uint64_t t;
 void Debug_Task_Loop(void)
 {
-    //printf("a=%f,b=%f\r\n", g_orin_data.receiving.auto_aiming.yaw, g_orin_data.receiving.auto_aiming.pitch);
+    static uint32_t counter = 0;
+    // DEBUG_PRINTF(&huart6, "time=%.1f,a=%f,b=%f,c=%f,d=%d\r\n", (float) counter / 1000.0f * DEBUG_PERIOD, 
+    //             g_imu.deg.yaw, g_imu.deg.pitch, g_imu.deg.roll, counter);
+    counter++;
+    if (counter > 0xFFFFFFFF) {
+        counter = 0;
+    }
 }

src/bsp/Inc/bsp_serial.h

@@ -1,6 +1,15 @@
 #ifndef BSP_SERIAL
 #define BSP_SERIAL
 #include <stdio.h>
+#include "robot_param.h"
+#include "usart.h"
 
+#ifdef DEBUG
+#define DEBUG_PRINTF(huart, format, ...) debug_printf((huart), (format), ##__VA_ARGS__)
+#else
+#define DEBUG_PRINTF(huart, format, ...) ((void)0)
+#endif
+
+void debug_printf(UART_HandleTypeDef *huart, const char *format, ...);
 extern int _write(int file, char *ptr, int len);
 #endif

src/bsp/Src/bsp_serial.c

@@ -1,9 +1,45 @@
 #include "bsp_serial.h"
 
 #include "usart.h"
+#include "stdarg.h"
+#include "stdio.h"
+#include "string.h"
 
+#define MAX_STRING_LENGTH 1028
+
+#ifdef DEBUG
+#define DEBUG_PRINTF(huart, format, ...) debug_printf((huart), (format), ##__VA_ARGS__)
+#else
+#define DEBUG_PRINTF(huart, format, ...) ((void)0)
+#endif
+char buffer[MAX_STRING_LENGTH];
+
+/**
+ * @brief Non-blocking printf function using DMA
+*/
+void debug_printf(UART_HandleTypeDef *huart, const char *format, ...) {
+    va_list args;
+    va_start(args, format);
+
+    // Use vsnprintf instead of vsprintf to avoid buffer overflow
+    vsnprintf(buffer, MAX_STRING_LENGTH, format, args);
+    va_end(args);
+
+    // Send the string over UART using DMA
+    HAL_UART_Transmit_DMA(huart, (uint8_t*)buffer, strlen(buffer));
+    while (__HAL_UART_GET_FLAG(huart, UART_FLAG_TC) == RESET) {
+            // Optionally, insert a timeout mechanism here to break from the loop if needed
+    }
+}
+
+/**
+ * Obsolete function required for printf to work
+ * this function calls HAL_UART_Transmit, which will block the program 
+ * until all data is sent.
+ * @ref debug_printf for a non-blocking version using DMA
+*/
 int _write(int file, char *ptr, int len) {
-    HAL_UART_Transmit(&huart1, (uint8_t *)ptr, len, HAL_MAX_DELAY);
+    HAL_UART_Transmit(&huart6, (uint8_t *)ptr, len, HAL_MAX_DELAY);
     //while(!huart6.Instance->SR & UART_FLAG_IDLE);
     return len;
 }