feat: 初始化

crates/dji-esc/src/codec.rs

@@ -0,0 +1,249 @@
+//! 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);
+    }
+}