refactor: migrate from DJI motors to DM motors, simplify to gimbal-only control

- Rewrite DM motor driver based on DM_6220 reference protocol (MIT/PosVel/Velocity modes) - Use velocity control mode: MCU runs angle PID with IMU feedback, motor handles speed loop - Rewrite vision protocol with 3 frame types (0xA5 auto data, 0xA6 manual data, 0xA7 command) - Decouple serial parsing (opencv module) from mode switching (robot_cmd module) - Implement dual-loop architecture: vision PID outer loop → angle ref → motor PID inner loop - Add KF/PID reset on mode switch transitions - Merge motor_reverse_flag and feedback_reverse_flag into single output_reverse flag - Remove DMMotorEnable/DMMotorStop (motor always runs, controlled by angle ref) - Remove unused modules: DJI/LK/HT/step/servo motors, chassis, shoot, referee - Add ExitRun0Mode() for STM32H723 power supply configuration - Clean up CMakeLists: 115 → 76 C source files
2026-05-29 23:37:36 +08:00
parent 5b3333125d
commit 4d1702b577
17 changed files with 790 additions and 1485 deletions
@@ -1,18 +1,14 @@
 #ifndef ROBOT_CMD_H
 #define ROBOT_CMD_H

-
 /**
 * @brief 机器人核心控制任务初始化,会被RobotInit()调用
- * 
 */
 void RobotCMDInit();

 /**
- * @brief 机器人核心控制任务,200Hz频率运行(必须高于视觉发送频率)
- * 
+ * @brief 机器人核心控制任务,500Hz频率运行
 */
 void RobotCMDTask();

-
-#endif // !ROBOT_CMD_H
+#endif // ROBOT_CMD_H
@@ -1,165 +1,113 @@
 #include "gimbal.h"
 #include "robot_def.h"
-#include "dji_motor.h"
 #include "dmmotor.h"
 #include "ins_task.h"
 #include "message_center.h"
 #include "general_def.h"
-#include "bmi088.h"
-#include "vofa.h"

-static attitude_t *gimba_IMU_data; // 云台IMU数据
+static attitude_t *gimbal_IMU_data;

-static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
-static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
-static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
-static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
+static Publisher_t *gimbal_pub;
+static Subscriber_t *gimbal_sub;
+static Gimbal_Upload_Data_s gimbal_feedback_data;
+static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;

-static DJIMotorInstance *yaw_motor;  // 云台yaw电机实例
-static DMMotorInstance *pitch_motor; // 云台pitch电机实例
+static DMMotorInstance *yaw_motor;   // DM-G6220
+static DMMotorInstance *pitch_motor; // DM-J4310-2EC

-static float yaw_speed_feedforward = 0.0f; // yaw电机的速度前馈
-static float yaw_current_feedforward_abs = 500.0f;
-static float yaw_current_feedforward = 0.0f; // yaw电机的电流前馈
-
-static float yaw_last_ref = 0.0f; // 上一次yaw电机的设定值,用于计算前馈
-
-static BMI088Instance *bmi088; // 云台IMU
 void GimbalInit()
 {
-    HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_SET);
-    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
-    HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_RESET);
+    gimbal_IMU_data = INS_Init();
+
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

-    // 初始化yaw电机
-    Motor_Init_Config_s yaw_motor_config = {
-        .can_init_config.can_handle = &hcan1,
-        .can_init_config.tx_id = 0x01,
-        .controller_param_init_config = {
-            .angle_PID = {
-                .Kp = 0.3,
-                .Ki = 0.8,
-                .Kd = 0.005,
-                .IntegralLimit = 20,
-                .CoefA = 15.0f,
-                .CoefB = 1.0f,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
-                .MaxOut = 20,
+    /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
+    DM_Motor_Init_Config_s yaw_config = {
+        .motor_config = {
+            .can_init_config = {
+                .can_handle = &hcan1,
+                .tx_id      = 0x01,
+                .rx_id      = 0xF1,
            },
-            .speed_PID = {
-                .Kp = 1200,
-                .Ki = 0,
-                .Kd = 3,
-                .IntegralLimit = 16000,
-                .Output_LPF_RC = 0.001,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter,
-                .MaxOut = 16000,
+            .controller_param_init_config = {
+                .angle_PID = {
+                    .Kp = 30, .Ki = 80, .Kd = 0.2f,
+                    .IntegralLimit = 1000,
+                    .CoefA = 20, .CoefB = 10,
+                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
+                               PID_Derivative_On_Measurement | PID_ChangingIntegrationRate |
+                               PID_DerivativeFilter | PID_OutputFilter,
+                    .MaxOut = 2000,
+                },
+                .other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle,
+                .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2],
            },
-            .other_angle_feedback_ptr = &gimba_IMU_data->YawTotalAngle,
-            .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
-            .speed_feedforward_ptr = &yaw_speed_feedforward, // 速度前馈指针
-
+            .controller_setting_init_config = {
+                .angle_feedback_source = OTHER_FEED,
+                .speed_feedback_source = OTHER_FEED,
+                .outer_loop_type       = ANGLE_LOOP,
+                .close_loop_type       = ANGLE_LOOP,
+                .output_reverse    = OUTPUT_DIRECTION_NORMAL,
+            },
+            .motor_type = DM_MOTOR,
        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
-            .outer_loop_type = ANGLE_LOOP,              // 设置为开环，电机设定值由下面的功率控制设定，不走普通的pid
-            .close_loop_type = SPEED_LOOP | ANGLE_LOOP, // 角度环和速度环
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
-            .feedforward_flag = SPEED_FEEDFORWARD | CURRENT_FEEDFORWARD, // 开启速度前馈
-        },
-        .motor_type = GM6020_NEW,
+        .ctrl_mode = DM_MODE_VELOCITY,
+        .mit_kp = 0, .mit_kd = 0,
    };
-    yaw_motor = DJIMotorInit(&yaw_motor_config);
-    // 初始化PITCH电机
-    Motor_Init_Config_s pitch_config = {
-        .can_init_config = {
-            .can_handle = &hcan1,
-            .tx_id = 0x06,
-            .rx_id = 0x16,
-        },
-        .controller_param_init_config = {
-            .angle_PID = {
-                .Kp = 1,
-                .Ki = 0,
-                .Kd = 0.005,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter | PID_DerivativeFilter,
-                .IntegralLimit = 10,
-                .MaxOut = 10,
+    yaw_motor = DMMotorInit(&yaw_config);
+
+    /* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */
+    DM_Motor_Init_Config_s pitch_config = {
+        .motor_config = {
+            .can_init_config = {
+                .can_handle = &hcan1,
+                .tx_id      = 0x02,
+                .rx_id      = 0xF2,
            },
-            .speed_PID = {
-                .Kp = 0.8,
-                .Ki = 0,
-                .Kd = 0,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
-                .IntegralLimit = 0.1,
-                .MaxOut = 1,
+            .controller_param_init_config = {
+                .angle_PID = {
+                    .Kp = 20, .Ki = 50, .Kd = 0.1f,
+                    .IntegralLimit = 500,
+                    .CoefA = 15, .CoefB = 5,
+                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
+                               PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
+                    .MaxOut = 1000,
+                },
+                .other_angle_feedback_ptr = &gimbal_IMU_data->Pitch,
+                .other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],
            },
-            .other_angle_feedback_ptr = &gimba_IMU_data->Pitch,
-            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
-            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
-        },
-        .controller_setting_init_config = {
-            .angle_feedback_source = OTHER_FEED,
-            .speed_feedback_source = OTHER_FEED,
-            .outer_loop_type = ANGLE_LOOP,
-            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-            .feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
+            .controller_setting_init_config = {
+                .angle_feedback_source = OTHER_FEED,
+                .speed_feedback_source = OTHER_FEED,
+                .outer_loop_type       = ANGLE_LOOP,
+                .close_loop_type       = ANGLE_LOOP,
+                .output_reverse    = OUTPUT_DIRECTION_NORMAL,
+            },
+            .motor_type = DM_MOTOR,
        },
+        .ctrl_mode = DM_MODE_VELOCITY,
+        .mit_kp = 0, .mit_kd = 0,
    };
    pitch_motor = DMMotorInit(&pitch_config);
 }

-/* 机器人云台控制核心任务,后续考虑只保留IMU控制,不再需要电机的反馈 */
 void GimbalTask()
 {
-    // 获取云台控制数据
-    // 后续增加未收到数据的处理
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);

-    switch (gimbal_cmd_recv.gimbal_mode)
+    if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
    {
-    // 停止
-    case GIMBAL_ZERO_FORCE:
-        DJIMotorStop(yaw_motor);
-        DMMotorStop(pitch_motor);
-
-        yaw_speed_feedforward = 0.0f; // 停止时清零前馈
-        yaw_last_ref = 0.0f;          // 停止时清零上一次参考值
-        break;
-    // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用
-    case GIMBAL_GYRO_MODE: // 后续只保留此模式
-        DJIMotorEnable(yaw_motor);
-        DMMotorEnable(pitch_motor);
-
-        yaw_speed_feedforward = DEGREE_2_RAD * (gimbal_cmd_recv.yaw - yaw_last_ref) * 500.0f;
-        LIMIT_MIN_MAX(yaw_speed_feedforward, -10.0f, 10.0f); // 限制前馈值
-        yaw_last_ref = gimbal_cmd_recv.yaw;                  // 更新上一次的参考值
-        DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);      // yaw和pitch会在robot_cmd中处理好多圈和单圈
+        /* 零力模式: 保持当前IMU角度, 电机不主动出力 */
+        DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle);
+        DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch);
+    }
+    else
+    {
+        DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
        DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
-        if (yaw_motor->motor_controller.speed_PID.Ref >= 0.5f)
-        {
-            yaw_current_feedforward = 500.0f; // 前馈电流
-        }
-        else if (yaw_motor->motor_controller.speed_PID.Ref <= -0.5f)
-        {
-            yaw_current_feedforward = -500.0f; // 前馈电流
-        }
-        else
-        {
-            yaw_current_feedforward = yaw_current_feedforward_abs * yaw_motor->motor_controller.speed_PID.Ref / 0.5f; // 前馈电流
-        }
-
-        break;
-    default:
-        break;
    }

-    // 设置反馈数据,主要是imu和yaw的ecd
-    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-
-    // 推送消息
+    gimbal_feedback_data.gimbal_imu_data = *gimbal_IMU_data;
    PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
-}
+}
@@ -3,14 +3,12 @@

 /**
 * @brief 初始化云台,会被RobotInit()调用
- * 
 */
 void GimbalInit();

 /**
- * @brief 云台任务
- * 
+ * @brief 云台任务,从gimbal_cmd话题获取控制指令并执行闭环控制
 */
 void GimbalTask();

-#endif // GIMBAL_H
+#endif // GIMBAL_H
@@ -3,58 +3,23 @@
 #include "robot_def.h"
 #include "robot_task.h"

-// 编译warning,提醒开发者修改机器人参数
-#ifndef ROBOT_DEF_PARAM_WARNING
-#define ROBOT_DEF_PARAM_WARNING
-#pragma message "check if you have configured the parameters in robot_def.h, IF NOT, please refer to the comments AND DO IT, otherwise the robot will have FATAL ERRORS!!!"
-#endif // !ROBOT_DEF_PARAM_WARNING
-
-#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-#include "chassis.h"
-#endif
-
-#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
 #include "gimbal.h"
-#include "shoot.h"
 #include "robot_cmd.h"
-#endif
-

 void RobotInit()
-{  
-    // 关闭中断,防止在初始化过程中发生中断
-    // 请不要在初始化过程中使用中断和延时函数！
-    // 若必须,则只允许使用DWT_Delay()
+{
    __disable_irq();
-    
-    BSPInit();

-#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
+    BSPInit();
    RobotCMDInit();
    GimbalInit();
-    // ShootInit();
-#endif
+    OSTaskInit();

-#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisInit();
-#endif
-
-    OSTaskInit(); // 创建基础任务
-
-    // 初始化完成,开启中断
    __enable_irq();
 }

 void RobotTask()
 {
-#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
    RobotCMDTask();
    GimbalTask();
-    // ShootTask();
-#endif
-
-#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
-    //ChassisTask();
-#endif
-
-}
+}
@@ -1,18 +1,14 @@
 #ifndef ROBOT_H
 #define ROBOT_H

-/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
-
 /**
 * @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数
- * 
 */
 void RobotInit();

 /**
 * @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务
- * 
 */
 void RobotTask();

-#endif
+#endif
@@ -1,14 +1,4 @@
-/**
- * @file robot_def.h
- * @author NeoZeng neozng1@hnu.edu.cn
- * @author Even
- * @version 0.1
- * @date 2022-12-02
- *
- * @copyright Copyright (c) HNU YueLu EC 2022 all rights reserved
- *
- */
-#pragma once // 可以用#pragma once代替#ifndef ROBOT_DEF_H(header guard)
+#pragma once
 #ifndef ROBOT_DEF_H
 #define ROBOT_DEF_H

@@ -16,58 +6,33 @@
 #include "master_process.h"
 #include "stdint.h"

-/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
+/* 开发板类型定义 */
 #define ONE_BOARD // 单板控制整车
-// #define CHASSIS_BOARD //底盘板
-// #define GIMBAL_BOARD  //云台板

 //#define VISION_USE_VCP  // 使用虚拟串口发送视觉数据
 #define VISION_USE_UART // 使用串口发送视觉数据

-/* 机器人重要参数定义,注意根据不同机器人进行修改,浮点数需要以.0或f结尾,无符号以u结尾 */
-// 云台参数
-#define YAW_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
-#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
-#define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
-#define PITCH_MAX_ANGLE 0           // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-#define PITCH_MIN_ANGLE 0           // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度)
-// 发射参数
-#define ONE_BULLET_DELTA_ANGLE 36    // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出
-#define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f
-#define NUM_PER_CIRCLE 10            // 拨盘一圈的装载量
-// 机器人底盘修改的参数,单位为mm(毫米)
-#define WHEEL_BASE 350              // 纵向轴距(前进后退方向)
-#define TRACK_WIDTH 300             // 横向轮距(左右平移方向)
-#define CENTER_GIMBAL_OFFSET_X 0    // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
-#define CENTER_GIMBAL_OFFSET_Y 0    // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
-#define RADIUS_WHEEL 60             // 轮子半径
-#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
+/* 云台参数 */
+#define YAW_CHASSIS_ALIGN_ECD 2711
+#define PITCH_HORIZON_ECD     3412
+#define PITCH_MAX_ANGLE       0
+#define PITCH_MIN_ANGLE       0

-#define GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
-#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
-#define GYRO2GIMBAL_DIR_ROLL 1  // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
+/* IMU->云台方向 */
+#define GYRO2GIMBAL_DIR_YAW   1
+#define GYRO2GIMBAL_DIR_PITCH 1
+#define GYRO2GIMBAL_DIR_ROLL  1

-// 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错
-#if (defined(ONE_BOARD) && defined(CHASSIS_BOARD)) || \
-    (defined(ONE_BOARD) && defined(GIMBAL_BOARD)) ||  \
-    (defined(CHASSIS_BOARD) && defined(GIMBAL_BOARD))
-#error Conflict board definition! You can only define one board type.
-#endif
+#pragma pack(1)

-#pragma pack(1) // 压缩结构体,取消字节对齐,下面的数据都可能被传输
-/* -------------------------基本控制模式和数据类型定义-------------------------*/
-/**
- * @brief 这些枚举类型和结构体会作为CMD控制数据和各应用的反馈数据的一部分
- *
- */
-// 机器人状态
+/* 机器人状态 */
 typedef enum
 {
    ROBOT_STOP = 0,
    ROBOT_READY,
 } Robot_Status_e;

-// 应用状态
+/* 应用状态 */
 typedef enum
 {
    APP_OFFLINE = 0,
@@ -75,125 +40,22 @@ typedef enum
    APP_ERROR,
 } App_Status_e;

-// 底盘模式设置
-/**
- * @brief 后续考虑修改为云台跟随底盘,而不是让底盘去追云台,云台的惯量比底盘小.
- *
- */
+/* 云台模式 */
 typedef enum
 {
-    CHASSIS_ZERO_FORCE = 0,    // 电流零输入
-    CHASSIS_ROTATE,            // 小陀螺模式
-    CHASSIS_NO_FOLLOW,         // 不跟随，允许全向平移
-    CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
-} chassis_mode_e;
-
-// 云台模式设置
-typedef enum
-{
-    GIMBAL_ZERO_FORCE = 0, // 电流零输入
-    GIMBAL_FREE_MODE,      // 云台自由运动模式,即与底盘分离(底盘此时应为NO_FOLLOW)反馈值为电机total_angle;似乎可以改为全部用IMU数据?
-    GIMBAL_GYRO_MODE,      // 云台陀螺仪反馈模式,反馈值为陀螺仪pitch,total_yaw_angle,底盘可以为小陀螺和跟随模式
+    GIMBAL_ZERO_FORCE = 0,
+    GIMBAL_GYRO_MODE,
 } gimbal_mode_e;

-// 发射模式设置
-typedef enum
-{
-    SHOOT_OFF = 0,
-    SHOOT_ON,
-} shoot_mode_e;
-typedef enum
-{
-    FRICTION_OFF = 0, // 摩擦轮关闭
-    FRICTION_ON,      // 摩擦轮开启
-} friction_mode_e;
-
-typedef enum
-{
-    LID_OPEN = 0, // 弹舱盖打开
-    LID_CLOSE,    // 弹舱盖关闭
-} lid_mode_e;
-
-typedef enum
-{
-    LOAD_STOP = 0,  // 停止发射
-    LOAD_REVERSE,   // 反转
-    LOAD_1_BULLET,  // 单发
-    LOAD_3_BULLET,  // 三发
-    LOAD_BURSTFIRE, // 连发
-} loader_mode_e;
-
-// 功率限制,从裁判系统获取,是否有必要保留?
-typedef struct
-{ // 功率控制
-    float chassis_power_mx;
-} Chassis_Power_Data_s;
-
-/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
-/**
- * @brief 对于双板情况,遥控器和pc在云台,裁判系统在底盘
- *
- */
-// cmd发布的底盘控制数据,由chassis订阅
+/* ---- CMD 发布给 gimbal 的控制数据 ---- */
 typedef struct
 {
-    // 控制部分
-    float vx;           // 前进方向速度
-    float vy;           // 横移方向速度
-    float wz;           // 旋转速度
-    float offset_angle; // 底盘和归中位置的夹角
-    chassis_mode_e chassis_mode;
-    int chassis_speed_buff;
-    // UI部分
-    //  ...
-
-} Chassis_Ctrl_Cmd_s;
-
-// cmd发布的云台控制数据,由gimbal订阅
-typedef struct
-{ // 云台角度控制
-    float yaw;
-    float pitch;
-    float chassis_rotate_wz;
-
+    float yaw;        // yaw 目标角度 (degrees, multi-turn)
+    float pitch;      // pitch 目标角度 (degrees)
    gimbal_mode_e gimbal_mode;
 } Gimbal_Ctrl_Cmd_s;

-// cmd发布的发射控制数据,由shoot订阅
-typedef struct
-{
-    shoot_mode_e shoot_mode;
-    loader_mode_e load_mode;
-    lid_mode_e lid_mode;
-    friction_mode_e friction_mode;
-    Bullet_Speed_e bullet_speed; // 弹速枚举
-    uint8_t rest_heat;
-    float shoot_rate; // 连续发射的射频,unit per s,发/秒
-} Shoot_Ctrl_Cmd_s;
-
-/* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/
-/**
- * @brief 由cmd订阅,其他应用也可以根据需要获取.
- *
- */
-
-typedef struct
-{
-#if defined(CHASSIS_BOARD) || defined(GIMBAL_BOARD) // 非单板的时候底盘还将imu数据回传(若有必要)
-    // attitude_t chassis_imu_data;
-#endif
-    // 后续增加底盘的真实速度
-    // float real_vx;
-    // float real_vy;
-    // float real_wz;
-
-    uint8_t rest_heat;           // 剩余枪口热量
-    Bullet_Speed_e bullet_speed; // 弹速限制
-    Enemy_Color_e enemy_color;   // 0 for blue, 1 for red
-
-} Chassis_Upload_Data_s;
-
-
+/* ---- gimbal 发布给 CMD 的反馈数据 ---- */
 typedef struct
 {
    attitude_t gimbal_imu_data;
@@ -201,12 +63,6 @@ typedef struct
    uint8_t gimbal_power;
 } Gimbal_Upload_Data_s;

-typedef struct
-{
-    // code to go here
-    // ...
-} Shoot_Upload_Data_s;
+#pragma pack()

-#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
-
-#endif // !ROBOT_DEF_H
+#endif // !ROBOT_DEF_H
@@ -1,4 +1,3 @@
-/* 注意该文件应只用于任务初始化,只能被robot.c包含*/
 #pragma once

 #include "FreeRTOS.h"
@@ -9,12 +8,8 @@
 #include "robot.h"
 #include "ins_task.h"
 #include "motor_task.h"
-#include "referee_task.h"
-#include "master_process.h"
 #include "daemon.h"
-#include "HT04.h"
 #include "buzzer.h"
-#include "dmmotor.h"

 #include "bsp_log.h"

@@ -22,23 +17,16 @@ osThreadId insTaskHandle;
 osThreadId robotTaskHandle;
 osThreadId motorTaskHandle;
 osThreadId daemonTaskHandle;
-osThreadId uiTaskHandle;

 void StartINSTASK(void const *argument);
 void StartMOTORTASK(void const *argument);
 void StartDAEMONTASK(void const *argument);
 void StartROBOTTASK(void const *argument);
-void StartUITASK(void const *argument);

-/**
- * @brief 初始化机器人任务,所有持续运行的任务都在这里初始化
- *
- */
 void OSTaskInit()
 {
    osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024);
-    insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行
-    // // 后续修改为读取传感器数据准备好的中断处理,
+    insTaskHandle = osThreadCreate(osThread(instask), NULL);

    osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
    motorTaskHandle = osThreadCreate(osThread(motortask), NULL);
@@ -48,36 +36,27 @@ void OSTaskInit()

    osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
    robotTaskHandle = osThreadCreate(osThread(robottask), NULL);
-
-    // osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512);
-    // uiTaskHandle = osThreadCreate(osThread(uitask), NULL);
-     DMMotorControlInit(); // 初始化大电机
-
-
 }

 __attribute__((noreturn)) void StartINSTASK(void const *argument)
 {
-    static float ins_start;
-    static float ins_dt;
-    INS_Init(); // 确保BMI088被正确初始化.
+    static float ins_start, ins_dt;
+    INS_Init();
    LOGINFO("[freeRTOS] INS Task Start");
    for (;;)
    {
-        // 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);
+            LOGERROR("[freeRTOS] INS Task DELAY dt=%.3f", ins_dt);
        osDelay(1);
    }
 }

 __attribute__((noreturn)) void StartMOTORTASK(void const *argument)
 {
-    static float motor_dt;
-    static float motor_start;
+    static float motor_start, motor_dt;
    LOGINFO("[freeRTOS] MOTOR Task Start");
    for (;;)
    {
@@ -85,56 +64,39 @@ __attribute__((noreturn)) void StartMOTORTASK(void const *argument)
        MotorControlTask();
        motor_dt = DWT_GetTimeline_ms() - motor_start;
        if (motor_dt > 1)
-            LOGERROR("[freeRTOS] MOTOR Task is being DELAY! dt = [%f]", &motor_dt);
+            LOGERROR("[freeRTOS] MOTOR Task DELAY dt=%.3f", motor_dt);
        osDelay(1);
    }
 }

 __attribute__((noreturn)) void StartDAEMONTASK(void const *argument)
 {
-    static float daemon_dt;
-    static float daemon_start;
+    static float daemon_start, daemon_dt;
    BuzzerInit();
    LOGINFO("[freeRTOS] Daemon Task Start");
    for (;;)
    {
-        // 100Hz
        daemon_start = DWT_GetTimeline_ms();
        DaemonTask();
        BuzzerTask();
        daemon_dt = DWT_GetTimeline_ms() - daemon_start;
        if (daemon_dt > 10)
-            LOGERROR("[freeRTOS] Daemon Task is being DELAY! dt = [%f]", &daemon_dt);
+            LOGERROR("[freeRTOS] Daemon Task DELAY dt=%.3f", daemon_dt);
        osDelay(10);
    }
 }

 __attribute__((noreturn)) void StartROBOTTASK(void const *argument)
 {
-    static float robot_dt;
-    static float robot_start;
+    static float robot_start, robot_dt;
    LOGINFO("[freeRTOS] ROBOT core Task Start");
-    // 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz
    for (;;)
    {
        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);
+            LOGERROR("[freeRTOS] ROBOT core Task DELAY dt=%.3f", robot_dt);
        osDelay(2);
    }
 }
-
-__attribute__((noreturn)) void StartUITASK(void const *argument)
-{
-    LOGINFO("[freeRTOS] UI Task Start");
-    MyUIInit();
-    LOGINFO("[freeRTOS] UI Init Done, communication with ref has established");
-    for (;;)
-    {
-        // 每给裁判系统发送一包数据会挂起一次,详见UITask函数的refereeSend()
-        UITask();
-        osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换
-    }
-}