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

CMakeLists.txt

@@ -75,47 +75,35 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     bsp/log/bsp_log.c
     bsp/bsp_tools.c
 
-    # modules
+    # modules - algorithm
     modules/algorithm/controller.c
     modules/algorithm/kalman_filter.c
     modules/algorithm/QuaternionEKF.c
     modules/algorithm/crc8.c
     modules/algorithm/crc16.c
     modules/algorithm/user_lib.c
-    modules/bluetooth/HC05.c
+    # modules - sensors
     modules/BMI088/bmi088.c
     modules/imu/BMI088driver.c
     modules/imu/BMI088Middleware.c
     modules/imu/ins_task.c
+    # modules - communication
     modules/master_machine/master_process.c
     modules/master_machine/seasky_protocol.c
-    modules/motor/DJImotor/dji_motor.c
-    modules/motor/HTmotor/HT04.c
-    modules/motor/LKmotor/LK9025.c
-    modules/motor/DMmotor/dmmotor.c
-    modules/motor/step_motor/step_motor.c
-    modules/motor/step_motor/X_V2.c
-    modules/motor/servo_motor/servo_motor.c
-    modules/motor/motor_task.c
-    modules/oled/oled.c
-    modules/referee/crc_ref.c
-    modules/referee/rm_referee.c
-    modules/referee/referee_UI.c
-    modules/referee/referee_task.c
     modules/remote/remote_control.c
-    modules/super_cap/super_cap.c
+    modules/timachine/board_comm.c
-    modules/can_comm/can_comm.c
+    # modules - motor
+    modules/motor/DMmotor/dmmotor.c
+    modules/motor/motor_task.c
+    # modules - system
     modules/message_center/message_center.c
     modules/daemon/daemon.c
     modules/alarm/buzzer.c
     modules/vofa/vofa.c
     modules/opencv/opencv.c
-    modules/timachine/board_comm.c
 
     # application
     application/gimbal/gimbal.c
-    application/chassis/chassis.c
-    application/shoot/shoot.c
     application/cmd/robot_cmd.c
     application/robot.c
 )
@@ -144,21 +132,12 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
 
     # modules
     modules/algorithm
-    modules/bluetooth
     modules/BMI088
     modules/imu
     modules/master_machine
-    modules/motor/DJImotor
-    modules/motor/LKmotor
-    modules/motor/HTmotor
-    modules/motor/step_motor
-    modules/motor/servo_motor
     modules/motor/DMmotor
     modules/motor
-    modules/oled
-    modules/referee
     modules/remote
-    modules/super_cap
     modules/can_comm
     modules/message_center
     modules/daemon
@@ -169,8 +148,6 @@ target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
     modules
 
     # application
-    application/chassis
-    application/shoot
     application/gimbal
     application/cmd
     application

Core/Src/fdcan.c

@@ -46,7 +46,7 @@ void MX_FDCAN1_Init(void)
   hfdcan1.Init.TransmitPause = DISABLE;
   hfdcan1.Init.ProtocolException = ENABLE;
   hfdcan1.Init.NominalPrescaler = 3;
-  hfdcan1.Init.NominalSyncJumpWidth = 1;
+  hfdcan1.Init.NominalSyncJumpWidth = 10;
   hfdcan1.Init.NominalTimeSeg1 = 29;
   hfdcan1.Init.NominalTimeSeg2 = 10;
   hfdcan1.Init.DataPrescaler = 3;
@@ -54,7 +54,7 @@ void MX_FDCAN1_Init(void)
   hfdcan1.Init.DataTimeSeg1 = 29;
   hfdcan1.Init.DataTimeSeg2 = 10;
   hfdcan1.Init.MessageRAMOffset = 0;
-  hfdcan1.Init.StdFiltersNbr = 14;
+  hfdcan1.Init.StdFiltersNbr = 1;
   hfdcan1.Init.ExtFiltersNbr = 0;
   hfdcan1.Init.RxFifo0ElmtsNbr = 4;
   hfdcan1.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;

application/cmd/robot_cmd.h

@@ -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

application/gimbal/gimbal.c

@@ -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 Publisher_t *gimbal_pub;
-static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
+static Subscriber_t *gimbal_sub;
-static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
+static Gimbal_Upload_Data_s gimbal_feedback_data;
-static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
+static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
 
-static DJIMotorInstance *yaw_motor;  // 云台yaw电机实例
+static DMMotorInstance *yaw_motor;   // DM-G6220
-static DMMotorInstance *pitch_motor; // 云台pitch电机实例
+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);
+    gimbal_IMU_data = INS_Init();
-    gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源
+
-    HAL_GPIO_WritePin(POWER_24V_1_GPIO_Port, POWER_24V_1_Pin, GPIO_PIN_RESET);
     gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
     gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
 
-    // 初始化yaw电机
+    /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
-    Motor_Init_Config_s yaw_motor_config = {
+    DM_Motor_Init_Config_s yaw_config = {
-        .can_init_config.can_handle = &hcan1,
+        .motor_config = {
-        .can_init_config.tx_id = 0x01,
+            .can_init_config = {
-        .controller_param_init_config = {
+                .can_handle = &hcan1,
-            .angle_PID = {
+                .tx_id      = 0x01,
-                .Kp = 0.3,
+                .rx_id      = 0xF1,
-                .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,
             },
-            .speed_PID = {
+            .controller_param_init_config = {
-                .Kp = 1200,
+                .angle_PID = {
-                .Ki = 0,
+                    .Kp = 30, .Ki = 80, .Kd = 0.2f,
-                .Kd = 3,
+                    .IntegralLimit = 1000,
-                .IntegralLimit = 16000,
+                    .CoefA = 20, .CoefB = 10,
-                .Output_LPF_RC = 0.001,
+                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_OutputFilter,
+                               PID_Derivative_On_Measurement | PID_ChangingIntegrationRate |
-                .MaxOut = 16000,
+                               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,
+            .controller_setting_init_config = {
-            .other_speed_feedback_ptr = &gimba_IMU_data->Gyro[2],
+                .angle_feedback_source = OTHER_FEED,
-            .speed_feedforward_ptr = &yaw_speed_feedforward, // 速度前馈指针
+                .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 = {
+        .ctrl_mode = DM_MODE_VELOCITY,
-            .angle_feedback_source = OTHER_FEED, .speed_feedback_source = OTHER_FEED,
+        .mit_kp = 0, .mit_kd = 0,
-            .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,
     };
-    yaw_motor = DJIMotorInit(&yaw_motor_config);
+    yaw_motor = DMMotorInit(&yaw_config);
-    // 初始化PITCH电机
+
-    Motor_Init_Config_s pitch_config = {
+    /* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */
-        .can_init_config = {
+    DM_Motor_Init_Config_s pitch_config = {
-            .can_handle = &hcan1,
+        .motor_config = {
-            .tx_id = 0x06,
+            .can_init_config = {
-            .rx_id = 0x16,
+                .can_handle = &hcan1,
-        },
+                .tx_id      = 0x02,
-        .controller_param_init_config = {
+                .rx_id      = 0xF2,
-            .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,
             },
-            .speed_PID = {
+            .controller_param_init_config = {
-                .Kp = 0.8,
+                .angle_PID = {
-                .Ki = 0,
+                    .Kp = 20, .Ki = 50, .Kd = 0.1f,
-                .Kd = 0,
+                    .IntegralLimit = 500,
-                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
+                    .CoefA = 15, .CoefB = 5,
-                .IntegralLimit = 0.1,
+                    .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
-                .MaxOut = 1,
+                               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,
+            .controller_setting_init_config = {
-            // 还需要增加角速度额外反馈指针,注意方向,ins_task.md中有c板的bodyframe坐标系说明
+                .angle_feedback_source = OTHER_FEED,
-            .other_speed_feedback_ptr = (&gimba_IMU_data->Gyro[0]),
+                .speed_feedback_source = OTHER_FEED,
-        },
+                .outer_loop_type       = ANGLE_LOOP,
-        .controller_setting_init_config = {
+                .close_loop_type       = ANGLE_LOOP,
-            .angle_feedback_source = OTHER_FEED,
+                .output_reverse    = OUTPUT_DIRECTION_NORMAL,
-            .speed_feedback_source = OTHER_FEED,
+            },
-            .outer_loop_type = ANGLE_LOOP,
+            .motor_type = DM_MOTOR,
-            .close_loop_type = SPEED_LOOP | ANGLE_LOOP,
-            .feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
-            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
         },
+        .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)
     {
-    // 停止
+        /* 零力模式: 保持当前IMU角度, 电机不主动出力 */
-    case GIMBAL_ZERO_FORCE:
+        DMMotorSetRef(yaw_motor, gimbal_IMU_data->YawTotalAngle);
-        DJIMotorStop(yaw_motor);
+        DMMotorSetRef(pitch_motor, gimbal_IMU_data->Pitch);
-        DMMotorStop(pitch_motor);
+    }
-
+    else
-        yaw_speed_feedforward = 0.0f; // 停止时清零前馈
+    {
-        yaw_last_ref = 0.0f;          // 停止时清零上一次参考值
+        DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
-        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中处理好多圈和单圈
         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 = *gimbal_IMU_data;
-    gimbal_feedback_data.gimbal_imu_data = *gimba_IMU_data;
-
-    // 推送消息
     PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
-}
+}

application/gimbal/gimbal.h

@@ -3,14 +3,12 @@
 
 /**
  * @brief 初始化云台,会被RobotInit()调用
- * 
  */
 void GimbalInit();
 
 /**
- * @brief 云台任务
+ * @brief 云台任务,从gimbal_cmd话题获取控制指令并执行闭环控制
- * 
  */
 void GimbalTask();
 
-#endif // GIMBAL_H
+#endif // GIMBAL_H

application/robot.c

@@ -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();
+    OSTaskInit();
-#endif
 
-#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
-
-}

application/robot.h

@@ -1,18 +1,14 @@
 #ifndef ROBOT_H
 #define ROBOT_H
 
-/* Robot利用robot_def.h中的宏对不同的机器人进行了大量的兼容,同时兼容了两个开发板(云台板和底盘板)的配置 */
-
 /**
  * @brief 机器人初始化,请在开启rtos之前调用.这也是唯一需要放入main函数的函数
- * 
  */
 void RobotInit();
 
 /**
  * @brief 机器人任务,放入实时系统以一定频率运行,内部会调用各个应用的任务
- * 
  */
 void RobotTask();
 
-#endif
+#endif

application/robot_def.h

@@ -1,14 +1,4 @@
-/**
+#pragma once
- * @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)
 #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_CHASSIS_ALIGN_ECD 2711  // 云台和底盘对齐指向相同方向时的电机编码器值,若对云台有机械改动需要修改
+#define PITCH_HORIZON_ECD     3412
-#define YAW_ECD_GREATER_THAN_4096 0 // ALIGN_ECD值是否大于4096,是为1,否为0;用于计算云台偏转角度
+#define PITCH_MAX_ANGLE       0
-#define PITCH_HORIZON_ECD 3412      // 云台处于水平位置时编码器值,若对云台有机械改动需要修改
+#define PITCH_MIN_ANGLE       0
-#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 GYRO2GIMBAL_DIR_YAW 1   // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
+/* IMU->云台方向 */
-#define GYRO2GIMBAL_DIR_PITCH 1 // 陀螺仪数据相较于云台的pitch的方向,1为相同,-1为相反
+#define GYRO2GIMBAL_DIR_YAW   1
-#define GYRO2GIMBAL_DIR_ROLL 1  // 陀螺仪数据相较于云台的roll的方向,1为相同,-1为相反
+#define GYRO2GIMBAL_DIR_PITCH 1
+#define GYRO2GIMBAL_DIR_ROLL  1
 
-// 检查是否出现主控板定义冲突,只允许一个开发板定义存在,否则编译会自动报错
+#pragma pack(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) // 压缩结构体,取消字节对齐,下面的数据都可能被传输
+/* 机器人状态 */
-/* -------------------------基本控制模式和数据类型定义-------------------------*/
-/**
- * @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,    // 电流零输入
+    GIMBAL_ZERO_FORCE = 0,
-    CHASSIS_ROTATE,            // 小陀螺模式
+    GIMBAL_GYRO_MODE,
-    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_mode_e;
 
-// 发射模式设置
+/* ---- CMD 发布给 gimbal 的控制数据 ---- */
-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订阅
 typedef struct
 {
-    // 控制部分
+    float yaw;        // yaw 目标角度 (degrees, multi-turn)
-    float vx;           // 前进方向速度
+    float pitch;      // pitch 目标角度 (degrees)
-    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;
-
     gimbal_mode_e gimbal_mode;
 } Gimbal_Ctrl_Cmd_s;
 
-// cmd发布的发射控制数据,由shoot订阅
+/* ---- gimbal 发布给 CMD 的反馈数据 ---- */
-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;
-
-
 typedef struct
 {
     attitude_t gimbal_imu_data;
@@ -201,12 +63,6 @@ typedef struct
     uint8_t gimbal_power;
 } Gimbal_Upload_Data_s;
 
-typedef struct
+#pragma pack()
-{
-    // code to go here
-    // ...
-} Shoot_Upload_Data_s;
 
-#pragma pack() // 开启字节对齐,结束前面的#pragma pack(1)
+#endif // !ROBOT_DEF_H
-
-#endif // !ROBOT_DEF_H

application/robot_task.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_start, ins_dt;
-    static float ins_dt;
+    INS_Init();
-    INS_Init(); // 确保BMI088被正确初始化.
     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, motor_dt;
-    static float motor_start;
     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, daemon_dt;
-    static float daemon_start;
     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, robot_dt;
-    static float robot_start;
     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中无法切换
-    }
-}

modules/algorithm/controller.c

@@ -28,8 +28,8 @@ static void f_Changing_Integration_Rate(PIDInstance *pid)
         // 积分呈累积趋势
         if (abs(pid->Err) <= pid->CoefB)
             return; // Full integral
-        if (abs(pid->Err) <= (pid->CoefA + pid->CoefB))
+        if (abs(pid->Err) <= (pid->CoefB + pid->CoefA))
-            pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
+            pid->ITerm *= (pid->CoefB + pid->CoefA - abs(pid->Err)) / pid->CoefA;
         else // 最大阈值,不使用积分
             pid->ITerm = 0;
     }

modules/motor/DMmotor/dmmotor.c

@@ -1,23 +1,23 @@
 #include "dmmotor.h"
-#include "memory.h"
 #include "general_def.h"
-#include "user_lib.h"
-#include "cmsis_os.h"
 #include "string.h"
 #include "daemon.h"
 #include "stdlib.h"
 #include "bsp_log.h"
+#include "bsp_dwt.h"
 
 static uint8_t idx;
 static DMMotorInstance *dm_motor_instance[DM_MOTOR_CNT];
-static osThreadId dm_task_handle[DM_MOTOR_CNT];
+
-/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */
+/* ---- float <-> uint conversion for MIT mode ---- */
+
 static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
 {
     float span = x_max - x_min;
     float offset = x_min;
     return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
 }
+
 static float uint_to_float(int x_int, float x_min, float x_max, int bits)
 {
     float span = x_max - x_min;
@@ -25,74 +25,155 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits)
     return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
 }
 
+/* ---- Low-level CAN send functions (all three modes) ---- */
+
+/* MIT mode: CAN ID = motor_id */
+void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq)
+{
+    uint16_t pos_uint  = float_to_uint(pos,  DM_P_MIN, DM_P_MAX, 16);
+    uint16_t vel_uint  = float_to_uint(vel,  DM_V_MIN, DM_V_MAX, 12);
+    uint16_t kp_uint   = float_to_uint(kp,   DM_KP_MIN, DM_KP_MAX, 12);
+    uint16_t kd_uint   = float_to_uint(kd,   DM_KD_MIN, DM_KD_MAX, 12);
+    uint16_t torq_uint = float_to_uint(torq, DM_T_MIN, DM_T_MAX, 12);
+
+    uint8_t *buf = can->tx_buff;
+    buf[0] = (uint8_t)(pos_uint >> 8);
+    buf[1] = (uint8_t)(pos_uint);
+    buf[2] = (uint8_t)(vel_uint >> 4);
+    buf[3] = (uint8_t)(((vel_uint & 0xF) << 4) | (kp_uint >> 8));
+    buf[4] = (uint8_t)(kp_uint);
+    buf[5] = (uint8_t)(kd_uint >> 4);
+    buf[6] = (uint8_t)(((kd_uint & 0xF) << 4) | (torq_uint >> 8));
+    buf[7] = (uint8_t)(torq_uint);
+
+    CANTransmit(can, 1);
+}
+
+/* Position-Velocity mode: CAN ID = motor_id + 0x100 */
+void DMMotorSendPosVel(CANInstance *can, float pos, float vel)
+{
+    uint8_t *buf = can->tx_buff;
+    uint8_t *pos_bytes = (uint8_t *)&pos;
+    uint8_t *vel_bytes = (uint8_t *)&vel;
+
+    buf[0] = pos_bytes[0];
+    buf[1] = pos_bytes[1];
+    buf[2] = pos_bytes[2];
+    buf[3] = pos_bytes[3];
+    buf[4] = vel_bytes[0];
+    buf[5] = vel_bytes[1];
+    buf[6] = vel_bytes[2];
+    buf[7] = vel_bytes[3];
+
+    /* Temporarily change tx_id to motor_id + 0x100 for this transmission */
+    uint32_t saved_id = can->tx_id;
+    can->tx_id = saved_id + 0x100;
+    CANTransmit(can, 1);
+    can->tx_id = saved_id;
+}
+
+/* Velocity mode: CAN ID = motor_id + 0x200 */
+void DMMotorSendVelocity(CANInstance *can, float vel)
+{
+    uint8_t *buf = can->tx_buff;
+    uint8_t *vel_bytes = (uint8_t *)&vel;
+
+    buf[0] = vel_bytes[0];
+    buf[1] = vel_bytes[1];
+    buf[2] = vel_bytes[2];
+    buf[3] = vel_bytes[3];
+    buf[4] = 0;
+    buf[5] = 0;
+    buf[6] = 0;
+    buf[7] = 0;
+
+    uint32_t saved_id = can->tx_id;
+    can->tx_id = saved_id + 0x200;
+    CANTransmit(can, 1);
+    can->tx_id = saved_id;
+}
+
+/* ---- Motor commands (enable/disable/zero/clear error) ---- */
+
 void DMMotorSetMode(DMMotor_Mode_e cmd, DMMotorInstance *motor)
 {
-    memset(motor->motor_can_instace->tx_buff, 0xff, 7);  // 发送电机指令的时候前面7bytes都是0xff
+    memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
-    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
+    motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
-    CANTransmit(motor->motor_can_instace, 1);
+    CANTransmit(motor->motor_can_instance, 1);
 }
 
+/* ---- Feedback decode: CAN RX interrupt callback ---- */
+
 static void DMMotorDecode(CANInstance *motor_can)
 {
-    uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量
     uint8_t *rxbuff = motor_can->rx_buff;
     DMMotorInstance *motor = (DMMotorInstance *)motor_can->id;
-    DM_Motor_Measure_s *measure = &(motor->measure); // 将can实例中保存的id转换成电机实例的指针
+    DM_Motor_Measure_s *measure = &motor->measure;
+    uint16_t tmp;
 
     DaemonReload(motor->motor_daemon);
-    measure->id = rxbuff[0] & 0x0F;         // 低 4 位是控制器 ID
+
-    measure->state = (rxbuff[0] >> 4) & 0x0F;  // ERR 存储在 state 字段
+    measure->id    = rxbuff[0] & 0x0F;
+    measure->state = (rxbuff[0] >> 4) & 0x0F;
 
     measure->last_position = measure->position;
+
     tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]);
     measure->position = uint_to_float(tmp, DM_P_MIN, DM_P_MAX, 16);
 
-    tmp = (uint16_t)((rxbuff[3] << 4) | rxbuff[4] >> 4);
+    tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4));
     measure->velocity = uint_to_float(tmp, DM_V_MIN, DM_V_MAX, 12);
 
-    tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]);
+    tmp = (uint16_t)(((rxbuff[4] & 0x0F) << 8) | rxbuff[5]);
     measure->torque = uint_to_float(tmp, DM_T_MIN, DM_T_MAX, 12);
 
-    measure->T_Mos = (float)rxbuff[6];
+    measure->T_Mos   = (float)rxbuff[6];
     measure->T_Rotor = (float)rxbuff[7];
-    // 多圈角度计算
-    if (measure->position - measure->last_position > 12.5)
-        measure->total_round--;
-    else if (measure->position - measure->last_position < -12.5)
-        measure->total_round++;
-    measure->total_angle = measure->position + measure->total_round * 25;
 
-    measure->total_angle = measure->total_angle * RAD_2_DEGREE;
+    /* multi-turn tracking */
-    measure->velocity = measure->velocity * RAD_2_DEGREE;
+    if (measure->position - measure->last_position > 10.0f)
+        measure->total_round--;
+    else if (measure->position - measure->last_position < -10.0f)
+        measure->total_round++;
+
+    measure->total_angle = (measure->position + measure->total_round * 25.1327f) * RAD_2_DEGREE;
+    measure->velocity    = measure->velocity * RAD_2_DEGREE;
 }
 
 static void DMMotorLostCallback(void *motor_ptr)
 {
     DMMotorInstance *motor = (DMMotorInstance *)motor_ptr;
-    uint16_t can_bus = motor->motor_can_instace->can_handle == &hcan1 ? 1 : 2;
+    LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
-    LOGWARNING("[DMmotor] Motor lost, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id);
 }
-void DMMotorCaliEncoder(DMMotorInstance *motor)
+
-{
+/* ---- Init ---- */
-    DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
+
-    DWT_Delay(0.1);
+DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
-}
-DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
 {
     DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
     memset(motor, 0, sizeof(DMMotorInstance));
 
-    motor->motor_settings = config->controller_setting_init_config;
+    motor->motor_settings = config->motor_config.controller_setting_init_config;
-    PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
+    motor->ctrl_mode = config->ctrl_mode;
-    PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
+    motor->mit_kp = config->mit_kp;
-    PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
+    motor->mit_kd = config->mit_kd;
-    motor->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
-    motor->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
 
-    config->can_init_config.can_module_callback = DMMotorDecode;
+    /* init PID controllers */
-    config->can_init_config.id = motor;
+    PIDInit(&motor->motor_controller.angle_PID,  &config->motor_config.controller_param_init_config.angle_PID);
-    motor->motor_can_instace = CANRegister(&config->can_init_config);
+    PIDInit(&motor->motor_controller.speed_PID,  &config->motor_config.controller_param_init_config.speed_PID);
+    PIDInit(&motor->motor_controller.current_PID, &config->motor_config.controller_param_init_config.current_PID);
 
+    motor->motor_controller.other_angle_feedback_ptr  = config->motor_config.controller_param_init_config.other_angle_feedback_ptr;
+    motor->motor_controller.other_speed_feedback_ptr   = config->motor_config.controller_param_init_config.other_speed_feedback_ptr;
+    motor->motor_controller.speed_feedforward_ptr      = config->motor_config.controller_param_init_config.speed_feedforward_ptr;
+    motor->motor_controller.current_feedforward_ptr    = config->motor_config.controller_param_init_config.current_feedforward_ptr;
+
+    /* register CAN instance */
+    config->motor_config.can_init_config.can_module_callback = DMMotorDecode;
+    config->motor_config.can_init_config.id = motor;
+    motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);
+
+    /* register daemon */
     Daemon_Init_Config_s conf = {
         .callback = DMMotorLostCallback,
         .owner_id = motor,
@@ -100,133 +181,110 @@ DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
     };
     motor->motor_daemon = DaemonRegister(&conf);
 
-    DMMotorEnable(motor);
+    /* enable motor */
     DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-    // DWT_Delay(0.1);
+
-    // DMMotorCaliEncoder(motor);
-    // DWT_Delay(0.1);
     dm_motor_instance[idx++] = motor;
     return motor;
 }
 
+/* ---- Runtime control ---- */
+
 void DMMotorSetRef(DMMotorInstance *motor, float ref)
 {
     motor->pid_ref = ref;
 }
 
-void DMMotorEnable(DMMotorInstance *motor)
-{
-    motor->stop_flag = MOTOR_ENALBED;
-}
-
-void DMMotorStop(DMMotorInstance *motor) // 不使用使能模式是因为需要收到反馈
-{
-    motor->stop_flag = MOTOR_STOP;
-}
-
 void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type)
 {
     motor->motor_settings.outer_loop_type = type;
 }
 
-//@Todo: 目前只实现了力控，更多位控PID等请自行添加
+void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
-void DMMotorTask(void const *argument)
 {
-    float pid_ref, set;
+    motor->ctrl_mode = mode;
-    DMMotorInstance *motor = (DMMotorInstance *)argument;
+}
-    DM_Motor_Measure_s *measure = &motor->measure;
+
-    Motor_Control_Setting_s *setting = &motor->motor_settings;
+/* ---- Main control: called from Motor Task at 1kHz ---- */
-    // CANInstance *motor_can = motor->motor_can_instace;
+
-    // uint16_t tmp;
+void DMMotorControl(void)
-    DMMotor_Send_s motor_send_mailbox;
+{
-    float pid_measure; // 电机PID测量值和设定值
+    for (uint8_t i = 0; i < idx; i++)
-    while (1)
     {
-        if(motor->measure.state == 0x00)
+        DMMotorInstance *motor = dm_motor_instance[i];
-        {
+        DM_Motor_Measure_s *measure = &motor->measure;
-            DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
+        Motor_Control_Setting_s *setting = &motor->motor_settings;
-            osDelay(20);
+        Motor_Controller_s *ctrl = &motor->motor_controller;
-        }
+        CANInstance *can = motor->motor_can_instance;
-        if(motor->measure.state != 0x00&&motor->measure.state != 0x01)
+
+        /* handle motor error states */
+        if (measure->state != 0x00 && measure->state != 0x01)
         {
             DMMotorSetMode(DM_CMD_CLEAR_ERROR, motor);
-            osDelay(20);
+            DWT_Delay(0.001);
             DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-            osDelay(20);
+            continue;
         }
-        pid_ref = motor->pid_ref;
+
-        if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
+        float pid_ref = motor->pid_ref;
-            pid_ref *= -1; // 设置反转
+
+        /* ---- Angle loop (outermost) ---- */
         if ((setting->close_loop_type & ANGLE_LOOP) && setting->outer_loop_type == ANGLE_LOOP)
         {
+            float pid_measure;
             if (setting->angle_feedback_source == OTHER_FEED)
-            {
+                pid_measure = *ctrl->other_angle_feedback_ptr;
-                pid_measure = *motor->other_angle_feedback_ptr;
-                // char buff[20];
-                // Float2Str(buff, pid_measure);
-                // LOGINFO("pid_measure: %s", buff);
-            }
             else
                 pid_measure = measure->total_angle;
-            // 更新pid_ref进入下一个环
+
-            pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
+            pid_ref = PIDCalculate(&ctrl->angle_PID, pid_measure, pid_ref);
         }
+
+        /* ---- Speed loop (inner) ---- */
         if ((setting->close_loop_type & SPEED_LOOP) && (setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP)))
         {
             if (setting->feedforward_flag & SPEED_FEEDFORWARD)
-                pid_ref += *motor->speed_feedforward_ptr;
+                pid_ref += *ctrl->speed_feedforward_ptr;
 
+            float pid_measure;
             if (setting->speed_feedback_source == OTHER_FEED)
-                pid_measure = *motor->other_speed_feedback_ptr;
+                pid_measure = *ctrl->other_speed_feedback_ptr;
-            else // MOTOR_FEED
+            else
                 pid_measure = measure->velocity;
-            // 更新pid_ref进入下一个环
+
-            pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
+            pid_ref = PIDCalculate(&ctrl->speed_PID, pid_measure, pid_ref);
         }
-        if (setting->feedforward_flag == 1)
+
-            pid_ref += 0.8;
+        /* output reverse */
-        
+        if (setting->output_reverse == OUTPUT_DIRECTION_REVERSE)
-        if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
             pid_ref *= -1;
 
-        set = pid_ref;
+        /* ---- Send via selected control mode ---- */
+        float vel_cmd = pid_ref * DEGREE_2_RAD; // deg/s → rad/s
+        LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
 
-        LIMIT_MIN_MAX(set, DM_T_MIN, DM_T_MAX);
+        switch (motor->ctrl_mode)
-        motor_send_mailbox.position_des = float_to_uint(0, DM_P_MIN, DM_P_MAX, 16);
+        {
-        motor_send_mailbox.velocity_des = float_to_uint(0, DM_V_MIN, DM_V_MAX, 12);
+        case DM_MODE_VELOCITY:
-        motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
+            /* Velocity mode: MCU angle PID output → motor velocity ref */
-        motor_send_mailbox.Kp = 0;
+            DMMotorSendVelocity(can, vel_cmd);
-        motor_send_mailbox.Kd = 0;
+            break;
 
-        if (motor->stop_flag == MOTOR_STOP)
+        case DM_MODE_MIT: {
-            motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
+            /* MIT mode: MCU angle PID output → motor position ref, motor handles position-velocity internally */
+            float pos_cmd = pid_ref * DEGREE_2_RAD;
+            LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
+            DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f);
+            break;
+        }
 
-        motor->motor_can_instace->tx_buff[0] = (uint8_t)(motor_send_mailbox.position_des >> 8);
+        case DM_MODE_POS_VEL: {
-        motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
+            /* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */
-        motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
+            float pos_cmd = pid_ref * DEGREE_2_RAD;
-        motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
+            LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
-        motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
+            DMMotorSendPosVel(can, pos_cmd, vel_cmd);
-        motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
+            break;
-        motor->motor_can_instace->tx_buff[6] = (uint8_t)(((motor_send_mailbox.Kd & 0xF) << 4) | (motor_send_mailbox.torque_des >> 8));
+        }
-        motor->motor_can_instace->tx_buff[7] = (uint8_t)(motor_send_mailbox.torque_des);
+        }
-        
-        CANTransmit(motor->motor_can_instace, 1);
-
-        osDelay(1);
     }
 }
-void DMMotorControlInit()
-{
-    char dm_task_name[5] = "dm";
-    // 遍历所有电机实例,创建任务
-    if (!idx)
-        return;
-    for (size_t i = 0; i < idx; i++)
-    {
-        char dm_id_buff[2] = {0};
-        __itoa(i, dm_id_buff, 10);
-        strcat(dm_task_name, dm_id_buff);
-        osThreadDef(dm_task_name, DMMotorTask, osPriorityNormal, 0, 128);
-        dm_task_handle[i] = osThreadCreate(osThread(dm_task_name), dm_motor_instance[i]);
-    }
-}

modules/motor/DMmotor/dmmotor.h

@@ -1,5 +1,6 @@
 #ifndef DMMOTOR_H
 #define DMMOTOR_H
+
 #include <stdint.h>
 #include "bsp_can.h"
 #include "controller.h"
@@ -8,73 +9,99 @@
 
 #define DM_MOTOR_CNT 4
 
-#define DM_P_MIN  (-12.56637f)
+/* DM motor parameter ranges (from reference DM_6220 driver) */
-#define DM_P_MAX  12.56637f
+#define DM_P_MIN  (-12.5f)
+#define DM_P_MAX  12.5f
 #define DM_V_MIN  (-45.0f)
 #define DM_V_MAX  45.0f
-#define DM_T_MIN  (-18.0f)
+#define DM_T_MIN  (-10.0f)
-#define DM_T_MAX   18.0f
+#define DM_T_MAX  10.0f
+#define DM_KP_MIN 0.0f
+#define DM_KP_MAX 500.0f
+#define DM_KD_MIN 0.0f
+#define DM_KD_MAX 5.0f
 
-typedef struct 
+/* DM motor control modes */
+typedef enum
 {
-    uint8_t id;
+    DM_MODE_MIT       = 0,  // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
-    uint8_t state;
+    DM_MODE_POS_VEL   = 1,  // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
-    float velocity;
+    DM_MODE_VELOCITY  = 2,  // Velocity mode: CAN ID + 0x200, float velocity only
-    float last_position;
+} DM_Motor_Mode_e;
-    float position;
-    float torque;
-    float T_Mos;
-    float T_Rotor;
-    int32_t total_round;
-    float total_angle;
-}DM_Motor_Measure_s;
 
+/* DM motor measurement data (parsed from feedback CAN frame) */
 typedef struct
 {
-    uint16_t position_des;
+    uint8_t id;            // motor ID from feedback byte[0:3]
-    uint16_t velocity_des;
+    uint8_t state;         // motor state from feedback byte[7:4]: 0=reset, 1=run
-    uint16_t torque_des;
+    float position;        // current position (rad, range ±12.5)
-    uint16_t Kp;
+    float velocity;        // current velocity (rad/s, range ±45)
-    uint16_t Kd;
+    float torque;          // current torque (N·m, range ±10)
-}DMMotor_Send_s;
+    float T_Mos;           // MOS temperature (Celsius)
-typedef struct 
+    float T_Rotor;         // rotor temperature (Celsius)
+    float last_position;   // previous position for multi-turn calculation
+    int32_t total_round;   // accumulated full revolutions
+    float total_angle;     // multi-turn angle (degrees)
+} DM_Motor_Measure_s;
+
+/* DM motor instance (runtime state) */
+typedef struct
 {
     DM_Motor_Measure_s measure;
     Motor_Control_Setting_s motor_settings;
-    PIDInstance current_PID;
+    Motor_Controller_s motor_controller;
-    PIDInstance speed_PID;
-    PIDInstance angle_PID;
-    float *other_angle_feedback_ptr;
-    float *other_speed_feedback_ptr;
-    float *speed_feedforward_ptr;
-    float *current_feedforward_ptr;
-    float pid_ref;
-    Motor_Working_Type_e stop_flag;
-    CANInstance *motor_can_instace;
-    DaemonInstance* motor_daemon;
-    uint32_t lost_cnt;
-    uint8_t lost_status;
-    uint8_t last_lost_status;
-}DMMotorInstance;
 
+    CANInstance *motor_can_instance;  // CAN communication instance
+    DaemonInstance *motor_daemon;     // daemon watchdog
+    DM_Motor_Mode_e ctrl_mode; // current control mode (MIT / Pos-Vel / Velocity)
+    float mit_kp;              // MIT mode KP (motor internal position gain, 0~500)
+    float mit_kd;              // MIT mode KD (motor internal velocity gain, 0~5)
+
+    float pid_ref;             // reference input from application
+} DMMotorInstance;
+
+/* DM motor init config */
+typedef struct
+{
+    Motor_Init_Config_s motor_config;   // generic motor PID and setting config
+    DM_Motor_Mode_e ctrl_mode;          // default control mode
+    float mit_kp;                       // MIT mode KP (only for MIT mode)
+    float mit_kd;                       // MIT mode KD (only for MIT mode)
+} DM_Motor_Init_Config_s;
+
+/* DM motor command codes */
 typedef enum
 {
-    DM_CMD_MOTOR_MODE = 0xfc,   // 使能,会响应指令
+    DM_CMD_MOTOR_MODE    = 0xFC,  // enable motor
-    DM_CMD_RESET_MODE = 0xfd,   // 停止
+    DM_CMD_RESET_MODE    = 0xFD,  // disable motor
-    DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
+    DM_CMD_ZERO_POSITION = 0xFE,  // set current position as encoder zero
-    DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
+    DM_CMD_CLEAR_ERROR   = 0xFB,  // clear motor error
-}DMMotor_Mode_e;
+} DMMotor_Mode_e;
 
-DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config);
+/* ---- API ---- */
+
+DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config);
 
 void DMMotorSetRef(DMMotorInstance *motor, float ref);
-
+void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type);
-void DMMotorOuterLoop(DMMotorInstance *motor,Closeloop_Type_e closeloop_type);
+void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
-
-void DMMotorEnable(DMMotorInstance *motor);
-
-void DMMotorStop(DMMotorInstance *motor);
-void DMMotorCaliEncoder(DMMotorInstance *motor);
-void DMMotorControlInit();
 void DMMotorSetMode(DMMotor_Mode_e mode, DMMotorInstance *motor);
-#endif // !DMMOTOR
+
+/**
+ * @brief Main control function called from Motor Task at 1kHz.
+ *        Iterates all registered motors, runs PID cascade, sends CAN frames.
+ */
+void DMMotorControl(void);
+
+/* ---- Low-level CAN send functions (all three modes) ---- */
+
+/* MIT mode: send position + velocity + KP + KD + torque in packed 8-byte frame */
+void DMMotorSendMIT(CANInstance *can, float pos, float vel, float kp, float kd, float torq);
+
+/* Position-Velocity mode: send float position + float velocity */
+void DMMotorSendPosVel(CANInstance *can, float pos, float vel);
+
+/* Velocity mode: send float velocity only */
+void DMMotorSendVelocity(CANInstance *can, float vel);
+
+#endif // DMMOTOR_H

modules/motor/motor_def.h

@@ -49,19 +49,13 @@ typedef enum
     OTHER_FEED,
 } Feedback_Source_e;
 
-/* 电机正反转标志 */
+/* 输出反向标志: 取反最终PID输出 */
 typedef enum
 {
-    MOTOR_DIRECTION_NORMAL = 0,
+    OUTPUT_DIRECTION_NORMAL  = 0,
-    MOTOR_DIRECTION_REVERSE = 1
+    OUTPUT_DIRECTION_REVERSE = 1,
-} Motor_Reverse_Flag_e;
+} Output_Reverse_Flag_e;
 
-/* 反馈量正反标志 */
-typedef enum
-{
-    FEEDBACK_DIRECTION_NORMAL = 0,
-    FEEDBACK_DIRECTION_REVERSE = 1
-} Feedback_Reverse_Flag_e;
 typedef enum
 {
     MOTOR_STOP = 0,
@@ -77,14 +71,13 @@ typedef enum
 /* 电机控制设置,包括闭环类型,反转标志和反馈来源 */
 typedef struct
 {
-    Closeloop_Type_e outer_loop_type;              // 最外层的闭环,未设置时默认为最高级的闭环
+    Closeloop_Type_e outer_loop_type;        // 最外层的闭环
-    Closeloop_Type_e close_loop_type;              // 使用几个闭环(串级)
+    Closeloop_Type_e close_loop_type;        // 使用几个闭环(串级)
-    Motor_Reverse_Flag_e motor_reverse_flag;       // 是否反转
+    Output_Reverse_Flag_e output_reverse;    // 输出反向
-    Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
+    Feedback_Source_e angle_feedback_source; // 角度反馈类型
-    Feedback_Source_e angle_feedback_source;       // 角度反馈类型
+    Feedback_Source_e speed_feedback_source; // 速度反馈类型
-    Feedback_Source_e speed_feedback_source;       // 速度反馈类型
+    uint8_t feedforward_flag;                // 前馈标志
-    uint8_t feedforward_flag;            // 前馈标志
+    MOTOR_SEND_MODE_e send_mode;             // 发送模式
-    MOTOR_SEND_MODE_e send_mode;                   // 发送模式
 
 
 } Motor_Control_Setting_s;
@@ -111,12 +104,7 @@ typedef struct
 typedef enum
 {
     MOTOR_TYPE_NONE = 0,
-    GM6020,
+    DM_MOTOR,
-    M3508,
-    M2006,
-    LK9025,
-    HT04,
-    GM6020_NEW //新版固件6020
 } Motor_Type_e;
 
 /**

modules/motor/motor_task.c

@@ -1,25 +1,7 @@
 #include "motor_task.h"
-#include "LK9025.h"
+#include "dmmotor.h"
-#include "HT04.h"
-#include "dji_motor.h"
-#include "step_motor.h"
-#include "servo_motor.h"
 
 void MotorControlTask()
 {
-    // static uint8_t cnt = 0; 设定不同电机的任务频率
+    DMMotorControl();
-    // if(cnt%5==0) //200hz
-    // if(cnt%10==0) //100hz
-    DJIMotorControl();
-
-    /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
-    LKMotorControl();
-
-    // legacy support
-    // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
-    // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
-    // HTMotorControl();
-    // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
-
-    // StepMotorControl();
 }

modules/opencv/opencv.c

@@ -1,99 +1,75 @@
 #include "opencv.h"
-#include "daemon.h"
-#define FRAME_SOF          0xA5
-#define FRAME_PAYLOAD_LEN  (2+2+4*12)  // 52
-#define FRAME_FULL_LEN     (1+1+FRAME_PAYLOAD_LEN) //54
 
-float opencv_Vision_data[12];
+VisionData_t vision_data;
 
-uint8_t Vison_revc_flag = 0; // 视觉数据接收标志位
-uint8_t vision_used_flag = 0; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
-
-
-
-float Vison_Frequency; // 视觉数据接收频率,单位Hz
 static USARTInstance *cv_usart_instance;
-static DaemonInstance *OPENCV_vision_daemon_instance;
+static DaemonInstance *vision_daemon_instance;
 
-/**
+static uint8_t calc_checksum(uint8_t *buf, uint8_t len)
- * @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
- * @todo  1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer
- *        2.添加标志位解码
- */
-static void DecodeVision()
 {
-    DaemonReload(OPENCV_vision_daemon_instance); // 喂狗,防止离线
+    uint8_t sum = 0;
-    static uint32_t counter;
+    for (uint8_t i = 0; i < len - 1; i++)
-    float dt = DWT_GetDeltaT(&counter); 
+        sum += buf[i];
-    Vison_Frequency = 1.0f / dt; // 计算频率
+    return sum;
-    const uint16_t len = cv_usart_instance->recv_buff_size;
-    uint16_t flag_register;
-    uint8_t *buf = cv_usart_instance->recv_buff;
-    if (len != FRAME_FULL_LEN) {
-        // 长度不符
-        LOGERROR("Vision frame length error");
-        return;
-    }
-    if (buf[0] != FRAME_SOF) {
-        // SOF 错误
-        LOGERROR("Vision frame SOF error");
-        return;
-    }
-    uint8_t payload_len = buf[1];
-    if (payload_len != FRAME_PAYLOAD_LEN) {
-        // Payload 长度不符
-        LOGERROR("Vision frame payload length error");
-        return;
-    }
-    // 解析命令号和 flags（小端）
-    uint16_t cmd_id = buf[2] | (buf[3] << 8);
-    uint16_t flags  = buf[4] | (buf[5] << 8);
-
-    // 解析 12 个 float
-    for (int i = 0; i < 12; i++) {
-        // 从 buf[6 + 4*i] 连续读 4 字节
-        memcpy(&opencv_Vision_data[i], buf + 6 + 4*i, sizeof(float));
-    }
-    // // 解析任务标志位(拟合最接近的int)
-    // int last_task_cmd = Vison_task_cmd;
-    // Vison_task_cmd = (int)(Vison_data[10] + 0.5f);
-    // if (Vison_task_cmd != last_task_cmd) {
-    //     Vison_task_changed = 1; // 任务命令变化
-    // }
-
-    Vison_revc_flag = 1; // 设置接收标志位
-    // vision_used_flag = 1; // 设置视觉数据使用标志位
-    //解析暂停标志位
-    // run = (int)(Vison_data[11] + 0.5f);
-
-    // 处理...
-    // printf("Got frame: cmd=0x%04X flags=0x%04X\n", cmd_id, flags);
-    // for (int i = 0; i < 12; i += 2) {
-    //     printf("  coord %d: (%.3f, %.3f)\n",
-    //            i/2, data[i], data[i+1]);
-    // float a = data[0];
-    
-    // TODO: code to resolve flag_register;
 }
 
-void VisionOfflineCallback(void *owner_id)
+static void DecodeVision()
 {
-    Vison_revc_flag = 0; // 清除接收标志位
+    DaemonReload(vision_daemon_instance);
+
+    static uint32_t counter;
+    float dt = DWT_GetDeltaT(&counter);
+    vision_data.frequency = 1.0f / dt;
+
+    uint8_t *buf = cv_usart_instance->recv_buff;
+    uint16_t len = cv_usart_instance->recv_buff_size;
+    uint8_t sof = buf[0];
+
+    if (sof == VISION_SOF_AUTO && len == VISION_AUTO_LEN)
+    {
+        if (calc_checksum(buf, VISION_AUTO_LEN) != buf[VISION_AUTO_LEN - 1])
+            return;
+        memcpy(&vision_data.error_x, buf + 1, 4);
+        memcpy(&vision_data.error_y, buf + 5, 4);
+        vision_data.auto_updated = 1;
+    }
+    else if (sof == VISION_SOF_MANUAL && len == VISION_MANUAL_LEN)
+    {
+        if (calc_checksum(buf, VISION_MANUAL_LEN) != buf[VISION_MANUAL_LEN - 1])
+            return;
+        vision_data.zero_force = buf[1];
+        memcpy(&vision_data.yaw_ref,   buf + 2, 4);
+        memcpy(&vision_data.pitch_ref, buf + 6, 4);
+        vision_data.manual_updated = 1;
+    }
+    else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN)
+    {
+        if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1])
+            return;
+        vision_data.cmd = buf[1];
+        vision_data.cmd_pending = 1;
+    }
+}
+
+static void VisionOfflineCallback(void *id)
+{
+    (void)id;
+    USARTServiceInit(cv_usart_instance);
 }
 
 void Vision_Uart_Init(UART_HandleTypeDef *_handle)
 {
-    USART_Init_Config_s conf;
+    USART_Init_Config_s conf = {
-    conf.module_callback = DecodeVision;
+        .module_callback = DecodeVision,
-    conf.recv_buff_size = 54;
+        .recv_buff_size  = 64,
-    conf.usart_handle = _handle;
+        .usart_handle    = _handle,
+    };
     cv_usart_instance = USARTRegister(&conf);
 
-    // 为master process注册daemon,用于判断视觉通信是否离线
     Daemon_Init_Config_s daemon_conf = {
-        .callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
+        .callback     = NULL,
-        .owner_id = NULL,
+        .owner_id     = NULL,
         .reload_count = 10,
     };
-    OPENCV_vision_daemon_instance = DaemonRegister(&daemon_conf);
+    vision_daemon_instance = DaemonRegister(&daemon_conf);
-}
+}

modules/opencv/opencv.h

@@ -1,22 +1,75 @@
 #ifndef OPENCV_H
 #define OPENCV_H
+
 #include "bsp_usart.h"
-#include "opencv.h"
-#include "bsp_log.h"
 #include "bsp_dwt.h"
+#include "bsp_log.h"
 #include "usart.h"
-#include "robot_def.h"
+#include "daemon.h"
 #include <string.h>
-#include <stdio.h>
 
+/*
+ * 上位机串口协议 — 三种帧类型, 通过帧头区分
+ *
+ * ---- 自动/视觉追踪数据帧 0xA5 (10 字节) ----
+ *   [SOF:0xA5][error_x:4][error_y:4][checksum:1]
+ *   error_x/y: float, 归一化误差, 视觉PID外环反馈值
+ *
+ * ---- 手动控制数据帧 0xA6 (11 字节) ----
+ *   [SOF:0xA6][zero_force:1][yaw_ref:4][pitch_ref:4][checksum:1]
+ *   zero_force: 0=正常角度控制, 1=零力/保持当前位置
+ *   yaw_ref/pitch_ref: float, 角度参考值 (degrees)
+ *
+ * ---- 命令帧 0xA7 (3 字节) ----
+ *   [SOF:0xA7][cmd:1][checksum:1]
+ *   cmd: 0x01=切换到自动模式, 0x02=切换到手动模式, 0x03=激光开, 0x04=激光关
+ *
+ * checksum: 帧头到数据末尾前一字节累加和取低8位
+ */
 
+#define VISION_SOF_AUTO    0xA5
+#define VISION_SOF_MANUAL  0xA6
+#define VISION_SOF_CMD     0xA7
 
+#define VISION_AUTO_LEN    10
+#define VISION_MANUAL_LEN  11
+#define VISION_CMD_LEN     3
+
+/* 命令码 */
+#define VISION_CMD_SWITCH_AUTO   0x01
+#define VISION_CMD_SWITCH_MANUAL 0x02
+#define VISION_CMD_LASER_ON      0x03
+#define VISION_CMD_LASER_OFF     0x04
+
+/* 工作模式 */
+typedef enum
+{
+    VISION_MODE_AUTO   = 0,  // 自动/视觉追踪
+    VISION_MODE_MANUAL = 1,  // 手动
+} Vision_Mode_e;
+
+typedef struct
+{
+    /* 自动模式数据 */
+    float error_x;
+    float error_y;
+    uint8_t auto_updated;   // 收到新的自动数据帧标志
+
+    /* 手动模式数据 */
+    uint8_t zero_force;
+    float yaw_ref;
+    float pitch_ref;
+    uint8_t manual_updated; // 收到新的手动数据帧标志
+
+    /* 命令 */
+    uint8_t cmd;            // 最新命令码
+    uint8_t cmd_pending;    // 有待处理命令标志
+
+    float frequency;        // 接收频率 (Hz)
+} VisionData_t;
+
+extern VisionData_t vision_data;
 
 void Vision_Uart_Init(UART_HandleTypeDef *_handle);
-extern float opencv_Vision_data[12]; // 视觉数据,12个float
-extern uint8_t Vison_revc_flag; // 视觉数据接收标志位
-extern uint8_t vision_used_flag; // 视觉数据使用标志位,用于判断是否需要使用视觉数据
-extern float Vison_Frequency; // 视觉数据频率
 
-
+#endif // OPENCV_H
-#endif // !OPENCV_H