250 lines
8.0 KiB
Rust
250 lines
8.0 KiB
Rust
//! DJI 电调（C620 / C610）CAN 协议编解码。
|
||
//!
|
||
//! 帧格式（大端）：
|
||
//!
|
||
//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）：
|
||
//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令，
|
||
//! 槽位按组内电调顺序。
|
||
//! - 反馈（`0x200 + 电调ID`）：
|
||
//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。
|
||
//!
|
||
//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在
|
||
//! [`crate::bus`] 中按节拍调用。
|
||
|
||
use crate::frame::{
|
||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE,
|
||
};
|
||
use thiserror::Error;
|
||
|
||
/// 电调 ID 下限。
|
||
pub const MIN_ESC_ID: u8 = 1;
|
||
/// 电调 ID 上限。
|
||
pub const MAX_ESC_ID: u8 = 8;
|
||
|
||
/// 电调型号决定电流标度。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub enum EscModel {
|
||
/// C620（配 GM3508）：±16384 ↔ ±20 A。
|
||
C620,
|
||
/// C610（配 M2006）：±10000 ↔ ±10 A。
|
||
///
|
||
/// 接入前需实测核对。
|
||
C610,
|
||
}
|
||
|
||
impl EscModel {
|
||
/// 满量程电流命令（raw）。
|
||
#[must_use]
|
||
pub const fn max_current_raw(self) -> i16 {
|
||
match self {
|
||
Self::C620 => 16384,
|
||
Self::C610 => 10_000,
|
||
}
|
||
}
|
||
|
||
/// 满量程电流（安培）。
|
||
#[must_use]
|
||
pub const fn max_current_amp(self) -> f32 {
|
||
match self {
|
||
Self::C620 => 20.0,
|
||
Self::C610 => 10.0,
|
||
}
|
||
}
|
||
|
||
/// 安培 → raw（限幅）。
|
||
#[must_use]
|
||
pub fn raw_from_amp(self, amp: f32) -> i16 {
|
||
let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp();
|
||
(raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16)
|
||
.clamp(-self.max_current_raw(), self.max_current_raw())
|
||
}
|
||
|
||
/// raw → 安培。
|
||
#[must_use]
|
||
pub fn amp_from_raw(self, raw: i16) -> f32 {
|
||
f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw())
|
||
}
|
||
}
|
||
|
||
/// 一帧电调反馈（原始、电调侧，未经方向修正）。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub struct EscFeedback {
|
||
/// 转子机械角原始值（0..8191，对应 0..360°）。
|
||
pub angle_raw: u16,
|
||
/// 转子转速（rpm，电调侧符号）。
|
||
pub speed_rpm: i16,
|
||
/// 转矩电流 raw（电调侧符号）。
|
||
pub current_raw: i16,
|
||
/// 电机温度（℃）。
|
||
pub temperature_c: i8,
|
||
}
|
||
|
||
/// 帧解码错误。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||
pub enum DecodeError {
|
||
/// 帧不属于电调反馈范围（0x201..=0x208）。
|
||
#[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")]
|
||
NotFeedback {
|
||
/// 收到的帧 ID。
|
||
id: u32,
|
||
},
|
||
}
|
||
|
||
/// 电调 ID → 其电流命令所在的组控制帧 ID。
|
||
///
|
||
/// 返回 `None` 表示 ID 越界。
|
||
#[must_use]
|
||
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
|
||
match esc_id {
|
||
1..=4 => Some(COMMAND_FRAME_ID_PRIMARY),
|
||
5..=8 => Some(COMMAND_FRAME_ID_SECONDARY),
|
||
_ => None,
|
||
}
|
||
}
|
||
|
||
/// 将 raw 电流限幅到电调满量程。
|
||
#[must_use]
|
||
pub const fn clamp_current(model: EscModel, raw: i16) -> i16 {
|
||
if raw > model.max_current_raw() {
|
||
model.max_current_raw()
|
||
} else if raw < -model.max_current_raw() {
|
||
-model.max_current_raw()
|
||
} else {
|
||
raw
|
||
}
|
||
}
|
||
|
||
/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。
|
||
///
|
||
/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。
|
||
/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成
|
||
/// （见 [`clamp_current`]）。
|
||
#[must_use]
|
||
pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame {
|
||
let mut data = [0u8; 8];
|
||
for (slot, raw) in currents.iter().enumerate() {
|
||
data[slot * 2] = (raw >> 8) as u8;
|
||
data[slot * 2 + 1] = (raw & 0xFF) as u8;
|
||
}
|
||
CanFrame::new(frame_id, data)
|
||
}
|
||
|
||
/// 组装一帧电调反馈（仿真器 / 测试用）。
|
||
#[must_use]
|
||
pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame {
|
||
let mut data = [0u8; 8];
|
||
data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
|
||
data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
|
||
data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
|
||
data[6] = feedback.temperature_c as u8;
|
||
CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
|
||
}
|
||
|
||
/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。
|
||
///
|
||
/// # Errors
|
||
///
|
||
/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。
|
||
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
|
||
if frame.id <= FEEDBACK_FRAME_ID_BASE
|
||
|| frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID)
|
||
{
|
||
return Err(DecodeError::NotFeedback { id: frame.id });
|
||
}
|
||
let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8;
|
||
Ok((
|
||
esc_id,
|
||
EscFeedback {
|
||
angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]),
|
||
speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]),
|
||
current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]),
|
||
temperature_c: frame.data[6] as i8,
|
||
},
|
||
))
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn command_frame_id_routing() {
|
||
assert_eq!(command_frame_id(1), Some(0x200));
|
||
assert_eq!(command_frame_id(4), Some(0x200));
|
||
assert_eq!(command_frame_id(5), Some(0x1FF));
|
||
assert_eq!(command_frame_id(8), Some(0x1FF));
|
||
assert_eq!(command_frame_id(0), None);
|
||
assert_eq!(command_frame_id(9), None);
|
||
}
|
||
|
||
#[test]
|
||
fn encode_current_frame_bytes() {
|
||
let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]);
|
||
assert_eq!(frame.id, 0x200);
|
||
assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]);
|
||
|
||
let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]);
|
||
assert_eq!(frame.id, 0x1FF);
|
||
assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]);
|
||
}
|
||
|
||
#[test]
|
||
fn decode_feedback_known_bytes() {
|
||
let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||
let (esc_id, fb) = decode_feedback(&frame).unwrap();
|
||
assert_eq!(esc_id, 5);
|
||
assert_eq!(fb.angle_raw, 8191);
|
||
assert_eq!(fb.speed_rpm, -364);
|
||
assert_eq!(fb.current_raw, 123);
|
||
assert_eq!(fb.temperature_c, 45);
|
||
}
|
||
|
||
#[test]
|
||
fn decode_feedback_rejects_non_feedback_ids() {
|
||
for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] {
|
||
let frame = CanFrame::new(id, [0; 8]);
|
||
assert_eq!(
|
||
decode_feedback(&frame),
|
||
Err(DecodeError::NotFeedback { id }),
|
||
"id = 0x{id:03X}"
|
||
);
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn feedback_roundtrip_all_ids() {
|
||
let fb = EscFeedback {
|
||
angle_raw: 8191,
|
||
speed_rpm: -9254,
|
||
current_raw: -16384,
|
||
temperature_c: -40,
|
||
};
|
||
for esc_id in MIN_ESC_ID..=MAX_ESC_ID {
|
||
let frame = encode_feedback_frame(esc_id, &fb);
|
||
assert_eq!(frame.id, 0x200 + u32::from(esc_id));
|
||
assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb));
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn amp_conversion() {
|
||
let c620 = EscModel::C620;
|
||
assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4);
|
||
assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4);
|
||
assert_eq!(c620.raw_from_amp(20.0), 16384);
|
||
assert_eq!(c620.raw_from_amp(-5.0), -4096);
|
||
// 超量程限幅。
|
||
assert_eq!(c620.raw_from_amp(99.0), 16384);
|
||
let c610 = EscModel::C610;
|
||
assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4);
|
||
assert_eq!(c610.raw_from_amp(10.0), 10000);
|
||
}
|
||
|
||
#[test]
|
||
fn clamp_current_respects_model() {
|
||
assert_eq!(clamp_current(EscModel::C620, 20000), 16384);
|
||
assert_eq!(clamp_current(EscModel::C610, 15000), 10000);
|
||
assert_eq!(clamp_current(EscModel::C620, 1000), 1000);
|
||
}
|
||
}
|