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
This commit is contained in:
2026-05-29 23:37:36 +08:00
parent 5b3333125d
commit 4d1702b577
17 changed files with 790 additions and 1485 deletions
+187 -129
View File
@@ -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]);
}
}