ELCT/Simba-OmniInfantry
9
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

电流环控制，并实现简单的补偿控制，还差参数具体化。

Browse Source
This commit is contained in:
lyf
2026-10-16 00:54:52 +08:00
parent 67eea77789
commit 370d4519b5
20 changed files with 477 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
云台/modules/motor/dji_motor.c
+32 -10
View File
@@ -6,9 +6,8 @@
* (无 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 低位电压共用,按槽位区分
* dji_motor_send_current_low/high() 0x200 / 0x1FF 电流帧(+/-16384 = 20A/10A)
* dji_motor_send_gm6020_current_low/high() 0x1FE / 0x2FE GM6020 电流帧(+/-16384 = 3A)
* ========================================================================== */
#include "dji_motor.h"
@@ -20,7 +19,8 @@
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);
static void dji_motor_can1_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc);
static void dji_motor_can2_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])
@@ -71,16 +71,32 @@ void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, dji_moto
if (dji_motor_count < DJI_MOTOR_MAX)
{
dji_motor_list[dji_motor_count++] = motor;
can_rx_register(hcan, rx_id, dji_motor_rx_callback);
if (hcan == &hcan1)
can_rx_register(hcan, rx_id, dji_motor_can1_rx_callback);
else
can_rx_register(hcan, rx_id, dji_motor_can2_rx_callback);
}
}
static void dji_motor_rx_callback(uint32_t std_id, const uint8_t rx_data[8], uint32_t dlc)
static void dji_motor_can1_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)
if (dji_motor_list[i]->hcan == &hcan1 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
}
}
}
static void dji_motor_can2_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]->hcan == &hcan2 && dji_motor_list[i]->rx_id == std_id)
{
dji_motor_parse(dji_motor_list[i], rx_data);
return;
@@ -119,8 +135,14 @@ void dji_motor_send_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t
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)
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] = {v5, v6, v7, 0};
dji_motor_send_int16x4(hcan, 0x2FF, v);
const int16_t v[4] = {i1, i2, i3, i4};
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, int16_t i8)
{
const int16_t v[4] = {i5, i6, i7, i8};
dji_motor_send_int16x4(hcan, 0x2FE, v);
}
云台/modules/motor/dji_motor.h
+7 -6
View File
@@ -5,13 +5,13 @@
/* DJI 电机(M3508/M2006/GM6020)统一驱动
* - 反馈:can_start() 后通过 can_rx_register 按 rx_id 注册,接收中断内解析(多圈累计)
* - 控制:组装 0x200/0x1FF/0x2FF 控制帧,经 CAN_SendTxMessage 发送 */
* - 控制:组装 0x200/0x1FF/0x1FE/0x2FE 控制帧,经 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_6020, // 直驱, 电流控制(ID1~4 为 0x1FE 帧, ±16384 对应 ±3A)
} dji_motor_type_e;
typedef struct
@@ -20,7 +20,7 @@ typedef struct
int64_t total_tick; // 累计角度位置原始数据(8192*圈数+raw,无 ±1 圈限制)
float rotor_angle_deg; // 累计转子机械角(°)
int16_t rotor_speed; // 转子转速(RPM)
int16_t raw_torque; // 转矩电流/转矩电压原始数据
int16_t raw_torque; // 转矩电流原始数据
float torque_current; // 转矩电流(A)
uint8_t temperature; // 电机温度(℃)
uint32_t last_online_tick; // 最近一次收到反馈的 tick(HAL_GetTick)
@@ -39,11 +39,12 @@ extern void dji_motor_init(dji_motor_t * motor, hcan_t * hcan, uint16_t rx_id, d
/* ---- 控制帧发送 ----
* M3508/M2006 ID1~4:0x200 电流帧(±16384 对应 ±20A/±10A)
* M3508/M2006 ID5~8:0x1FF 电流帧;GM6020 ID1~4:0x1FF 电压帧(±25000)
* GM6020 ID5~7:0x2FF 电压帧 */
* M3508/M2006 ID5~8:0x1FF 电流帧(±16384)
* GM6020 电流控制:ID1~4 为 0x1FE 电流帧(±16384 对应 ±3A),ID5~8 为 0x2FE 电流帧(±16384) */
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);
extern void dji_motor_send_gm6020_current_low(hcan_t * hcan, int16_t i1, int16_t i2, int16_t i3, int16_t i4);
extern void dji_motor_send_gm6020_current_high(hcan_t * hcan, int16_t i5, int16_t i6, int16_t i7, int16_t i8);
/* 通用 int16x4 发送(板间通讯等复用) */
extern void dji_motor_send_int16x4(hcan_t * hcan, uint32_t std_id, const int16_t val[4]);