STM32 电机驱动代码级详细讲解

// 统一的API设计
uint8_t xxx_motor_init(xxx_motor_handle_t *motor, ...);
uint8_t xxx_motor_deinit(xxx_motor_handle_t *motor);

// 统一的回调函数原型
static void can_callback(void *node_obj, can_rx_header_t *can_rx_header,
                         uint8_t *can_msg);

// 初始化时注册回调
can_list_add_new_node(can_select, (void *)motor, can_id, mask,
                      CAN_ID_STD/EXT, can_callback);

typedef struct {
    // M3508/2006 专用
    float real_current;    // 实际电流
    int16_t given_current; // 期望电流（仅M3508）

    // GM6020 专用
    int16_t torque_current; // 转矩电流
    uint8_t temperature;    // 温度

    // 共用参数
    uint16_t angle;         // 绝对角度（0-8192）
    uint16_t last_angle;    // 上次角度
    uint16_t offset_angle;  // 上电初始角度
    bool got_offset;        // 是否已获取偏移

    int32_t total_angle;    // 总角度（上电后为0）
    int32_t round_cnt;      // 圈数计数
    float rotor_degree;     // 转子角度（度）

    int16_t speed_rpm;      // 速度（RPM）
    uint8_t hall;           // 霍尔传感器值

    dji_motor_model_t motor_model;
    dji_can_id_t motor_id;
    can_selected_t can_select;
} dji_motor_handle_t;

static void can_callback(void *node_obj, can_rx_header_t *can_rx_header,
                         uint8_t *can_msg) {
    dji_motor_handle_t *motor_point = (dji_motor_handle_t *)node_obj;

    // 1. 角度解析（16位，高字节在前）
    motor_point->angle = (uint16_t)((can_msg[0] << 8) | can_msg[1]);

    // 2. 首次获取角度偏移
    if (!(motor_point->got_offset)) {
        motor_point->offset_angle = motor_point->angle;
        motor_point->last_angle = motor_point->angle;
        motor_point->got_offset = true;
        motor_point->round_cnt = 0;
    }

    // 3. 根据电机型号解析不同数据
    switch (motor_point->motor_model) {
        case DJI_M3508:
            // 速度（字节2-3）
            motor_point->speed_rpm = (int16_t)((can_msg[2] << 8) | can_msg[3]);
            // 期望电流（字节4-5），负号是因为方向定义
            motor_point->given_current = (int16_t)((can_msg[4] << 8) | can_msg[5]) / -5.0f;
            break;

        case DJI_M2006:
            motor_point->speed_rpm = (int16_t)((can_msg[2] << 8) | can_msg[3]);
            // 实际电流转换：原始值 * 5.0 / 16384
            motor_point->real_current = (float)((can_msg[4] << 8) | can_msg[5]) * 5.0f / 16384.0f;
            break;

        case DJI_GM6020:
            motor_point->speed_rpm = (int16_t)((can_msg[2] << 8) | can_msg[3]);
            motor_point->torque_current = (int16_t)((can_msg[4] << 8) | can_msg[5]);
            motor_point->temperature = can_msg[6];
            break;
    }

    // 4. 圈数计数（关键算法）
    if (motor_point->angle - motor_point->last_angle > 4096) {
        --(motor_point->round_cnt);  // 反向过零点
    } else if (motor_point->angle - motor_point->last_angle < -4096) {
        ++(motor_point->round_cnt);  // 正向过零点
    }

    // 5. 计算总角度
    motor_point->total_angle = motor_point->round_cnt * 4096 * 2 +
                               motor_point->angle - motor_point->offset_angle;

    // 6. 计算转子角度（考虑减速比）
    switch (motor_point->motor_model) {
        case DJI_M3508:  // 19:1减速比
            motor_point->rotor_degree = (float)(motor_point->total_angle) / (19.0f * 8192.0f) * 360.0f;
            break;

        case DJI_M2006:  // 36:1减速比
            motor_point->rotor_degree = (float)(motor_point->total_angle) / (36.0f * 8192.0f) * 360.0f;
            break;

        case DJI_GM6020:  // 1:1减速比，绝对位置
            motor_point->rotor_degree = (float)(motor_point->angle) / 22.75f;  // 8192/360 = 22.75
            break;
    }
}

// 示例：正向旋转
last_angle = 8190
angle = 2
angle - last_angle = 2 - 8190 = -8188 < -4096
→ round_cnt++  // 正向增加一圈

// 示例：反向旋转
last_angle = 2
angle = 8190
angle - last_angle = 8190 - 2 = 8188 > 4096
→ round_cnt--  // 反向减少一圈

void dji_motor_set_current(can_selected_t can_select, uint16_t can_identify,
                           int16_t iq1, int16_t iq2, int16_t iq3, int16_t iq4) {
    uint8_t send_msg[8];
    // 4个电机，每个2字节电流值
    send_msg[0] = (iq1 >> 8) & 0xFF;
    send_msg[1] = iq1 & 0xFF;
    send_msg[2] = (iq2 >> 8) & 0xFF;
    send_msg[3] = iq2 & 0xFF;
    send_msg[4] = (iq3 >> 8) & 0xFF;
    send_msg[5] = iq3 & 0xFF;
    send_msg[6] = (iq4 >> 8) & 0xFF;
    send_msg[7] = iq4 & 0xFF;

    can_send_message(can_select, CAN_ID_STD, can_identify, 8, send_msg);
}

// 定义电机句柄
dji_motor_handle_t m3508_motors[4];

// 初始化
for (int i = 0; i < 4; i++) {
    dji_motor_init(&m3508_motors[i], DJI_M3508,
                    CAN_Motor1_ID + i, can1_selected);
}

// 控制电流
dji_motor_set_current(can1_selected, DJI_MOTOR_GROUP1,
                      1000, 0, 0, 0);  // 电机1设置1000mA

// 读取状态
float speed = m3508_motors[0].speed_rpm;
float angle = m3508_motors[0].rotor_degree;

typedef struct {
    uint32_t master_id;        // 反馈主机ID（接收时匹配）
    uint32_t device_id;        // 控制设备ID（发送时使用）
    dm_model_t model;          // 电机型号
    dm_mode_t mode;            // 当前模式

    // 反馈数据
    float position;            // 位置（rad）
    float speed;               // 速度（rad/s）
    float torque;              // 扭矩（N·m）
    float mos_temperature;     // MOS温度（°C）
    float motor_temperature;   // 电机温度（°C）
    dm_error_t error;          // 错误信息

    // 控制参数（需与上位机设定一致）
    float pos_limit;           // 位置限制（rad）
    float spd_limit;           // 速度限制（rad/s）
    float torq_limit;          // 扭矩限制（N·m）

    can_selected_t can_select;
} dm_handle_t;

typedef enum {
    DM_OK_DISABLED = 0x00U,      // 无故障，失能状态
    DM_OK_ENABLED,               // 无故障，使能状态
    DM_ERR_OVER_VOLTAGE = 0x08U, // 过压
    DM_ERR_UNDER_VOLTAGE,        // 欠压
    DM_ERR_OVER_CURRENT,         // 过流
    DM_ERR_MOS_TEMPERATURE,      // MOS过温
    DM_ERR_MOTOR_TEMPERATURE,    // 电机过温
    DM_ERR_LOST_COMMUNICATION,   // 通信丢失
    DM_ERR_OVER_LOAD             // 过载
} dm_error_t;

static void can_callback(void *node_obj, can_rx_header_t *can_rx_header,
                         uint8_t *can_msg) {
    dm_handle_t *motor = (dm_handle_t *)node_obj;

    // 检查ID匹配
    if (can_rx_header->id != motor->master_id) {
        return;
    }

    // 解析字节0：低4位=device_id，高4位=error
    motor->device_id = can_msg[0] & 0x0F;
    motor->error = (can_msg[0] >> 4) & 0xF;

    // 解析位置（字节1-2，16位）
    int temp = (can_msg[1] << 8) | can_msg[2];
    motor->position = uint_to_float(temp, -motor->pos_limit,
                                    motor->pos_limit, 16);

    // 解析速度（字节3高4位+字节4，12位）
    temp = (can_msg[3] << 4) | (can_msg[4] >> 4);
    motor->speed = uint_to_float(temp, -motor->spd_limit,
                                motor->spd_limit, 12);

    // 解析扭矩（字节4低4位+字节5，12位）
    temp = ((can_msg[4] & 0x0F) << 8) | can_msg[5];
    motor->torque = uint_to_float(temp, -motor->torq_limit,
                                  motor->torq_limit, 12);

    // 温度（字节6-7）
    motor->mos_temperature = (float)can_msg[6];
    motor->motor_temperature = (float)can_msg[7];
}

字节0: [error(4bit)][device_id(4bit)]
字节1-2: position (16位)
字节3-4: [speed(12位)][torque(12位)]
字节6: mos_temperature (8位)
字节7: motor_temperature (8位)

// uint转float（解码接收数据）
float uint_to_float(int x_int, float x_min, float x_max, int bits) {
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

// float转uint（编码发送数据）
int float_to_uint(float x, float x_min, float x_max, int bits) {
    float span = x_max - x_min;
    float offset = x_min;
    return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

位置限制：-π ~ π
实际位置：1.57 rad (π/2)
bits: 16

编码结果：
uint = (1.57 - (-π)) * (65535 / (π - (-π)))
      = 4.7124 * 65535 / 6.2832
      = 49152

void dm_mit_ctrl(dm_handle_t *motor, float position, float speed,
                 float kp, float kd, float torque) {
    uint8_t send_msg[8];
    uint16_t pos_tmp, spd_tmp, kp_tmp, kd_tmp, torq_tmp;

    // 编码各个参数
    pos_tmp = float_to_uint(position, -motor->pos_limit,
                            motor->pos_limit, 16);
    spd_tmp = float_to_uint(speed, -motor->spd_limit,
                            motor->spd_limit, 12);
    kp_tmp = float_to_uint(kp, DM_KP_MIN, DM_KP_MAX, 12);  // 0-500
    kd_tmp = float_to_uint(kd, DM_KD_MIN, DM_KD_MAX, 12);  // 0-5
    torq_tmp = float_to_uint(torque, -motor->torq_limit,
                            motor->torq_limit, 12);

    // 位域打包（关键）
    send_msg[0] = (pos_tmp >> 8);           // 位置高8位
    send_msg[1] = pos_tmp;                  // 位置低8位
    send_msg[2] = (spd_tmp >> 4);           // 速度高8位
    send_msg[3] = ((spd_tmp & 0xF) << 4) | (kp_tmp >> 8);  // 速度低4位+KP高8位
    send_msg[4] = kp_tmp;                   // KP低8位
    send_msg[5] = (kd_tmp >> 4);            // KD高8位
    send_msg[6] = ((kd_tmp & 0xF) << 4) | (torq_tmp >> 8); // KD低4位+扭矩高8位
    send_msg[7] = torq_tmp;                // 扭矩低8位

    can_send_message(motor->can_select, CAN_ID_STD,
                    motor->device_id + MIT_MODE, 8, send_msg);
}

输出扭矩 = Torque_cmd + Kp × (Pos_des - Pos) + Kd × (Spd_des - Spd)

void dm_pos_speed_ctrl(dm_handle_t *motor, float position, float speed) {
    uint8_t send_msg[8];
    // 直接使用IEEE754浮点格式（每个4字节）
    memcpy(&send_msg[0], &position, sizeof(float));
    memcpy(&send_msg[4], &speed, sizeof(float));

    can_send_message(motor->can_select, CAN_ID_STD,
                    motor->device_id + POS_SPEED_MODE, 8, send_msg);
}

void dm_speed_ctrl(dm_handle_t *motor, float speed) {
    uint8_t send_msg[4];
    memcpy(send_msg, &speed, sizeof(float));

    can_send_message(motor->can_select, CAN_ID_STD,
                    motor->device_id + SPEED_MODE, 4, send_msg);
}

// 使能
void dm_motor_enable(dm_handle_t *motor) {
    uint8_t send_msg[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC};
    uint32_t id = motor->device_id;

    switch (motor->mode) {
        case DM_MODE_MIT:      id += MIT_MODE; break;
        case DM_MODE_POS_SPEED: id += POS_SPEED_MODE; break;
        case DM_MODE_SPEED:    id += SPEED_MODE; break;
    }

    can_send_message(motor->can_select, CAN_ID_STD, id, 8, send_msg);
}

// 失能
void dm_motor_disable(dm_handle_t *motor) {
    uint8_t send_msg[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};
    // 其余代码与enable相同
}

void dm_save_zero(dm_handle_t *motor) {
    uint8_t send_msg[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE};
    // 发送0xFE命令
}

void dm_clear_error(dm_handle_t *motor) {
    uint8_t send_msg[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB};
    // 发送0xFB命令
}

// 定义电机
dm_handle_t dm_motor;

// 初始化
dm_motor_init(&dm_motor, 0x01, 0x01, DM_MODE_MIT, DM_J4310,
              3.14159f, 30.0f, 18.0f, can1_selected);

// 使能
dm_motor_enable(&dm_motor);

// MIT控制
dm_mit_ctrl(&dm_motor, 1.57f, 0.0f, 100.0f, 1.0f, 0.5f);
// 位置: 1.57rad (90度)
// 速度: 0.0 rad/s
// Kp: 100
// Kd: 1
// 前馈扭矩: 0.5 N·m

// 读取反馈
float pos = dm_motor.position;
float spd = dm_motor.speed;
float torque = dm_motor.torque;

STM32 ──┬── EN ──> 步进电机驱动器
        ├── DIR
        └── STEP (TIM PWM)

typedef struct {
    step_motor_state_t state;   // 状态（停止/运行）

    // GPIO定义
    step_motor_gpio_t en_pin;   // EN引脚
    step_motor_gpio_t dir_pin;  // DIR引脚
    step_motor_gpio_t step_pin; // STEP引脚

    // PWM配置
    TIM_HandleTypeDef *htim;    // 定时器句柄
    uint32_t channel;          // 定时器通道
    uint32_t pulse_remain;     // 剩余脉冲数

    step_motor_dir_t dir;      // 当前方向
} step_motor_handle_t;

typedef struct {
    GPIO_TypeDef *port;
    uint32_t pin;
} step_motor_gpio_t;

typedef enum {
    STEP_MOTOR_STATE_RESET,  // 未初始化
    STEP_MOTOR_STATE_STOP,   // 停止
    STEP_MOTOR_STATE_RUN     // 运行
} step_motor_state_t;

typedef enum {
    STEP_MOTOR_AWAY,    // 远离电机方向
    STEP_MOTOR_TOWARDS  // 朝着电机方向
} step_motor_dir_t;

void step_motor_init(step_motor_handle_t *handle) {
    GPIO_InitTypeDef gpio_init_struct = {
        .Mode = GPIO_MODE_OUTPUT_PP,
        .Pull = GPIO_PULLUP,
        .Speed = GPIO_SPEED_FREQ_HIGH
    };
    TIM_OC_InitTypeDef tim_pwm_config = {0};

    // 1. 配置EN引脚（推挽输出）
    gpio_init_struct.Pin = handle->en_pin.pin;
    HAL_GPIO_Init(handle->en_pin.port, &gpio_init_struct);

    // 2. 配置DIR引脚（推挽输出）
    gpio_init_struct.Pin = handle->dir_pin.pin;
    HAL_GPIO_Init(handle->dir_pin.port, &gpio_init_struct);

    // 3. 配置STEP引脚（复用推挽输出，连接定时器PWM）
    gpio_init_struct.Mode = GPIO_MODE_AF_PP;
    gpio_init_struct.Pin = handle->step_pin.pin;
    HAL_GPIO_Init(handle->step_pin.port, &gpio_init_struct);

    // 4. 配置定时器基础参数
    handle->htim->Init.Prescaler = 72 - 1;      // 72MHz / 72 = 1MHz
    handle->htim->Init.Period = STEP_MOTOR_INIT_PERIOD - 1;  // 1800 - 1
    handle->htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    handle->htim->Init.CounterMode = TIM_COUNTERMODE_UP;

    HAL_TIM_PWM_Init(handle->htim);

    // 5. 配置PWM通道
    tim_pwm_config.OCMode = TIM_OCMODE_PWM1;      // PWM模式1
    tim_pwm_config.OCPolarity = TIM_OCPOLARITY_HIGH;  // 高电平有效
    tim_pwm_config.Pulse = (STEP_MOTOR_INIT_PERIOD / 2) - 1;  // 50%占空比
    tim_pwm_config.OCFastMode = TIM_OCFAST_DISABLE;
    tim_pwm_config.OCIdleState = TIM_OCIDLESTATE_RESET;

    HAL_TIM_PWM_ConfigChannel(handle->htim, &tim_pwm_config, handle->channel);

    // 6. 初始化状态
    handle->pulse_remain = 0;
    handle->state = STEP_MOTOR_STATE_STOP;
    step_motor_disable(handle);  // 默认失能
}

Prescaler = 72 - 1 = 71
Period = 1800 - 1 = 1799

定时器频率 = 72MHz / (71+1) / (1799+1) = 72MHz / 72 / 1800 = 555.5 Hz

PWM周期 = 1 / 555.5 = 1.8ms = 1800μs

void step_motor_enable(step_motor_handle_t *handle) {
    // 低电平使能（根据驱动器规格）
    HAL_GPIO_WritePin(handle->en_pin.port, handle->en_pin.pin, GPIO_PIN_RESET);
}

void step_motor_disable(step_motor_handle_t *handle) {
    // 高电平失能（释放电机，可以自由转动）
    HAL_GPIO_WritePin(handle->en_pin.port, handle->en_pin.pin, GPIO_PIN_SET);
}

void step_motor_set_dir(step_motor_handle_t *handle, step_motor_dir_t dir) {
    switch (dir) {
        case STEP_MOTOR_TOWARDS:  // 朝着电机方向
            HAL_GPIO_WritePin(handle->dir_pin.port, handle->dir_pin.pin,
                              (GPIO_PinState)dir);  // dir = 1
            break;

        case STEP_MOTOR_AWAY:     // 远离电机方向
            HAL_GPIO_WritePin(handle->dir_pin.port, handle->dir_pin.pin,
                              (GPIO_PinState)dir);  // dir = 0
            break;
    }
    handle->dir = dir;
}

void step_mtoor_set_speed(step_motor_handle_t *handle, uint16_t period) {
    if (handle->state == STEP_MOTOR_STATE_RUN) {
        return;  // 运行中不能修改速度
    }

    // 修改ARR寄存器改变PWM周期
    handle->htim->Instance->ARR = period - 1;
    // 修改比较值保持50%占空比
    __HAL_TIM_SET_COMPARE(handle->htim, handle->channel, period / 2);
}

PWM频率 = 72MHz / 72 / period = 1MHz / period (Hz)

例如：
period = 1800 → 频率 = 555.5 Hz → 速度 = 555.5 步/秒
period = 900  → 频率 = 1111 Hz  → 速度 = 1111 步/秒

void step_motor_run(step_motor_handle_t *handle, int32_t pulse_num) {
    if (pulse_num == 0) {
        return;  // 不需要运动
    }

    // 1. 使能电机
    step_motor_enable(handle);

    // 2. 设置方向
    if (pulse_num > 0) {
        step_motor_set_dir(handle, STEP_MOTOR_TOWARDS);
    } else {
        step_motor_set_dir(handle, STEP_MOTOR_AWAY);
    }

    // 3. 设置剩余脉冲数
    handle->pulse_remain = abs(pulse_num);
    handle->state = STEP_MOTOR_STATE_RUN;

    // 4. 启动PWM中断
    HAL_TIM_PWM_Start_IT(handle->htim, handle->channel);
}

void step_motor_interrupt_callback(step_motor_handle_t *handle) {
    // 每个PWM周期调用一次（一个脉冲）

    if (handle->pulse_remain == 0) {
        // 脉冲发送完毕
        HAL_TIM_PWM_Stop_IT(handle->htim, handle->channel);
        handle->state = STEP_MOTOR_STATE_STOP;
        step_motor_disable(handle);  // 失能电机
        return;
    }

    // 减少剩余脉冲数
    --handle->pulse_remain;
}

PWM周期触发中断
    ↓
检查剩余脉冲数
    ↓
    ├─ pulse_remain = 0 → 停止PWM，失能电机
    └─ pulse_remain > 0 → pulse_remain--，继续

// 定义步进电机
step_motor_handle_t step_motor;

// 配置GPIO
step_motor.en_pin.port = GPIOA;
step_motor.en_pin.pin = GPIO_PIN_0;

step_motor.dir_pin.port = GPIOA;
step_motor.dir_pin.pin = GPIO_PIN_1;

step_motor.step_pin.port = GPIOA;
step_motor.step_pin.pin = GPIO_PIN_2;

// 配置定时器（TIM3 CH1）
step_motor.htim = &htim3;
step_motor.channel = TIM_CHANNEL_1;

// 初始化
step_motor_init(&step_motor);

// 设置速度（period = 900，约1111步/秒）
step_mtoor_set_speed(&step_motor, 900);

// 运动400个脉冲（一圈，假设一圈400脉冲）
step_motor_run(&step_motor, 400);

// 运动-400个脉冲（反向一圈）
step_motor_run(&step_motor, -400);

// 运动一圈需要的脉冲数（根据电机规格修改）
#define STEP_MOTOR_CIRCLE_PULSE 400

// 初始PWM周期（越大频率越低，速度越慢）
#define STEP_MOTOR_INIT_PERIOD 1800

typedef struct {
    uint8_t vesc_id;         // 电机ID (0-255)
    can_selected_t can_select;

    // 控制输入（通过指令设置）

    // 状态反馈
    float input_voltage;     // 输入电压 (V)
    float duty;              // MOSFET占空比 (0-1)
    float erpm;              // 电气转速 (ERPM)

    float amp_hours;         // 电流时间 (Ah)
    float amp_hours_charged; // 充电电流时间 (Ah)

    float watt_hours;        // 功率时间 (Wh)
    float watt_hours_charged; // 充电功率时间 (Wh)

    float motor_current;     // 电机电流 (A)
    float total_current;     // 总电流 (A)

    float mosfet_temperature; // MOS温度 (°C)
    float motor_temperature;  // 电机温度 (°C)

    float pid_pos;           // 转子位置 (rad)

    int32_t tachometer_value; // 转速表
    vesc_fault_code_t error_code; // 错误码
} vesc_motor_handle_t;

typedef enum {
    CAN_PACKET_SET_DUTY = 0,              // 设置占空比
    CAN_PACKET_SET_CURRENT,               // 设置电流
    CAN_PACKET_SET_CURRENT_BRAKE,         // 设置制动电流
    CAN_PACKET_SET_RPM,                   // 设置转速
    CAN_PACKET_SET_POS,                   // 设置位置
    CAN_PACKET_FILL_RX_BUFFER,            // 填充接收缓冲区
    CAN_PACKET_FILL_RX_BUFFER_LONG,       // 填充长接收缓冲区
    CAN_PACKET_PROCESS_RX_BUFFER,         // 处理接收缓冲区
    CAN_PACKET_PROCESS_SHORT_BUFFER,      // 处理短缓冲区
    CAN_PACKET_STATUS,                    // 状态1
    CAN_PACKET_SET_CURRENT_REL,           // 设置相对电流
    CAN_PACKET_SET_CURRENT_BRAKE_REL,     // 设置相对制动电流
    CAN_PACKET_SET_CURRENT_HANDBRAKE,     // 设置手刹电流
    CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, // 设置相对手刹电流
    CAN_PACKET_STATUS_2,                  // 状态2
    CAN_PACKET_STATUS_3,                  // 状态3
    CAN_PACKET_STATUS_4,                  // 状态4
    CAN_PACKET_PING,                      // 心跳
    CAN_PACKET_PONG,                      // 心跳响应
    CAN_PACKET_DETECT_APPLY_ALL_FOC,      // FOC检测
    CAN_PACKET_DETECT_APPLY_ALL_FOC_RES,   // FOC检测结果
    CAN_PACKET_CONF_CURRENT_LIMITS,       // 配置电流限制
    CAN_PACKET_CONF_STORE_CURRENT_LIMITS, // 存储电流限制
    CAN_PACKET_CONF_CURRENT_LIMITS_IN,    // 配置输入电流限制
    CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, // 存储输入电流限制
    CAN_PACKET_CONF_FOC_ERPMS,            // 配置FOC ERPM
    CAN_PACKET_CONF_STORE_FOC_ERPMS,      // 存储FOC ERPM
    CAN_PACKET_STATUS_5                   // 状态5
} can_packet_id_t;

位0-7:   VESC设备ID (0-255)
位8-15:  指令类型（can_packet_id_t）
位16-28: 保留

设备ID = 1
指令 = CAN_PACKET_SET_DUTY (0)

CAN ID = 0x00000100
       = (1 << 0) | (0 << 8)
       = 0x00000100

void vesc_can_callback(void *can_ptr, can_rx_header_t *can_rx_header,
                       uint8_t *recv_msg) {
    uint32_t can_id = can_rx_header->id;
    vesc_motor_handle_t *vesc_motor = (vesc_motor_handle_t *)can_ptr;

    // 提取指令类型（位8-15）
    int32_t message_status = (can_id >> 8) & 0xFF;
    int32_t buffer_index = 0;

    switch (message_status) {
        case CAN_PACKET_STATUS: {
            // 状态1：转速、电流、占空比
            vesc_motor->erpm = buffer_get_float32(recv_msg, 1.0f, &buffer_index);
            vesc_motor->total_current = buffer_get_float16(recv_msg, 10.0f, &buffer_index);
            vesc_motor->duty = buffer_get_float16(recv_msg, 1000.0f, &buffer_index);
        } break;

        case CAN_PACKET_STATUS_2: {
            // 状态2：电流时间
            vesc_motor->amp_hours = buffer_get_float32(recv_msg, 10000.0f, &buffer_index);
            vesc_motor->amp_hours_charged = buffer_get_float32(recv_msg, 10000.0f, &buffer_index);
        } break;

        case CAN_PACKET_STATUS_3: {
            // 状态3：功率时间
            vesc_motor->watt_hours = buffer_get_float32(recv_msg, 10000.0f, &buffer_index);
            vesc_motor->watt_hours_charged = buffer_get_float32(recv_msg, 10000.0f, &buffer_index);
        } break;

        case CAN_PACKET_STATUS_4: {
            // 状态4：温度、电流、位置
            vesc_motor->mosfet_temperature = buffer_get_float32(recv_msg, 10.0f, &buffer_index);
            vesc_motor->motor_current = buffer_get_float32(recv_msg, 10.0f, &buffer_index);
            vesc_motor->total_current = buffer_get_float32(recv_msg, 10.0f, &buffer_index);
            vesc_motor->pid_pos = buffer_get_float32(recv_msg, 50.0f, &buffer_index);
        } break;

        case CAN_PACKET_STATUS_5: {
            // 状态5：转速表、输入电压
            vesc_motor->tachometer_value = buffer_get_int32(recv_msg, &buffer_index);
            vesc_motor->input_voltage = buffer_get_float16(recv_msg, 10.0f, &buffer_index);
        } break;
    }
}

// 32位浮点数解码
float buffer_get_float32(uint8_t *buffer, float scale, int32_t *index) {
    uint32_t res = ((uint32_t)buffer[*index]) |
                   ((uint32_t)buffer[*index + 1] << 8) |
                   ((uint32_t)buffer[*index + 2] << 16) |
                   ((uint32_t)buffer[*index + 3] << 24);
    *index += 4;
    return (*((float*)&res)) / scale;
}

// 16位浮点数解码
float buffer_get_float16(uint8_t *buffer, float scale, int32_t *index) {
    uint16_t res = ((uint16_t)buffer[*index]) |
                   ((uint16_t)buffer[*index + 1] << 8);
    *index += 2;
    return ((float)res) / scale;
}

// 32位整数解码
int32_t buffer_get_int32(uint8_t *buffer, int32_t *index) {
    int32_t res = ((int32_t)buffer[*index]) |
                  ((int32_t)buffer[*index + 1] << 8) |
                  ((int32_t)buffer[*index + 2] << 16) |
                  ((int32_t)buffer[*index + 3] << 24);
    *index += 4;
    return res;
}

原始数据: [0x00, 0x00, 0x0A, 0x00, 0x00, 0x64, 0x03, 0xE8]

erpm (字节0-3):
  0x00000A00 = 2560
  erpm = 2560 / 1.0 = 2560 ERPM

total_current (字节4-5):
  0x0064 = 100
  total_current = 100 / 10.0 = 10.0 A

duty (字节6-7):
  0x03E8 = 1000
  duty = 1000 / 1000.0 = 1.0 (100%占空比)

void vesc_motor_set_duty(vesc_motor_handle_t *motor, float duty) {
    int32_t index = 0;
    uint8_t buffer[4];

    // 编码32位浮点数，scale=100000
    buffer_append_float32(buffer, duty, 100000.0f, &index);

    // 发送：CAN_ID = vesc_id | (CAN_PACKET_SET_DUTY << 8)
    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_DUTY << 8)), 4, buffer);
}

duty = 0.5 (50%占空比)

buffer_append_float32编码:
  值 = 0.5 * 100000 = 50000
  字节: [0x50, 0xC3, 0x00, 0x00]

void vesc_motor_set_current(vesc_motor_handle_t *motor, float current) {
    int32_t index = 0;
    uint8_t buffer[4];

    // 编码32位浮点数，scale=1000（单位：毫安）
    buffer_append_float32(buffer, current, 1000.0f, &index);

    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_CURRENT << 8)), 4, buffer);
}

current = 10.5 A

buffer_append_float32编码:
  值 = 10.5 * 1000 = 10500
  字节: [0x34, 0x29, 0x00, 0x00]

void vesc_motor_set_break_current(vesc_motor_handle_t *motor, float current) {
    int32_t index = 0;
    uint8_t buffer[4];

    buffer_append_float32(buffer, current, 1000.0f, &index);

    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_CURRENT_BRAKE << 8)), 4, buffer);
}

void vesc_motor_set_erpm(vesc_motor_handle_t *motor, float erpm) {
    int32_t index = 0;
    uint8_t buffer[4];

    // 编码32位浮点数，scale=1.0（单位：ERPM）
    buffer_append_float32(buffer, erpm, 1.0f, &index);

    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_RPM << 8)), 4, buffer);
}

void vesc_motor_set_pos(vesc_motor_handle_t *motor, float pos) {
    int32_t index = 0;
    uint8_t buffer[4];

    // 编码32位浮点数，scale=1.0（单位：rad）
    buffer_append_float32(buffer, pos, 1.0f, &index);

    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_POS << 8)), 4, buffer);
}

void vesc_motor_set_relative_current(vesc_motor_handle_t *motor,
                                     float current) {
    int32_t index = 0;
    uint8_t buffer[4];

    // 编码32位浮点数，scale=100000（范围：-1.0 ~ 1.0）
    buffer_append_float32(buffer, current, 100000.0f, &index);

    can_send_message(motor->can_select, CAN_ID_EXT,
                     (motor->vesc_id | (CAN_PACKET_SET_CURRENT_REL << 8)), 4, buffer);
}

void vesc_motor_set_current_limit(vesc_motor_handle_t *motor, float min_current,
                                  float max_current, bool store_to_rom) {
    int32_t index = 0;
    uint8_t buffer[8];

    // 编码最小电流（scale=1000，单位：毫安）
    buffer_append_float32(buffer, min_current, 1000.0f, &index);

    // 编码最大电流（scale=1000，单位：毫安）
    buffer_append_float32(buffer, max_current, 1000.0f, &index);

    if (store_to_rom) {
        // 存储到ROM（永久保存）
        can_send_message(motor->can_select, CAN_ID_EXT,
                         (motor->vesc_id | (CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN << 8)),
                         8, buffer);
    } else {
        // 临时设置（掉电丢失）
        can_send_message(motor->can_select, CAN_ID_EXT,
                         (motor->vesc_id | (CAN_PACKET_CONF_CURRENT_LIMITS_IN << 8)),
                         8, buffer);
    }
}

// 32位浮点数编码
void buffer_append_float32(uint8_t *buffer, float number, float scale, int32_t *index) {
    int32_t value = (int32_t)(number * scale);
    buffer[*index] = value;
    buffer[*index + 1] = value >> 8;
    buffer[*index + 2] = value >> 16;
    buffer[*index + 3] = value >> 24;
    *index += 4;
}

number = 10.5
scale = 1000.0

value = 10.5 * 1000 = 10500 = 0x00002934

字节布局:
  buffer[0] = 0x34
  buffer[1] = 0x29
  buffer[2] = 0x00
  buffer[3] = 0x00

// 定义VESC电机
vesc_motor_handle_t vesc;

// 初始化（设备ID=1，使用CAN1）
vesc_motor_init(&vesc, 1, can1_selected);

// 设置占空比50%
vesc_motor_set_duty(&vesc, 0.5);

// 设置电流10.5A
vesc_motor_set_current(&vesc, 10.5);

// 设置转速5000 ERPM
vesc_motor_set_erpm(&vesc, 5000.0);

// 设置位置1.57 rad (90度)
vesc_motor_set_pos(&vesc, 1.57);

// 设置电流限制（最小-20A，最大20A，临时）
vesc_motor_set_current_limit(&vesc, -20.0, 20.0, false);

// 设置电流限制（永久保存到ROM）
vesc_motor_set_current_limit(&vesc, -20.0, 20.0, true);

// 读取状态
float voltage = vesc.input_voltage;
float current = vesc.motor_current;
float rpm = vesc.erpm;
float temp = vesc.mosfet_temperature;

typedef struct {
    uint8_t head[2];      // 包头：0xFE, 0xEE (2字节)
    RIS_Mode_t mode;      // 控制模式 (1字节)
    RIS_Comd_t comd;      // 控制参数 (12字节)
    uint16_t CRC16;       // CRC校验 (2字节)
} RIS_ControlData_t;     // 总共17字节

typedef struct {
    uint8_t head[2];      // 包头：0xFD, 0xEE (2字节)
    RIS_Mode_t mode;      // 控制模式 (1字节)
    RIS_Fbk_t fbk;        // 反馈数据 (11字节)
    uint16_t CRC16;       // CRC校验 (2字节)
} RIS_MotorData_t;       // 总共16字节

typedef struct {
    uint8_t id : 4;       // 电机ID: 0-14, 15为广播
    uint8_t status : 3;   // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留
    uint8_t reserve : 1;  // 保留位
} RIS_Mode_t;             // 1字节

字节布局:
  位0-3:   id (4位, 0-15)
  位4-6:   status (3位, 0-7)
  位7:     reserve (1位)

typedef struct {
    int16_t tor_des;      // 期望扭矩 (N·m)  Q8格式
    int16_t spd_des;      // 期望速度 (rad/s) Q8格式
    int32_t pos_des;      // 期望位置 (rad)   Q15格式
    int16_t k_pos;        // 刚度系数 (-1.0~1.0) Q15格式
    int16_t k_spd;        // 阻尼系数 (-1.0~1.0) Q15格式
} RIS_Comd_t;             // 12字节

typedef struct {
    int16_t torque;       // 实际扭矩 (N·m)   Q8格式
    int16_t speed;        // 实际速度 (rad/s) Q8格式
    int32_t pos;          // 实际位置 (rad)   Q15格式
    int8_t temp;          // 温度 (°C)        整数
    uint8_t MError : 3;   // 错误标识
    uint16_t force : 12;  // 足端力传感器 (0-4095)
    uint8_t none : 1;     // 保留位
} RIS_Fbk_t;              // 11字节

typedef struct {
    uint8_t motor_id;     // 电机ID
    uint8_t mode;         // 0:空闲 1:FOC控制 2:电机标定
    int Temp;             // 温度
    int MError;           // 错误码
    float T;              // 实际扭矩 (N·m)
    float W;              // 实际速度 (rad/s)
    float Pos;            // 实际位置 (rad)
    float offset_angle;   // 上电角度偏移
    int correct;          // 数据完整性标志 (0/1)
    int footForce;        // 足端力传感器
    bool got_offset;      // 是否已获取偏移

    uint16_t calc_crc;    // 计算的CRC
    uint32_t bad_msg;     // CRC错误次数
} unitree_motor_handle_t;

// RS485收发控制（GPIO控制）
#define RS485_REDE_Pin       GPIO_PIN_8
#define RS485_REDE_GPIO_Port GPIOA

// 接收模式（低电平）
#define RS485_RxMode() \
    (HAL_GPIO_WritePin(RS485_REDE_GPIO_Port, RS485_REDE_Pin, GPIO_PIN_RESET))

// 发送模式（高电平）
#define RS485_TxMode() \
    (HAL_GPIO_WritePin(RS485_REDE_GPIO_Port, RS485_REDE_Pin, GPIO_PIN_SET))

发送数据：
  1. RS485_TxMode()  → 切换到发送模式
  2. UART发送数据
  3. 发送完成中断 → RS485_RxMode()

接收数据：
  1. RS485_RxMode()  → 切换到接收模式
  2. UART接收数据
  3. 处理接收数据

const uint16_t crc_ccitt_table[256] = {
    0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf,
    0x8c48, 0x9dc1, 0xaf5a, 0xed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7,
    // ... 共256个表项
};

// 单字节CRC计算
uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c) {
    return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff];
}

// 数据块CRC计算
uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len) {
    while (len--)
        crc = crc_ccitt_byte(crc, *buffer++);
    return crc;
}

// 计算控制数据包CRC
uint16_t crc = crc_ccitt(0, (uint8_t *)&motor_send_data,
                         sizeof(RIS_ControlData_t) - sizeof(uint16_t));

// 校验反馈数据包CRC
uint16_t calc_crc = crc_ccitt(0, (uint8_t *)&motor_recv_data,
                               sizeof(RIS_MotorData_t) - sizeof(uint16_t));
if (motor_recv_data.CRC16 != calc_crc) {
    // CRC错误
}

// 消息结点
typedef struct rs_node {
    void *rs_data;        // 电机结构体指针
    uint8_t id;           // 电机ID
    struct rs_node *next; // 下一个结点（链表）
} rs_node_t;

// 哈希表
typedef struct {
    rs_node_t **table;     // 桶数组
    uint8_t len;           // 桶数量
} table_t;

// 全局哈希表
table_t *rs_table = NULL;

uint8_t rs_list_init(uint8_t len) {
    // 分配表结构
    rs_table = (table_t *)calloc(1, sizeof(table_t));
    if (rs_table == NULL) {
        return 1;
    }

    // 分配桶数组
    rs_table->table = (rs_node_t **)calloc(len, sizeof(rs_node_t *));
    if (rs_table->table == NULL) {
        return 2;
    }

    rs_table->len = len;
    return 0;
}

uint8_t rs_list_add_new_node(void *node_data, uint8_t id) {
    // 分配新节点
    rs_node_t *new_node = (rs_node_t *)malloc(sizeof(rs_node_t));
    if (new_node == NULL) {
        return 2;
    }

    // 设置节点数据
    new_node->rs_data = node_data;
    new_node->id = id;

    // 使用链地址法处理冲突
    uint8_t bucket_index = id % rs_table->len;
    new_node->next = rs_table->table[bucket_index];
    rs_table->table[bucket_index] = new_node;

    return 0;
}

假设 len = 4, 添加电机ID: 1, 5, 9, 2

rs_table->table[0]: NULL
rs_table->table[1]: node(id=1) -> node(id=5) -> node(id=9) -> NULL
rs_table->table[2]: node(id=2) -> NULL
rs_table->table[3]: NULL

uint8_t unitree_motor_init(unitree_motor_handle_t *motor, uint8_t motor_id,
                           uint8_t mode) {
    if (motor == NULL) {
        return 1;
    }

    motor->motor_id = motor_id;
    motor->mode = mode;
    motor->got_offset = false;

    // 添加到哈希表
    rs_list_add_new_node(motor, motor_id);

    // 切换到接收模式
    RS485_RxMode();
    return 0;
}