Simba-OmniInfantry/底盘/modules/motor/dji_motor.c

/* =============================================================================
 * dji_motor.c - DJI 电机统一驱动(M3508/M2006/GM6020)
 * 反馈路径(中断上下文):
 *   dji_motor_init() 注册 CAN 回调 --> can_rx_callback() 按 rx_id 分发
 *   --> dji_motor_parse(): 角度过零判定(+/-4096)多圈累计 total_tick
 *       (无 legacy 的 +-1 圈限制,底盘小陀螺时 Yaw 可连续旋转),
 *       记录 last_online_tick 供掉线检测
 * 控制路径(任务上下文):
 *   dji_motor_send_current_low/high()       0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
 *   dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE 电流帧(GM6020 +/-16384 = 3A)
 *   dji_motor_send_voltage_high()           0x2FF 电压帧(GM6020 旧协议)
 * ========================================================================== */
#include "dji_motor.h"

#define DJI_MOTOR_MAX 12

/* legacy 中的多圈判断以 4000 为阈值;这里改用对称的 ±4096 半圈阈值 */
#define DJI_MOTOR_HALF_TURN 4096

static dji_motor_t * dji_motor_list[DJI_MOTOR_MAX];
static uint8_t dji_motor_count = 0;

static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);

/* 解析一帧反馈,多圈累计(无 ±1 圈限制,底盘小陀螺时 Yaw 电机可连续旋转) */
static void dji_motor_parse(dji_motor_t * motor, const uint8_t data[8])
{
    dji_motor_feedback_t * fdb = &motor->fdb;
    uint16_t now_angle = (uint16_t)(((uint16_t)data[0] << 8) | data[1]);

    if (fdb->last_online_tick != 0)
    {
        int16_t diff = (int16_t)now_angle - (int16_t)fdb->raw_angle;
        if (diff > DJI_MOTOR_HALF_TURN)          // 反向过零(如 8100 -> 100)
            fdb->total_tick += diff - 8192;
        else if (diff < -DJI_MOTOR_HALF_TURN)    // 正向过零(如 100 -> 8100)
            fdb->total_tick += diff + 8192;
        else
            fdb->total_tick += diff;
    }
    fdb->raw_angle = now_angle;
    fdb->rotor_angle_deg = (float)fdb->total_tick * 0.0439453125f;  // /8192*360
    fdb->rotor_speed = (int16_t)(((uint16_t)data[2] << 8) | data[3]);
    fdb->raw_torque = (int16_t)(((uint16_t)data[4] << 8) | data[5]);
    fdb->temperature = data[6];

    switch (motor->type)
    {
        case DJI_MOTOR_3508:
            fdb->torque_current = fdb->raw_torque * 0.001220703125f;  // /16384*20
            break;
        case DJI_MOTOR_2006:
            fdb->torque_current = fdb->raw_torque * 0.001f;  // /10000*10
            break;
        case DJI_MOTOR_6020:
            fdb->torque_current = (float)fdb->raw_torque * (3.0f / 16384.0f);  // 实际转矩电流,±3A 量程
            break;
    }
    fdb->last_online_tick = HAL_GetTick();
}

void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_motor_type_e type)
{
    motor->type = type;
    motor->hcan = hcan;
    motor->rx_id = rx_id;
    motor->fdb.raw_angle = 0;
    motor->fdb.total_tick = 0;
    motor->fdb.last_online_tick = 0;  // 0 表示尚未收到反馈

    if (dji_motor_count < DJI_MOTOR_MAX)
    {
        dji_motor_list[dji_motor_count++] = motor;
        can_rx_register(hcan, rx_id, dji_motor_rx_callback);
    }
}

static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
{
    (void)dlc;
    for (uint8_t i = 0; i < dji_motor_count; i++)
    {
        if (dji_motor_list[i]->rx_id == std_id)
        {
            dji_motor_parse(dji_motor_list[i], rx_data);
            return;
        }
    }
}

/* ---- 控制帧 ---- */

void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4])
{
    CanCtrlData_s msg;
    msg.hcan = hcan;
    msg.tx_header.StdId = std_id;
    msg.tx_header.IDE = CAN_ID_STD;
    msg.tx_header.RTR = CAN_RTR_DATA;
    msg.tx_header.DLC = 8;
    msg.tx_header.TransmitGlobalTime = DISABLE;
    for (uint8_t i = 0; i < 4; i++)
    {
        msg.tx_data[2 * i] = (uint8_t)(val[i] >> 8);
        msg.tx_data[2 * i + 1] = (uint8_t)(val[i] & 0xFF);
    }
    CAN_SendTxMessage(&msg);
}

static inline int16_t dji_motor_clamp16(int16_t val, int16_t min_val, int16_t max_val)
{
    if (val > max_val) return max_val;
    if (val < min_val) return min_val;
    return val;
}

void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
    const int16_t v[4] = {i1, i2, i3, i4};
    dji_motor_send_int16x4(hcan, 0x200, v);
}

void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8)
{
    const int16_t v[4] = {i5, i6, i7, i8};
    dji_motor_send_int16x4(hcan, 0x1FF, v);
}

void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4)
{
    const int16_t v[4] = {
        dji_motor_clamp16(i1, -16384, 16384),
        dji_motor_clamp16(i2, -16384, 16384),
        dji_motor_clamp16(i3, -16384, 16384),
        dji_motor_clamp16(i4, -16384, 16384)
    };
    dji_motor_send_int16x4(hcan, 0x1FE, v);
}

void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7)
{
    const int16_t v[4] = {
        dji_motor_clamp16(i5, -16384, 16384),
        dji_motor_clamp16(i6, -16384, 16384),
        dji_motor_clamp16(i7, -16384, 16384),
        0
    };
    dji_motor_send_int16x4(hcan, 0x2FE, v);
}

void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7)
{
    const int16_t v[4] = {v5, v6, v7, 0};
    dji_motor_send_int16x4(hcan, 0x2FF, v);
}