refactor: 重构架构并整理仓库

crates/motor-protocol/Cargo.toml

@@ -0,0 +1,18 @@
+[package]
+name = "motor-protocol"
+description = "DJI C620/C610 and GM6020 wire protocols"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+can = { workspace = true }
+thiserror = { workspace = true }
+
+[dev-dependencies]
+proptest = { workspace = true }
+approx = { workspace = true }

crates/motor-protocol/src/lib.rs

@@ -0,0 +1,911 @@
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+//! DJI motor wire protocols.
+
+pub mod common {
+    //! 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-control` 的算法在
+    //! `motor / esc-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,
+        },
+        /// 数据长度不足 8 字节（协议载荷固定 8 字节）。
+        #[error("frame carries only {dlc} bytes; the ESC feedback payload needs 8")]
+        ShortFrame {
+            /// 实际数据长度码。
+            dlc: u8,
+        },
+    }
+
+    /// 电调 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,
+        }
+    }
+
+    /// 电调 ID → 其反馈帧 ID（`0x200 + 电调ID`）。
+    ///
+    /// 返回 `None` 表示 ID 越界。GM6020 用**另一张表**，见
+    /// [`crate::gm6020::feedback_frame_id`]。
+    #[must_use]
+    pub const fn feedback_frame_id(esc_id: u8) -> Option<u32> {
+        if esc_id >= MIN_ESC_ID && esc_id <= MAX_ESC_ID {
+            Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32)
+        } else {
+            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`]；
+    /// 数据长度不足 8 字节时返回 [`DecodeError::ShortFrame`]。
+    pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
+        if !frame.is_complete() {
+            return Err(DecodeError::ShortFrame { dlc: frame.dlc });
+        }
+        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);
+        }
+    }
+}
+
+pub mod gm6020 {
+    //! GM6020 云台电机协议（ARCHITECTURE.md §5.2）。
+    //!
+    //! 与 C620/C610 的差别只有两点，因此两个协议族共享同一套帧布局：
+    //!
+    //! | 项目 | C620/C610（M3508/M2006） | GM6020 |
+    //! | --- | --- | --- |
+    //! | 控制量 | **电流**（±16384 / ±10000 ↔ 满量程电流） | **电压**（±30000 ↔ 满电压） |
+    //! | 控制帧 ID | `0x200`（1–4）、`0x1FF`（5–8） | `0x1FF`（1–4）、`0x2FF`（5–8） |
+    //! | 反馈帧 ID | `0x200 + ID`（0x201–0x208） | `0x204 + ID`（0x205–0x208，ID 1–4） |
+    //! | 反馈载荷 | u16 角度 + i16 转速 + i16 电流 + i8 温度 | 同上 |
+    //!
+    //! > 命名说明：ARCHITECTURE.md §5.2 把命令字段写作 `current`，但 GM6020 的
+    //! > 控制字是**电压**占空比命令（±30000）。这里用 `voltage_raw` 以免把
+    //! > "电流环在电调内部"这件事记反——GM6020 内部同样只闭电流环。
+
+    use can::CanFrame;
+
+    use crate::common::{DecodeError, MAX_ESC_ID, MIN_ESC_ID};
+    use crate::id::MotorId;
+
+    /// 控制帧 ID：GM6020 ID 1–4。
+    pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x1FF;
+    /// 控制帧 ID：GM6020 ID 5–8。
+    pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x2FF;
+    /// 反馈帧 ID 基址：`0x204 + ID`（ID 1–4 → 0x205–0x208）。
+    pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x204;
+    /// 电压命令满量程（raw）。
+    pub const VOLTAGE_MAX_RAW: i16 = 30_000;
+    /// 编码器单圈刻度（与 C620 相同）。
+    pub const ENCODER_TICKS: u16 = 8192;
+
+    /// GM6020 一帧反馈。
+    #[derive(Debug, Clone, Copy, PartialEq)]
+    pub struct Gm6020Feedback {
+        /// 机械角（度，0..360）。
+        pub angle_deg: f32,
+        /// 单圈原始角度（0..8191）。
+        pub angle_raw: u16,
+        /// 转子转速（rpm）。
+        pub speed_rpm: i16,
+        /// 转矩电流（raw，±30000 ↔ 满量程）。
+        pub current_raw: i16,
+        /// 温度（℃）。
+        pub temperature_c: i8,
+    }
+
+    /// GM6020 电压命令。
+    #[derive(Debug, Clone, Copy, PartialEq, Eq)]
+    pub struct Gm6020Command {
+        /// 电压命令（raw，`-30000..=30000`）。
+        pub voltage_raw: i16,
+    }
+
+    impl Gm6020Command {
+        /// 构造并夹到合法范围。
+        #[must_use]
+        pub const fn new(voltage_raw: i16) -> Self {
+            Self { voltage_raw: clamp_voltage(voltage_raw) }
+        }
+
+        /// 编码为**广播控制帧**（同组四个槽位）。
+        ///
+        /// # Errors
+        ///
+        /// 帧 ID 非法时返回 [`DecodeError::NotFeedback`] 之外的 `BadFrameId`。
+        ///
+        /// 说明：单电机编码只是"其余槽位填 0"的特例，因此本函数要求调用方
+        /// 提供整组电压；单电机场景见 [`Gm6020Command::to_frame`]。
+        #[must_use]
+        pub fn encode_group(frame_id: u32, voltages: [i16; 4]) -> CanFrame {
+            encode_voltage_frame(frame_id, voltages)
+        }
+
+        /// 单电机编码：其余槽位补 0（会**覆盖**同组其它电机，仅用于单机联调）。
+        ///
+        /// 注意用的是 **GM6020 自己的**控制帧映射（`0x1FF` / `0x2FF`），
+        /// 而不是 [`MotorId::command_frame_id`] 的 C620 映射（`0x200` / `0x1FF`）——
+        /// 两个协议族的帧 ID 表不同，混淆会导致云台收不到任何命令。
+        #[must_use]
+        pub fn to_frame<const ID: u8>(&self, id: MotorId<ID>) -> CanFrame {
+            let esc_id = id.get();
+            let mut voltages = [0i16; 4];
+            let slot = command_slot(esc_id).unwrap_or(0);
+            voltages[slot] = self.voltage_raw;
+            let frame_id = command_frame_id(esc_id).unwrap_or(COMMAND_FRAME_ID_PRIMARY);
+            encode_voltage_frame(frame_id, voltages)
+        }
+    }
+
+    /// 夹到电压满量程。
+    #[must_use]
+    pub const fn clamp_voltage(raw: i16) -> i16 {
+        // `i16::clamp` 在 const fn 里尚不可用（Ord 未 const 稳定），手写三段。
+        if raw > VOLTAGE_MAX_RAW {
+            VOLTAGE_MAX_RAW
+        } else if raw < -VOLTAGE_MAX_RAW {
+            -VOLTAGE_MAX_RAW
+        } else {
+            raw
+        }
+    }
+
+    /// 由电调 ID 推出控制帧槽位（`None` 表示 ID 越界）。
+    #[must_use]
+    pub const fn command_slot(esc_id: u8) -> Option<usize> {
+        if esc_id >= MIN_ESC_ID && esc_id <= 4 {
+            Some((esc_id - 1) as usize)
+        } else if esc_id >= 5 && esc_id <= MAX_ESC_ID {
+            Some((esc_id - 5) as usize)
+        } else {
+            None
+        }
+    }
+
+    /// 由电调 ID 推出控制帧 ID（`None` 表示 ID 越界）。
+    #[must_use]
+    pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
+        match command_slot(esc_id) {
+            Some(_) if esc_id <= 4 => Some(COMMAND_FRAME_ID_PRIMARY),
+            Some(_) => Some(COMMAND_FRAME_ID_SECONDARY),
+            None => None,
+        }
+    }
+
+    /// 反馈帧 ID（`0x204 + ID`；ID 5–8 为 `0x208 + ID`，见模块文档）。
+    #[must_use]
+    pub const fn feedback_frame_id(esc_id: u8) -> Option<u32> {
+        if esc_id >= MIN_ESC_ID && esc_id <= 4 {
+            Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32)
+        } else if esc_id >= 5 && esc_id <= MAX_ESC_ID {
+            Some(0x208 + esc_id as u32)
+        } else {
+            None
+        }
+    }
+
+    /// 编码一组电压命令（4 × i16 大端）。
+    #[must_use]
+    pub fn encode_voltage_frame(frame_id: u32, voltages: [i16; 4]) -> CanFrame {
+        let mut data = [0u8; 8];
+        for (slot, raw) in voltages.iter().enumerate() {
+            let clamped = clamp_voltage(*raw);
+            data[slot * 2] = (clamped >> 8) as u8;
+            data[slot * 2 + 1] = (clamped & 0xFF) as u8;
+        }
+        CanFrame::new(frame_id, data)
+    }
+
+    /// 组装一帧 GM6020 反馈（仿真器 / 测试用）。
+    #[must_use]
+    pub fn encode_feedback_frame(esc_id: u8, feedback: &Gm6020Feedback) -> 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)
+    }
+
+    /// 解码一帧 GM6020 反馈，返回 `(电调ID, 反馈)`。
+    ///
+    /// # Errors
+    ///
+    /// 帧 ID 不在 GM6020 反馈范围（0x205–0x208、0x20D–0x210）内时返回
+    /// [`DecodeError::NotFeedback`]；长度不足时返回 [`DecodeError::ShortFrame`]。
+    pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, Gm6020Feedback), DecodeError> {
+        if !frame.is_complete() {
+            return Err(DecodeError::ShortFrame { dlc: frame.dlc });
+        }
+        let esc_id = if (0x205..=0x208).contains(&frame.id) {
+            (frame.id - FEEDBACK_FRAME_ID_BASE) as u8
+        } else if (0x20D..=0x210).contains(&frame.id) {
+            (frame.id - 0x208) as u8
+        } else {
+            return Err(DecodeError::NotFeedback { id: frame.id });
+        };
+        let angle_raw = u16::from_be_bytes([frame.data[0], frame.data[1]]);
+        let speed_rpm = i16::from_be_bytes([frame.data[2], frame.data[3]]);
+        let current_raw = i16::from_be_bytes([frame.data[4], frame.data[5]]);
+        let temperature_c = frame.data[6] as i8;
+        Ok((
+            esc_id,
+            Gm6020Feedback {
+                angle_raw: angle_raw % ENCODER_TICKS,
+                angle_deg: f32::from(angle_raw) * 360.0 / f32::from(ENCODER_TICKS),
+                speed_rpm,
+                current_raw,
+                temperature_c,
+            },
+        ))
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn voltage_is_clamped_to_full_scale() {
+            assert_eq!(Gm6020Command::new(32_000).voltage_raw, VOLTAGE_MAX_RAW);
+            assert_eq!(Gm6020Command::new(-32_000).voltage_raw, -VOLTAGE_MAX_RAW);
+            assert_eq!(Gm6020Command::new(1234).voltage_raw, 1234);
+        }
+
+        #[test]
+        fn single_motor_encoding_lands_in_the_right_slot() {
+            let frame = Gm6020Command::new(1000).to_frame(MotorId::<1>::NEW);
+            assert_eq!(frame.id, 0x1FF);
+            assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 1000);
+            assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), 0);
+
+            let frame = Gm6020Command::new(-1000).to_frame(MotorId::<6>::NEW);
+            assert_eq!(frame.id, 0x2FF, "ID 5–8 走副组帧");
+            assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -1000);
+        }
+
+        #[test]
+        fn group_encoding_matches_slot_order() {
+            let frame = encode_voltage_frame(COMMAND_FRAME_ID_PRIMARY, [10, -20, 30, -40]);
+            assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 10);
+            assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -20);
+            assert_eq!(i16::from_be_bytes([frame.data[4], frame.data[5]]), 30);
+            assert_eq!(i16::from_be_bytes([frame.data[6], frame.data[7]]), -40);
+        }
+
+        #[test]
+        fn feedback_round_trips_and_converts_angle() {
+            let feedback = Gm6020Feedback {
+                angle_deg: 0.0,
+                angle_raw: 2048,
+                speed_rpm: -321,
+                current_raw: 4567,
+                temperature_c: 42,
+            };
+            let frame = encode_feedback_frame(2, &feedback);
+            assert_eq!(frame.id, 0x206);
+            let (id, decoded) = decode_feedback(&frame).unwrap();
+            assert_eq!(id, 2);
+            assert_eq!(decoded.angle_raw, 2048);
+            assert!((decoded.angle_deg - 90.0).abs() < 1e-3, "2048/8192 应为 90°");
+            assert_eq!(decoded.speed_rpm, -321);
+            assert_eq!(decoded.current_raw, 4567);
+            assert_eq!(decoded.temperature_c, 42);
+        }
+
+        #[test]
+        fn feedback_id_range_is_enforced() {
+            assert_eq!(feedback_frame_id(1), Some(0x205));
+            assert_eq!(feedback_frame_id(4), Some(0x208));
+            assert_eq!(feedback_frame_id(9), None);
+            let c620 = CanFrame::new(0x201, [0; 8]);
+            assert!(matches!(decode_feedback(&c620), Err(DecodeError::NotFeedback { .. })));
+        }
+
+        #[test]
+        fn short_frames_are_rejected() {
+            let short = CanFrame::new(0x205, [0; 8]).with_dlc(4);
+            assert!(matches!(decode_feedback(&short), Err(DecodeError::ShortFrame { .. })));
+        }
+    }
+}
+
+pub mod m2006 {
+    //! M2006 + C610：拨弹 / 小负载电机。本项目只用到协议常量，用于未来的
+    //! 发射机构（发射机构尚未实现，见 ARCHITECTURE.md §5.7 的 `shooter` 任务）。
+
+    use crate::common::EscModel;
+
+    /// C610 电调型号（M2006 的配套电调）。
+    ///
+    /// 电流标度 ±10000 ↔ ±10 A **接入前需实测核对**（ARCHITECTURE.md §2.2 注）。
+    pub const C610: EscModel = EscModel::C610;
+
+    /// 减速比（转子 : 输出轴）＝ 36 : 1（M2006 常见配置，接硬件前核对）。
+    pub const GEAR_RATIO: f32 = 36.0;
+
+    /// 编码器单圈刻度。
+    pub const ENCODER_TICKS: u16 = 8192;
+
+    /// 额定电流满量程（A，对应 raw 满量程 ±10000）。
+    pub const MAX_CURRENT_AMP: f32 = 10.0;
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+        use crate::common::clamp_current;
+
+        #[test]
+        fn c610_saturation_uses_the_c610_scale() {
+            assert_eq!(C610.max_current_raw(), 10_000);
+            assert_eq!(clamp_current(C610, 20_000), 10_000);
+            assert_eq!(clamp_current(C610, -20_000), -10_000);
+        }
+
+        #[test]
+        fn max_current_amp_matches_the_raw_scale() {
+            assert!((C610.max_current_amp() - MAX_CURRENT_AMP).abs() < 1e-6);
+        }
+    }
+}
+
+pub mod m3508 {
+    //! GM3508 + C620：底盘驱动轮的电流协议族（本项目的主力电机）。
+    //!
+    //! 全部编解码都在 [`crate::common`]（C620 与 C610 帧布局相同，只有
+    //! 电流标度不同），本模块只固定 GM3508 的**型号常量与机械参数**，避免
+    //! 每个使用点各自抄一遍数字。
+
+    use crate::common::EscModel;
+
+    /// C620 电调型号（GM3508 的配套电调）。
+    pub const C620: EscModel = EscModel::C620;
+
+    /// 减速比（转子 : 输出轴）＝ 3591 / 187 ≈ 19.203。
+    pub const GEAR_RATIO: f32 = 3591.0 / 187.0;
+
+    /// 编码器单圈刻度。
+    pub const ENCODER_TICKS: u16 = 8192;
+
+    /// 额定电流满量程（A，对应 raw 满量程 ±16384）。
+    pub const MAX_CURRENT_AMP: f32 = 20.0;
+
+    /// 空载转速-电流增益（rpm / raw），来自台架实测（≈ 9254 / 16384）。
+    pub const NO_LOAD_GAIN_RPM_PER_RAW: f32 = 9254.0 / 16384.0;
+
+    /// 空载转速上限（rpm，转子）。
+    pub const NO_LOAD_MAX_RPM: f32 = 9254.0;
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+        use crate::common::clamp_current;
+
+        #[test]
+        fn gear_ratio_matches_the_manual() {
+            assert!((GEAR_RATIO - 19.203).abs() < 5e-3, "{GEAR_RATIO}");
+        }
+
+        #[test]
+        fn c620_saturation_uses_the_c620_scale() {
+            assert_eq!(C620.max_current_raw(), 16384);
+            assert_eq!(clamp_current(C620, 30_000), 16384);
+            assert_eq!(clamp_current(C620, -30_000), -16384);
+        }
+
+        #[test]
+        fn no_load_gain_is_the_inverse_of_the_documented_feedforward() {
+            // 台架文档里的前馈 ≈ 1.77 raw/rpm 正是 1 / 空载增益。
+            let feedforward = 1.0 / NO_LOAD_GAIN_RPM_PER_RAW;
+            assert!((feedforward - 1.77).abs() < 0.02, "{feedforward}");
+        }
+    }
+}
+
+pub mod frame {
+    //! 协议帧 ID 常量与帧类型再导出。
+    //!
+    //! 帧类型（[`CanFrame`]）由 L1 的 [`can`] 提供；本模块只保留各协议族的
+    //! **ID 约定**，因此"改 ID 常量"与"改帧结构"是两件互不影响的事。
+    //!
+    //! - C620/C610 电流协议族：本模块的常量 + [`crate::common`]；
+    //! - GM6020 电压协议族：见 [`crate::gm6020`]（ID 常量在那边）；
+    //! - DETA10 IMU：见 [`crate::delta10`]。
+
+    pub use can::CanFrame;
+
+    /// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。
+    pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200;
+    /// 控制帧 ID：电调 5–8 的电流命令。
+    pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF;
+    /// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。
+    pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200;
+}
+
+pub mod id {
+    //! 编译期电机 ID 校验（ARCHITECTURE.md §5.2）。
+    //!
+    //! 电调 ID 写错是实车上最常见的低级错误之一：ID 9 不会报错，只会**静默**
+    //! 收不到反馈。把 ID 放进 const 泛型后，"ID 必须在 1–8"变成**编译期**约束，
+    //! 接错线之前先接错类型。
+    //!
+    //! ```ignore
+    //! // 编译通过
+    //! let id = MotorId::<3>::NEW;
+    //! // 编译失败：Motor ID must be 1-8
+    //! let bad = MotorId::<9>::NEW;
+    //! ```
+
+    /// 合法的电调 ID 范围。
+    pub const MOTOR_ID_RANGE: core::ops::RangeInclusive<u8> = 1..=8;
+
+    /// 编译期已校验的电调 ID。
+    ///
+    /// 只能通过 [`MotorId::NEW`] 构造；该常量的求值会断言 ID 在 1–8 之间，
+    /// 越界即编译失败。
+    #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
+    pub struct MotorId<const ID: u8>;
+
+    impl<const ID: u8> MotorId<ID> {
+        /// 编译期校验并构造。
+        ///
+        /// # Panics
+        ///
+        /// `ID` 不在 `1..=8` 时在**编译期** panic（const 求值失败即编译错误）。
+        pub const NEW: Self = {
+            assert!(ID >= 1 && ID <= 8, "Motor ID must be 1-8");
+            Self
+        };
+
+        /// 取出 ID 数值。
+        #[must_use]
+        pub const fn get(&self) -> u8 {
+            ID
+        }
+
+        /// 所属控制帧 ID（**C620/C610 电流协议族**：主组 `0x200` / 副组 `0x1FF`）。
+        ///
+        /// GM6020 的电压协议族用的是**另一张表**（`0x1FF` / `0x2FF`），见
+        /// [`crate::gm6020::command_frame_id`]。两者不可混用。
+        #[must_use]
+        pub const fn command_frame_id(&self) -> u32 {
+            if ID <= 4 {
+                crate::frame::COMMAND_FRAME_ID_PRIMARY
+            } else {
+                crate::frame::COMMAND_FRAME_ID_SECONDARY
+            }
+        }
+
+        /// 组内槽位（0..4），即控制帧里第几个 i16。
+        #[must_use]
+        pub const fn slot(&self) -> usize {
+            if ID <= 4 { (ID - 1) as usize } else { (ID - 5) as usize }
+        }
+
+        /// 反馈帧 ID（`0x200 + ID`）。
+        #[must_use]
+        pub const fn feedback_frame_id(&self) -> u32 {
+            crate::frame::FEEDBACK_FRAME_ID_BASE + ID as u32
+        }
+    }
+
+    /// 运行期已知 ID 的校验（用于读配置 / 解析命令行）。
+    ///
+    /// # Errors
+    ///
+    /// ID 不在 `1..=8` 时返回原值，便于报错信息带上用户输入。
+    pub const fn validate_motor_id(id: u8) -> Result<u8, u8> {
+        if id >= 1 && id <= 8 { Ok(id) } else { Err(id) }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn primary_and_secondary_groups_split_correctly() {
+            assert_eq!(MotorId::<1>::NEW.command_frame_id(), 0x200);
+            assert_eq!(MotorId::<4>::NEW.command_frame_id(), 0x200);
+            assert_eq!(MotorId::<5>::NEW.command_frame_id(), 0x1FF);
+            assert_eq!(MotorId::<8>::NEW.command_frame_id(), 0x1FF);
+
+            assert_eq!(MotorId::<1>::NEW.slot(), 0);
+            assert_eq!(MotorId::<4>::NEW.slot(), 3);
+            assert_eq!(MotorId::<5>::NEW.slot(), 0);
+            assert_eq!(MotorId::<8>::NEW.slot(), 3);
+        }
+
+        #[test]
+        fn feedback_ids_follow_0x200_plus_id() {
+            assert_eq!(MotorId::<1>::NEW.feedback_frame_id(), 0x201);
+            assert_eq!(MotorId::<8>::NEW.feedback_frame_id(), 0x208);
+            assert_eq!(MotorId::<3>::NEW.get(), 3);
+        }
+
+        #[test]
+        fn runtime_validation_returns_the_offending_id() {
+            assert_eq!(validate_motor_id(1), Ok(1));
+            assert_eq!(validate_motor_id(8), Ok(8));
+            assert_eq!(validate_motor_id(0), Err(0));
+            assert_eq!(validate_motor_id(9), Err(9));
+        }
+    }
+}
+
+pub mod turn {
+    //! 多圈累计：单圈编码器原始角 → 连续圈数。
+
+    /// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。
+    pub const ENCODER_TICKS: u16 = 8192;
+
+    /// 半圈阈值：帧间角变化超过它视为发生回绕。
+    const HALF_TURN_TICKS: i32 = 4096;
+
+    /// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。
+    ///
+    /// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约
+    /// 9300 rpm ≈ 0.155 圈/ms，余量充足。
+    #[derive(Debug, Clone, Default)]
+    pub struct TurnTracker {
+        revolutions: i32,
+        prev_raw: Option<u16>,
+        last_angle_rev: f32,
+    }
+
+    impl TurnTracker {
+        /// 创建跟踪器。
+        #[must_use]
+        pub const fn new() -> Self {
+            Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 }
+        }
+
+        /// 送入新的原始角，返回连续累计角（圈）。
+        pub fn update(&mut self, raw: u16) -> f32 {
+            if let Some(prev) = self.prev_raw {
+                let delta = i32::from(raw) - i32::from(prev);
+                if delta > HALF_TURN_TICKS {
+                    // 正向跨越 8191 → 0。
+                    self.revolutions -= 1;
+                } else if delta < -HALF_TURN_TICKS {
+                    // 反向跨越 0 → 8191。
+                    self.revolutions += 1;
+                }
+            }
+            self.prev_raw = Some(raw);
+            self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0;
+            self.last_angle_rev
+        }
+
+        /// 最近一次 [`TurnTracker::update`] 的返回值。
+        #[must_use]
+        pub const fn last_angle_rev(&self) -> f32 {
+            self.last_angle_rev
+        }
+
+        /// 清零回到初始状态。
+        pub fn reset(&mut self) {
+            *self = Self::new();
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn first_reading_is_raw_fraction() {
+            let mut t = TurnTracker::new();
+            assert!((t.update(4096) - 0.5).abs() < 1e-4);
+        }
+
+        #[test]
+        fn forward_wrap_counts_up() {
+            let mut t = TurnTracker::new();
+            t.update(8000);
+            let after = t.update(100); // 8000 → 8191 → 0 → 100
+            assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}");
+        }
+
+        #[test]
+        fn reverse_wrap_counts_down() {
+            let mut t = TurnTracker::new();
+            t.update(100);
+            let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向）
+            assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}");
+        }
+
+        #[test]
+        fn small_deltas_do_not_wrap() {
+            let mut t = TurnTracker::new();
+            t.update(0);
+            assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4);
+            assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4);
+        }
+
+        #[test]
+        fn multi_turn_forward_then_reverse_returns() {
+            let mut t = TurnTracker::new();
+            // 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。
+            for _ in 0..3 {
+                t.update(0);
+                t.update(4000);
+                t.update(8000);
+                t.update(100);
+            }
+            let fwd = t.last_angle_rev();
+            let expected_fwd = 3.0 + 100.0 / 8192.0;
+            assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}");
+
+            // 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。
+            for _ in 0..3 {
+                t.update(8000);
+                t.update(4000);
+                t.update(0);
+            }
+            let back = t.last_angle_rev();
+            assert!(back.abs() < 1e-4, "should return to 0, got {back}");
+        }
+
+        #[test]
+        fn jitter_at_boundary_stays_put() {
+            // 0 附近抖动不产生虚假圈数。
+            let mut t = TurnTracker::new();
+            t.update(8191);
+            assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次
+            assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4);
+            assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回
+            assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4);
+        }
+
+        #[test]
+        fn reset_forgets_history() {
+            let mut t = TurnTracker::new();
+            t.update(8000);
+            t.update(100);
+            t.reset();
+            assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits());
+            assert!((t.update(4096) - 0.5).abs() < 1e-4);
+        }
+    }
+}
+
+pub use common::{
+    DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id,
+    decode_feedback, encode_current_frame, encode_feedback_frame, feedback_frame_id,
+};
+pub use frame::*;
+pub use gm6020::{Gm6020Command, Gm6020Feedback};
+pub use id::*;
+pub use m2006::C610;
+pub use m3508::C620;
+pub use turn::*;