149 lines
5.2 KiB
C
149 lines
5.2 KiB
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)
|
|
* ========================================================================== */
|
|
#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_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])
|
|
{
|
|
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;
|
|
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_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]->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;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ---- 控制帧 ---- */
|
|
|
|
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_gm6020_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, 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);
|
|
}
|