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

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))
-            pid->ITerm *= (pid->CoefA - abs(pid->Err) + pid->CoefB) / pid->CoefA;
+        if (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
-    motor->motor_can_instace->tx_buff[7] = (uint8_t)cmd; // 最后一位是命令id
-    CANTransmit(motor->motor_can_instace, 1);
+    memset(motor->motor_can_instance->tx_buff, 0xFF, 7);
+    motor->motor_can_instance->tx_buff[7] = (uint8_t)cmd;
+    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;
-    measure->velocity = measure->velocity * RAD_2_DEGREE;
+    /* multi-turn tracking */
+    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, can bus [%d] , id [%d]", can_bus, motor->motor_can_instace->tx_id);
+    LOGWARNING("[DMmotor] Motor lost, id [%d]", motor->motor_can_instance->tx_id);
 }
-void DMMotorCaliEncoder(DMMotorInstance *motor)
-{
-    DMMotorSetMode(DM_CMD_ZERO_POSITION, motor);
-    DWT_Delay(0.1);
-}
-DMMotorInstance *DMMotorInit(Motor_Init_Config_s *config)
+
+/* ---- Init ---- */
+
+DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
 {
     DMMotorInstance *motor = (DMMotorInstance *)malloc(sizeof(DMMotorInstance));
     memset(motor, 0, sizeof(DMMotorInstance));
 
-    motor->motor_settings = config->controller_setting_init_config;
-    PIDInit(&motor->current_PID, &config->controller_param_init_config.current_PID);
-    PIDInit(&motor->speed_PID, &config->controller_param_init_config.speed_PID);
-    PIDInit(&motor->angle_PID, &config->controller_param_init_config.angle_PID);
-    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;
+    motor->motor_settings = config->motor_config.controller_setting_init_config;
+    motor->ctrl_mode = config->ctrl_mode;
+    motor->mit_kp = config->mit_kp;
+    motor->mit_kd = config->mit_kd;
 
-    config->can_init_config.can_module_callback = DMMotorDecode;
-    config->can_init_config.id = motor;
-    motor->motor_can_instace = CANRegister(&config->can_init_config);
+    /* init PID controllers */
+    PIDInit(&motor->motor_controller.angle_PID,  &config->motor_config.controller_param_init_config.angle_PID);
+    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 DMMotorTask(void const *argument)
+void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode)
 {
-    float pid_ref, set;
-    DMMotorInstance *motor = (DMMotorInstance *)argument;
-    DM_Motor_Measure_s *measure = &motor->measure;
-    Motor_Control_Setting_s *setting = &motor->motor_settings;
-    // CANInstance *motor_can = motor->motor_can_instace;
-    // uint16_t tmp;
-    DMMotor_Send_s motor_send_mailbox;
-    float pid_measure; // 电机PID测量值和设定值
-    while (1)
+    motor->ctrl_mode = mode;
+}
+
+/* ---- Main control: called from Motor Task at 1kHz ---- */
+
+void DMMotorControl(void)
+{
+    for (uint8_t i = 0; i < idx; i++)
     {
-        if(motor->measure.state == 0x00)
-        {
-            DMMotorSetMode(DM_CMD_MOTOR_MODE, motor);
-            osDelay(20);
-        }
-        if(motor->measure.state != 0x00&&motor->measure.state != 0x01)
+        DMMotorInstance *motor = dm_motor_instance[i];
+        DM_Motor_Measure_s *measure = &motor->measure;
+        Motor_Control_Setting_s *setting = &motor->motor_settings;
+        Motor_Controller_s *ctrl = &motor->motor_controller;
+        CANInstance *can = motor->motor_can_instance;
+
+        /* 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)
-            pid_ref *= -1; // 设置反转
+
+        float pid_ref = motor->pid_ref;
+
+        /* ---- 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 = *motor->other_angle_feedback_ptr;
-                // char buff[20];
-                // Float2Str(buff, pid_measure);
-                // LOGINFO("pid_measure: %s", buff);
-            }
+                pid_measure = *ctrl->other_angle_feedback_ptr;
             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;
-            else // MOTOR_FEED
+                pid_measure = *ctrl->other_speed_feedback_ptr;
+            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;
-        
-        if (setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
+
+        /* output reverse */
+        if (setting->output_reverse == OUTPUT_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);
-        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);
-        motor_send_mailbox.torque_des = float_to_uint(set, DM_T_MIN, DM_T_MAX, 12);
-        motor_send_mailbox.Kp = 0;
-        motor_send_mailbox.Kd = 0;
+        switch (motor->ctrl_mode)
+        {
+        case DM_MODE_VELOCITY:
+            /* Velocity mode: MCU angle PID output → motor velocity ref */
+            DMMotorSendVelocity(can, vel_cmd);
+            break;
 
-        if (motor->stop_flag == MOTOR_STOP)
-            motor_send_mailbox.torque_des = float_to_uint(0, DM_T_MIN, DM_T_MAX, 12);
+        case DM_MODE_MIT: {
+            /* 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);
-        motor->motor_can_instace->tx_buff[1] = (uint8_t)(motor_send_mailbox.position_des);
-        motor->motor_can_instace->tx_buff[2] = (uint8_t)(motor_send_mailbox.velocity_des >> 4);
-        motor->motor_can_instace->tx_buff[3] = (uint8_t)(((motor_send_mailbox.velocity_des & 0xF) << 4) | (motor_send_mailbox.Kp >> 8));
-        motor->motor_can_instace->tx_buff[4] = (uint8_t)(motor_send_mailbox.Kp);
-        motor->motor_can_instace->tx_buff[5] = (uint8_t)(motor_send_mailbox.Kd >> 4);
-        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);
+        case DM_MODE_POS_VEL: {
+            /* Position-Velocity mode: MCU angle PID output → motor position ref + velocity ff */
+            float pos_cmd = pid_ref * DEGREE_2_RAD;
+            LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX);
+            DMMotorSendPosVel(can, pos_cmd, vel_cmd);
+            break;
+        }
+        }
     }
 }
-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)
-#define DM_P_MAX  12.56637f
+/* DM motor parameter ranges (from reference DM_6220 driver) */
+#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_MAX   18.0f
+#define DM_T_MIN  (-10.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;
-    uint8_t state;
-    float velocity;
-    float last_position;
-    float position;
-    float torque;
-    float T_Mos;
-    float T_Rotor;
-    int32_t total_round;
-    float total_angle;
-}DM_Motor_Measure_s;
+    DM_MODE_MIT       = 0,  // MIT mode: CAN ID, packed pos/vel/KP/KD/torque
+    DM_MODE_POS_VEL   = 1,  // Position-Velocity mode: CAN ID + 0x100, float pos + float vel
+    DM_MODE_VELOCITY  = 2,  // Velocity mode: CAN ID + 0x200, float velocity only
+} DM_Motor_Mode_e;
 
+/* DM motor measurement data (parsed from feedback CAN frame) */
 typedef struct
 {
-    uint16_t position_des;
-    uint16_t velocity_des;
-    uint16_t torque_des;
-    uint16_t Kp;
-    uint16_t Kd;
-}DMMotor_Send_s;
-typedef struct 
+    uint8_t id;            // motor ID from feedback byte[0:3]
+    uint8_t state;         // motor state from feedback byte[7:4]: 0=reset, 1=run
+    float position;        // current position (rad, range ±12.5)
+    float velocity;        // current velocity (rad/s, range ±45)
+    float torque;          // current torque (N·m, range ±10)
+    float T_Mos;           // MOS temperature (Celsius)
+    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;
-    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;
+    Motor_Controller_s motor_controller;
 
+    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_RESET_MODE = 0xfd,   // 停止
-    DM_CMD_ZERO_POSITION = 0xfe, // 将当前的位置设置为编码器零位
-    DM_CMD_CLEAR_ERROR = 0xfb // 清除电机过热错误
-}DMMotor_Mode_e;
+    DM_CMD_MOTOR_MODE    = 0xFC,  // enable motor
+    DM_CMD_RESET_MODE    = 0xFD,  // disable motor
+    DM_CMD_ZERO_POSITION = 0xFE,  // set current position as encoder zero
+    DM_CMD_CLEAR_ERROR   = 0xFB,  // clear motor error
+} 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 closeloop_type);
-
-void DMMotorEnable(DMMotorInstance *motor);
-
-void DMMotorStop(DMMotorInstance *motor);
-void DMMotorCaliEncoder(DMMotorInstance *motor);
-void DMMotorControlInit();
+void DMMotorOuterLoop(DMMotorInstance *motor, Closeloop_Type_e type);
+void DMMotorSetCtrlMode(DMMotorInstance *motor, DM_Motor_Mode_e mode);
 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,
-    MOTOR_DIRECTION_REVERSE = 1
-} Motor_Reverse_Flag_e;
+    OUTPUT_DIRECTION_NORMAL  = 0,
+    OUTPUT_DIRECTION_REVERSE = 1,
+} 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 close_loop_type;              // 使用几个闭环(串级)
-    Motor_Reverse_Flag_e motor_reverse_flag;       // 是否反转
-    Feedback_Reverse_Flag_e feedback_reverse_flag; // 反馈是否反向
-    Feedback_Source_e angle_feedback_source;       // 角度反馈类型
-    Feedback_Source_e speed_feedback_source;       // 速度反馈类型
-    uint8_t feedforward_flag;            // 前馈标志
-    MOTOR_SEND_MODE_e send_mode;                   // 发送模式
+    Closeloop_Type_e outer_loop_type;        // 最外层的闭环
+    Closeloop_Type_e close_loop_type;        // 使用几个闭环(串级)
+    Output_Reverse_Flag_e output_reverse;    // 输出反向
+    Feedback_Source_e angle_feedback_source; // 角度反馈类型
+    Feedback_Source_e speed_feedback_source; // 速度反馈类型
+    uint8_t feedforward_flag;                // 前馈标志
+    MOTOR_SEND_MODE_e send_mode;             // 发送模式
 
 
 } Motor_Control_Setting_s;
@@ -111,12 +104,7 @@ typedef struct
 typedef enum
 {
     MOTOR_TYPE_NONE = 0,
-    GM6020,
-    M3508,
-    M2006,
-    LK9025,
-    HT04,
-    GM6020_NEW //新版固件6020
+    DM_MOTOR,
 } Motor_Type_e;
 
 /**

modules/motor/motor_task.c

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

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;
 
-/**
- * @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用
- * @todo  1.提高可读性,将get_protocol_info的第四个参数增加一个float类型buffer
- *        2.添加标志位解码
- */
-static void DecodeVision()
+static uint8_t calc_checksum(uint8_t *buf, uint8_t len)
 {
-    DaemonReload(OPENCV_vision_daemon_instance); // 喂狗,防止离线
-    static uint32_t counter;
-    float dt = DWT_GetDeltaT(&counter); 
-    Vison_Frequency = 1.0f / dt; // 计算频率
-    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;
+    uint8_t sum = 0;
+    for (uint8_t i = 0; i < len - 1; i++)
+        sum += buf[i];
+    return sum;
 }
 
-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;
-    conf.module_callback = DecodeVision;
-    conf.recv_buff_size = 54;
-    conf.usart_handle = _handle;
+    USART_Init_Config_s conf = {
+        .module_callback = DecodeVision,
+        .recv_buff_size  = 64,
+        .usart_handle    = _handle,
+    };
     cv_usart_instance = USARTRegister(&conf);
 
-    // 为master process注册daemon,用于判断视觉通信是否离线
     Daemon_Init_Config_s daemon_conf = {
-        .callback = VisionOfflineCallback, // 离线时调用的回调函数,会重启串口接收
-        .owner_id = NULL,
+        .callback     = 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