添加了大量调试log，新增了dwt计时宏，增加了USB软件复位防止主控复位后上位机无法连接usb

application/cmd/robot_cmd.c

@@ -8,6 +8,9 @@
 #include "message_center.h"
 #include "general_def.h"
 #include "dji_motor.h"
+// bsp
+#include "bsp_dwt.h"
+#include "bsp_log.h"
 
 // 私有宏,自动将编码器转换成角度值
 #define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) // 对齐时的角度,0-360
@@ -266,12 +269,14 @@ static void EmergencyHandler()
         shoot_cmd_send.shoot_mode = SHOOT_OFF;
         shoot_cmd_send.friction_mode = FRICTION_OFF;
         shoot_cmd_send.load_mode = LOAD_STOP;
+        LOGERROR("[CMD] emergency stop!");
     }
     // 遥控器右侧开关为[上],恢复正常运行
     if (switch_is_up(rc_data[TEMP].rc.switch_right))
     {
         robot_state = ROBOT_READY;
         shoot_cmd_send.shoot_mode = SHOOT_ON;
+        LOGINFO("[CMD] reinstate, robot ready");
     }
 }
 

application/robot_task.h

@@ -13,6 +13,8 @@
 #include "daemon.h"
 #include "HT04.h"
 
+#include "bsp_log.h"
+
 osThreadId insTaskHandle;
 osThreadId robotTaskHandle;
 osThreadId motorTaskHandle;
@@ -54,12 +56,15 @@ void StartINSTASK(void const *argument)
 {
     static float ins_start, ins_dt;
     INS_Init(); // 确保BMI088被正确初始化.
+    LOGINFO("[freeRTOS] INS Task Start");
     while (1)
     {
         // 1kHz
         ins_start = DWT_GetTimeline_ms();
         INS_Task();
         ins_dt = DWT_GetTimeline_ms() - ins_start;
+        if (ins_dt > 1)
+            LOGERROR("[freeRTOS] INS Task is being DELAY! dt = [%f]", &ins_dt);
         VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织
         osDelay(1);
     }
@@ -68,21 +73,30 @@ void StartINSTASK(void const *argument)
 void StartMOTORTASK(void const *argument)
 {
     static float motor_dt, motor_start;
+    LOGINFO("[freeRTOS] MOTOR Task Start");
     while (1)
     {
         motor_start = DWT_GetTimeline_ms();
         MotorControlTask();
         motor_dt = DWT_GetTimeline_ms() - motor_start;
+        if (motor_dt > 1)
+            LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
         osDelay(1);
     }
 }
 
 void StartDAEMONTASK(void const *argument)
 {
+    static float daemon_dt, daemon_start;
+    LOGINFO("[freeRTOS] Daemon Task Start");
     while (1)
     {
         // 100Hz
+        daemon_start = DWT_GetTimeline_ms();
         DaemonTask();
+        daemon_dt = DWT_GetTimeline_ms() - daemon_start;
+        if (daemon_dt > 10)
+            LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);
         osDelay(10);
     }
 }
@@ -90,19 +104,24 @@ void StartDAEMONTASK(void const *argument)
 void StartROBOTTASK(void const *argument)
 {
     static float robot_dt, robot_start;
+    LOGINFO("[freeRTOS] ROBOT core Task Start");
     // 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
     while (1)
     {
         robot_start = DWT_GetTimeline_ms();
         RobotTask();
         robot_dt = DWT_GetTimeline_ms() - robot_start;
+        if (robot_dt > 5)
+            LOGERROR("[freeRTOS] ROBOT core Task is being DELAY! dt = [%f]", &robot_dt);
         osDelay(5);
     }
 }
 
 void StartUITASK(void const *argument)
 {
+    LOGINFO("[freeRTOS] UI Task Start");
     MyUIInit();
+    LOGINFO("[freeRTOS] UI Init Done, communication with ref has established");
     while (1)
     {
         // 每给裁判系统发送一包数据会挂起一次,详见UITask函数的refereeSend()