chore: 导入 Simba-OmniInfantry 基线(云台/底盘双工程)
- 云台(ROBOT_TYPE_GIMBAL)与底盘(ROBOT_TYPE_CHASSIS)完整固件源码 - 新增根级 .gitignore,排除 Keil MDK-ARM 构建产物(.o/.d/.crf/.axf/.lnp/.map/.htm/.sct/.dep/.iex/.lst 等) 与 *.uvguix.* / DebugConfig/(与机器绑定的 UI 布局) - core.autocrlf=false,避免固件源码在检出时被改动换行符
This commit is contained in:
@@ -0,0 +1,126 @@
|
||||
/* =============================================================================
|
||||
* 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_voltage_high() 0x2FF 电压帧(GM6020 ID5~7)
|
||||
* 注意 0x1FF 帧为 M3508/M2006 高位电流与 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 = fdb->raw_torque * -3.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);
|
||||
}
|
||||
|
||||
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_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);
|
||||
}
|
||||
@@ -0,0 +1,51 @@
|
||||
#ifndef DJI_MOTOR_H
|
||||
#define DJI_MOTOR_H
|
||||
|
||||
#include "bsp_can.h"
|
||||
|
||||
/* DJI 电机(M3508/M2006/GM6020)统一驱动
|
||||
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
|
||||
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
|
||||
|
||||
typedef enum
|
||||
{
|
||||
DJI_MOTOR_3508, // 减速比 3591:187(≈19:1), 转矩常数 0.3N·m/A
|
||||
DJI_MOTOR_2006, // 减速比 36:1, 转矩常数 0.18N·m/A
|
||||
DJI_MOTOR_6020, // 直驱, 电压控制
|
||||
} dji_motor_type_e;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t raw_angle; // 本次机械角度原始数据(0~8191 对应 0~360°)
|
||||
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
|
||||
float rotor_angle_deg; // 累计转子机械角(°)
|
||||
int16_t rotor_speed; // 转子转速(RPM)
|
||||
int16_t raw_torque; // 转矩电流/转矩电压原始数据
|
||||
float torque_current; // 转矩电流(A)
|
||||
uint8_t temperature; // 电机温度(℃)
|
||||
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
|
||||
} dji_motor_feedback_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
dji_motor_type_e type;
|
||||
hcan_t * hcan;
|
||||
uint16_t rx_id; // 反馈 ID:M3508/M2006 0x201~0x208,GM6020 0x205~0x20B
|
||||
dji_motor_feedback_t fdb;
|
||||
} dji_motor_t;
|
||||
|
||||
/* 初始化并注册 CAN 接收回调(can_start() 之后调用) */
|
||||
extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_motor_type_e type);
|
||||
|
||||
/* ---- 控制帧发送 ----
|
||||
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
|
||||
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000)
|
||||
* GM6020 ID5~7:0x2FF 电压帧 */
|
||||
extern void dji_motor_send_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
|
||||
extern void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
|
||||
extern void dji_motor_send_voltage_high(hcan_t * hcan, int16_t v5, int16_t v6, int16_t v7);
|
||||
|
||||
/* 通用 int16x4 发送(板间通讯等复用) */
|
||||
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user