feat: 初始化

crates/can-link/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "can-link"
+description = "CAN 传输抽象：帧与链路通道（无协议知识）"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+
+[dependencies]
+tokio = { workspace = true }
+
+[lints]
+workspace = true

crates/can-link/src/lib.rs

@@ -0,0 +1,75 @@
+//! CAN 传输抽象：帧与链路通道。
+//!
+//! 本 crate **不含任何协议知识**（无 DJI ID 常量）：只定义经典 CAN 数据帧
+//! 与一对 tokio 通道构成的链路端点。协议族（`dji-esc` / 未来 `dji-gm6020`）
+//! 与总线后端（SocketCAN 桥、仿真器）共享此类型，互不依赖。
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+use tokio::sync::mpsc;
+
+/// 经典 CAN 数据帧（数据长度固定 8 字节）。
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
+pub struct CanFrame {
+    /// 11 位标准 ID。
+    pub id: u32,
+    /// 数据域。
+    pub data: [u8; 8],
+}
+
+impl CanFrame {
+    /// 构造一帧。
+    #[must_use]
+    pub const fn new(id: u32, data: [u8; 8]) -> Self {
+        Self { id, data }
+    }
+}
+
+/// CAN 链路端点。
+///
+/// - `outgoing_tx`：本端发往总线的帧；
+/// - `incoming_rx`：本端从总线收到的帧。
+///
+/// 一条链路由两个端点构成（见 [`CanLink::pair`]）。持有本端、把对端交给
+/// 对侧（SocketCAN 桥、仿真器）即可通信。
+#[derive(Debug)]
+pub struct CanLink {
+    /// 发往总线。
+    pub outgoing_tx: mpsc::Sender<CanFrame>,
+    /// 来自总线。
+    pub incoming_rx: mpsc::Receiver<CanFrame>,
+}
+
+impl CanLink {
+    /// 由已有通道组装端点。
+    #[must_use]
+    pub const fn new(
+        outgoing_tx: mpsc::Sender<CanFrame>,
+        incoming_rx: mpsc::Receiver<CanFrame>,
+    ) -> Self {
+        Self { outgoing_tx, incoming_rx }
+    }
+
+    /// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。
+    ///
+    /// `bound` 为每方向缓冲帧数（建议 ≥ 64，1kHz 收发下留足余量）。
+    #[must_use]
+    pub fn pair(bound: usize) -> (Self, Self) {
+        let (tx_a_to_b, rx_at_b) = mpsc::channel(bound);
+        let (tx_b_to_a, rx_at_a) = mpsc::channel(bound);
+        (Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[tokio::test]
+    async fn pair_loops_both_directions() {
+        let (mut a, mut b) = CanLink::pair(8);
+        a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
+        b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
+        assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200);
+        assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201);
+    }
+}

crates/chassisd/Cargo.toml

@@ -0,0 +1,28 @@
+[package]
+name = "chassisd"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety supervision, and mock mode"
+
+[lints]
+workspace = true
+
+[dependencies]
+motor-core = { workspace = true }
+dji-esc = { workspace = true }
+dji-esc-socketcan = { workspace = true }
+mecanum = { workspace = true }
+tokio = { workspace = true }
+thiserror = { workspace = true }
+tracing = { workspace = true }
+tracing-subscriber = { workspace = true }
+serde = { workspace = true }
+toml = { workspace = true }
+clap = { workspace = true }
+
+[dev-dependencies]
+proptest = { workspace = true }
+approx = { workspace = true }
+tokio = { workspace = true, features = ["test-util"] }

crates/chassisd/src/config.rs

@@ -0,0 +1,504 @@
+//! `chassis.toml` 配置模型与加载。
+
+use std::time::Duration;
+
+use dji_esc::{BusOptions, EscConfig, EscModel};
+use mecanum::Mecanum;
+use serde::Deserialize;
+use thiserror::Error;
+
+/// 配置错误。
+#[derive(Debug, Clone, PartialEq, Eq, Error)]
+pub enum ConfigError {
+    /// TOML 语法 / 结构错误。
+    #[error("failed to parse config: {0}")]
+    Parse(String),
+    /// 语义校验失败。
+    #[error("invalid config: {0}")]
+    Invalid(String),
+}
+
+/// 顶层配置。
+#[derive(Debug, Clone, PartialEq, Deserialize)]
+#[serde(deny_unknown_fields)]
+pub struct Config {
+    /// CAN 配置。
+    pub can: CanConfig,
+    /// 底盘控制参数。
+    #[serde(default)]
+    pub chassis: ChassisConfig,
+    /// 麦轮几何。
+    pub mecanum: MecanumConfig,
+    /// 四电机配置，顺序固定：前左、前右、后左、后右。
+    #[serde(rename = "motor")]
+    pub motors: Vec<MotorConfig>,
+}
+
+/// CAN 接口配置（双 CAN 总线 seam）。
+///
+/// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。
+#[derive(Debug, Clone, PartialEq, Eq, Deserialize)]
+#[serde(deny_unknown_fields)]
+pub struct CanConfig {
+    /// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。
+    pub interface: String,
+    /// 功能总线（云台 / 拨弹）：与底盘物理隔离。
+    ///
+    /// 预留 seam；填写后由上层功能模块（gimbal / feeder）使用。
+    /// 典型：`can1`，承载 GM6020 云台 + M2006/C610 拨弹。
+    #[serde(default)]
+    pub function: Option<FunctionBusConfig>,
+}
+
+/// 功能总线配置（双 CAN 之二）。
+#[derive(Debug, Clone, PartialEq, Eq, Deserialize)]
+#[serde(deny_unknown_fields)]
+pub struct FunctionBusConfig {
+    /// SocketCAN 接口名（如 `can1`）。
+    pub interface: String,
+}
+
+/// 协议族——同一总线上**不得混用**。
+///
+/// C620/C610（电流）与 GM6020（电压）的控制/反馈帧 ID 空间重叠：
+/// `0x1FF` 既可作 C620 5–8 电流也可作 GM6020 1–4 电压；`0x205` 同理。
+/// 双 CAN 物理隔离是首选消解手段；单总线时必须保证同族。
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
+pub enum ProtocolFamily {
+    /// C620/C610 电流协议（驱动轮、拨弹轮）。
+    Esc,
+    /// GM6020 电压协议（云台）。
+    Gm6020,
+}
+
+impl ProtocolFamily {
+    /// 电调型号所属协议族。
+    #[must_use]
+    pub const fn of_esc_model(model: EscModelSerde) -> Self {
+        match model {
+            EscModelSerde::C620 | EscModelSerde::C610 => Self::Esc,
+        }
+    }
+}
+
+/// 底盘控制参数（全部可省略，采用默认值）。
+#[derive(Debug, Clone, PartialEq, Deserialize)]
+#[serde(deny_unknown_fields, default)]
+pub struct ChassisConfig {
+    /// 控制循环周期（毫秒）。
+    pub control_period_ms: u64,
+    /// 上层指令超时（毫秒）：超时后目标速度归零。
+    pub command_timeout_ms: u64,
+    /// 电机反馈离线阈值（毫秒）。
+    pub feedback_timeout_ms: u64,
+    /// 最大前进 / 横移速度（m/s）。
+    pub max_vx: f32,
+    /// 最大横移速度（m/s）。
+    pub max_vy: f32,
+    /// 最大角速度（rad/s）。
+    pub max_omega: f32,
+}
+
+impl Default for ChassisConfig {
+    fn default() -> Self {
+        Self {
+            control_period_ms: 1,
+            command_timeout_ms: 200,
+            feedback_timeout_ms: 100,
+            max_vx: 3.0,
+            max_vy: 3.0,
+            max_omega: 6.0,
+        }
+    }
+}
+
+/// 麦轮几何（米）。
+#[derive(Debug, Clone, Copy, PartialEq, Deserialize)]
+#[serde(deny_unknown_fields)]
+pub struct MecanumConfig {
+    /// 前后轮轴线到中心的纵向半距。
+    pub half_length: f32,
+    /// 左右轮到中心的横向半距。
+    pub half_width: f32,
+    /// 车轮半径。
+    pub wheel_radius: f32,
+}
+
+/// 单电机配置。
+#[derive(Debug, Clone, PartialEq, Deserialize)]
+#[serde(deny_unknown_fields)]
+pub struct MotorConfig {
+    /// 便于日志辨认的名字（`front_left` 等）。
+    pub name: String,
+    /// 电调 ID（1–8）。
+    pub esc_id: u8,
+    /// 电调型号（`C620` / `C610`）。
+    #[serde(default = "default_model")]
+    pub model: EscModelSerde,
+    /// 安装方向：`1` 或 `-1`。
+    #[serde(default = "default_direction")]
+    pub direction: f32,
+    /// 减速比（GM3508 ≈ 19.203）。
+    pub gear_ratio: f32,
+    /// 电流限幅（raw）。
+    #[serde(default = "default_current_limit")]
+    pub current_limit: i16,
+    /// 速度环 kp。
+    #[serde(default)]
+    pub kp: f32,
+    /// 速度环 ki。
+    #[serde(default)]
+    pub ki: f32,
+    /// 速度环 kd。
+    #[serde(default)]
+    pub kd: f32,
+    /// 速度前馈（raw/rpm）。
+    #[serde(default)]
+    pub feedforward: f32,
+}
+
+fn default_model() -> EscModelSerde {
+    EscModelSerde::C620
+}
+
+fn default_direction() -> f32 {
+    1.0
+}
+
+fn default_current_limit() -> i16 {
+    12_000
+}
+
+/// 配置文件中的电调型号。
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize)]
+#[serde(deny_unknown_fields, rename_all = "UPPERCASE")]
+pub enum EscModelSerde {
+    /// C620（GM3508）。
+    C620,
+    /// C610（M2006）。
+    C610,
+}
+
+impl From<EscModelSerde> for EscModel {
+    fn from(value: EscModelSerde) -> Self {
+        match value {
+            EscModelSerde::C620 => Self::C620,
+            EscModelSerde::C610 => Self::C610,
+        }
+    }
+}
+
+impl Config {
+    /// 从 TOML 文本解析并校验。
+    ///
+    /// # Errors
+    ///
+    /// 解析失败或语义校验失败（电机数量、几何、方向等）。
+    pub fn from_toml(text: &str) -> Result<Self, ConfigError> {
+        let config: Self = toml::from_str(text).map_err(|e| ConfigError::Parse(e.to_string()))?;
+        config.validate()?;
+        Ok(config)
+    }
+
+    /// 从文件读取并解析。
+    ///
+    /// # Errors
+    ///
+    /// 读取失败、解析失败或校验失败。
+    pub fn load(path: &std::path::Path) -> Result<Self, ConfigError> {
+        let text = std::fs::read_to_string(path)
+            .map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))?;
+        Self::from_toml(&text)
+    }
+
+    /// 语义校验。
+    ///
+    /// # Errors
+    ///
+    /// 任一约束不满足时返回 [`ConfigError::Invalid`]。
+    pub fn validate(&self) -> Result<(), ConfigError> {
+        if self.motors.len() != 4 {
+            return Err(ConfigError::Invalid(format!(
+                "mecanum chassis needs exactly 4 motors (FL, FR, RL, RR), got {}",
+                self.motors.len()
+            )));
+        }
+        let m = &self.mecanum;
+        if !(m.half_length > 0.0 && m.half_width > 0.0 && m.wheel_radius > 0.0) {
+            return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}")));
+        }
+        let c = &self.chassis;
+        if c.control_period_ms == 0 || c.command_timeout_ms == 0 || c.feedback_timeout_ms == 0 {
+            return Err(ConfigError::Invalid(
+                "control/command/feedback timeouts must be > 0 ms".into(),
+            ));
+        }
+        if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) {
+            return Err(ConfigError::Invalid("speed limits must be > 0".into()));
+        }
+        for motor in &self.motors {
+            let gear_ratio_valid = motor.gear_ratio > 0.0;
+            if !gear_ratio_valid {
+                return Err(ConfigError::Invalid(format!(
+                    "motor {}: gear_ratio must be > 0",
+                    motor.name
+                )));
+            }
+        }
+        // 每总线单协议族：底盘四轮必须同属 ESC 族（C620/C610）。
+        let chassis_families: std::collections::HashSet<ProtocolFamily> =
+            self.motors.iter().map(|m| ProtocolFamily::of_esc_model(m.model)).collect();
+        if chassis_families.len() > 1 {
+            return Err(ConfigError::Invalid(
+                "chassis bus mixes protocol families; keep one family per CAN bus".into(),
+            ));
+        }
+        // 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。
+        if let Some(function) = &self.can.function {
+            if function.interface == self.can.interface {
+                return Err(ConfigError::Invalid(format!(
+                    "function bus interface must differ from chassis interface (both '{}')",
+                    self.can.interface
+                )));
+            }
+        }
+        // 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。
+        Ok(())
+    }
+
+    /// `--mock` 演示 / 测试默认配置（与 `config/chassis.toml.example` 同构，
+    /// PID 与仿真器增益匹配）。
+    #[must_use]
+    pub fn mock_default() -> Self {
+        let motor = |name: &str, esc_id: u8, direction: f32| MotorConfig {
+            name: name.into(),
+            esc_id,
+            model: EscModelSerde::C620,
+            direction,
+            gear_ratio: 3591.0 / 187.0,
+            current_limit: 12_000,
+            kp: 8.0,
+            ki: 2.0,
+            kd: 0.0,
+            feedforward: 16384.0 / 9254.0,
+        };
+        Self {
+            can: CanConfig { interface: "vcan0".into(), function: None },
+            chassis: ChassisConfig::default(),
+            mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 },
+            motors: vec![
+                motor("front_left", 1, 1.0),
+                motor("front_right", 2, -1.0),
+                motor("rear_left", 3, 1.0),
+                motor("rear_right", 4, -1.0),
+            ],
+        }
+    }
+
+    /// 转换为 `EscBus` 配置（顺序 FL、FR、RL、RR）。
+    #[must_use]
+    pub fn to_esc_configs(&self) -> Vec<EscConfig> {
+        self.motors
+            .iter()
+            .map(|m| EscConfig {
+                name: m.name.clone(),
+                esc_id: m.esc_id,
+                model: m.model.into(),
+                gear_ratio: m.gear_ratio,
+                direction: m.direction,
+                current_limit: m.current_limit,
+                kp: m.kp,
+                ki: m.ki,
+                kd: m.kd,
+                feedforward: m.feedforward,
+            })
+            .collect()
+    }
+
+    /// 转换为 `EscBus` 运行参数。
+    #[must_use]
+    pub fn bus_options(&self) -> BusOptions {
+        BusOptions {
+            period: Duration::from_millis(self.chassis.control_period_ms),
+            feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms),
+        }
+    }
+
+    /// 麦轮运动学模型。
+    #[must_use]
+    pub fn kinematics(&self) -> Mecanum {
+        Mecanum::new(self.mecanum.half_length, self.mecanum.half_width, self.mecanum.wheel_radius)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    const EXAMPLE: &str = r#"
+[can]
+interface = "can0"
+
+[chassis]
+control_period_ms = 1
+command_timeout_ms = 200
+feedback_timeout_ms = 100
+
+[mecanum]
+half_length = 0.18
+half_width = 0.20
+wheel_radius = 0.0762
+
+[[motor]]
+name = "front_left"
+esc_id = 1
+direction = 1
+gear_ratio = 19.203
+current_limit = 12000
+kp = 18.0
+ki = 0.6
+kd = 0.0
+feedforward = 9.0
+
+[[motor]]
+name = "front_right"
+esc_id = 2
+direction = -1
+gear_ratio = 19.203
+current_limit = 12000
+kp = 18.0
+ki = 0.6
+feedforward = 9.0
+
+[[motor]]
+name = "rear_left"
+esc_id = 3
+direction = 1
+gear_ratio = 19.203
+current_limit = 12000
+kp = 18.0
+ki = 0.6
+feedforward = 9.0
+
+[[motor]]
+name = "rear_right"
+esc_id = 4
+direction = -1
+gear_ratio = 19.203
+current_limit = 12000
+kp = 18.0
+ki = 0.6
+feedforward = 9.0
+"#;
+
+    #[test]
+    fn parses_example() {
+        let cfg = Config::from_toml(EXAMPLE).unwrap();
+        assert_eq!(cfg.can.interface, "can0");
+        assert_eq!(cfg.motors.len(), 4);
+        assert_eq!(cfg.motors[0].name, "front_left");
+        assert_eq!(cfg.motors[3].esc_id, 4);
+        assert_eq!(cfg.motors[1].direction.to_bits(), (-1.0f32).to_bits());
+        assert!(cfg.validate().is_ok());
+        assert_eq!(cfg.bus_options().period, Duration::from_millis(1));
+        assert_eq!(cfg.to_esc_configs().len(), 4);
+    }
+
+    #[test]
+    fn minimal_config_uses_defaults() {
+        let cfg = Config::from_toml(
+            r#"
+[can]
+interface = "can1"
+[mecanum]
+half_length = 0.2
+half_width = 0.25
+wheel_radius = 0.0762
+[[motor]]
+name = "fl"
+esc_id = 1
+gear_ratio = 19.2
+[[motor]]
+name = "fr"
+esc_id = 2
+gear_ratio = 19.2
+[[motor]]
+name = "rl"
+esc_id = 3
+gear_ratio = 19.2
+[[motor]]
+name = "rr"
+esc_id = 4
+gear_ratio = 19.2
+"#,
+        )
+        .unwrap();
+        assert_eq!(cfg.chassis, ChassisConfig::default());
+        assert_eq!(cfg.motors[0].direction.to_bits(), 1.0f32.to_bits());
+        assert_eq!(cfg.motors[0].current_limit, 12_000);
+        assert_eq!(cfg.motors[0].model, EscModelSerde::C620);
+    }
+
+    #[test]
+    fn rejects_wrong_motor_count() {
+        let mut cfg = Config::from_toml(EXAMPLE).unwrap();
+        cfg.motors.pop();
+        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
+    }
+
+    #[test]
+    fn rejects_bad_geometry() {
+        let mut cfg = Config::from_toml(EXAMPLE).unwrap();
+        cfg.mecanum.wheel_radius = 0.0;
+        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
+    }
+
+    #[test]
+    fn rejects_unknown_fields() {
+        let text = EXAMPLE.replace("[can]", "[can]\nstray = 1");
+        assert!(matches!(Config::from_toml(&text), Err(ConfigError::Parse(_))));
+    }
+
+    #[test]
+    fn mock_default_is_valid_and_wired_like_example() {
+        let cfg = Config::mock_default();
+        assert!(cfg.validate().is_ok());
+        assert_eq!(cfg.motors.len(), 4);
+        let esc: Vec<u8> = cfg.to_esc_configs().iter().map(|m| m.esc_id).collect();
+        assert_eq!(esc, [1, 2, 3, 4]);
+    }
+
+    #[test]
+    fn function_bus_must_differ_from_chassis_bus() {
+        let mut cfg = Config::mock_default();
+        cfg.can.function = Some(FunctionBusConfig { interface: "vcan0".into() });
+        assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
+
+        cfg.can.function = Some(FunctionBusConfig { interface: "vcan1".into() });
+        assert!(cfg.validate().is_ok());
+    }
+
+    #[test]
+    fn rejects_mixed_protocol_families_on_chassis_bus() {
+        let mut cfg = Config::mock_default();
+        // 当前 ESC 族包含 C620 与 C610；若引入 GM6020 到底盘总线必须拒绝。
+        // 这里验证家族集合逻辑：全部 C620/C610 仍为单族。
+        cfg.motors[0].model = EscModelSerde::C610;
+        assert!(cfg.validate().is_ok());
+
+        // 人为构造跨族：通过 of_esc_model 映射覆盖后仍应单族（C610→Esc）。
+        assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C620), ProtocolFamily::Esc);
+        assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C610), ProtocolFamily::Esc);
+    }
+
+    #[test]
+    fn parses_function_bus_seam() {
+        let text = EXAMPLE.replace(
+            "[can]\ninterface = \"can0\"",
+            "[can]\ninterface = \"can0\"\n\n[can.function]\ninterface = \"can1\"",
+        );
+        let cfg = Config::from_toml(&text).unwrap();
+        let function = cfg.can.function.as_ref().unwrap();
+        assert_eq!(function.interface, "can1");
+    }
+}

crates/chassisd/src/controller.rs

@@ -0,0 +1,203 @@
+//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。
+
+use std::time::Duration;
+
+use mecanum::{Mecanum, Twist};
+use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive};
+use tokio::sync::watch;
+use tokio::task::JoinHandle;
+use tokio::time::{Instant, MissedTickBehavior, interval};
+use tracing::{info, warn};
+
+/// 底盘控制器。
+///
+/// 职责（1kHz 节拍）：
+/// 1. 读取最新 twist 指令，**指令超时则归零**；
+/// 2. 速度限幅（保持方向按比例收缩）；
+/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用；
+/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。
+///
+/// 电机反馈离线安全在驱动层完成（强制 0 电流）。
+pub struct ChassisController<W> {
+    kinematics: Mecanum,
+    /// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。
+    wheels: [W; 4],
+    /// (max_vx, max_vy, max_omega)。
+    limits: (f32, f32, f32),
+    command_timeout: Duration,
+}
+
+impl<W> ChassisController<W>
+where
+    W: VelocityDrive + MotorFeedback + Disable + 'static,
+{
+    /// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。
+    #[must_use]
+    pub fn new(
+        kinematics: Mecanum,
+        wheels: [W; 4],
+        limits: (f32, f32, f32),
+        command_timeout: Duration,
+    ) -> Self {
+        Self { kinematics, wheels, limits, command_timeout }
+    }
+
+    /// 启动控制循环任务。
+    pub fn spawn(self, commands: watch::Receiver<Twist>, period: Duration) -> JoinHandle<()> {
+        tokio::spawn(self.run(commands, period))
+    }
+
+    async fn run(self, mut commands: watch::Receiver<Twist>, period: Duration) {
+        let mut ticker = interval(period);
+        ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+        let mut latest = Twist::ZERO;
+        let mut last_command_at: Option<Instant> = None;
+        let mut ticks: u64 = 0;
+
+        loop {
+            ticker.tick().await;
+            let now = Instant::now();
+
+            if commands.has_changed().unwrap_or(false) {
+                latest = *commands.borrow_and_update();
+                last_command_at = Some(now);
+            }
+            let timed_out = match last_command_at {
+                Some(t) => now.duration_since(t) > self.command_timeout,
+                // 尚未收到任何指令：保持静止。
+                None => true,
+            };
+            let twist = if timed_out {
+                Twist::ZERO
+            } else {
+                latest.clamped(self.limits.0, self.limits.1, self.limits.2)
+            };
+
+            let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback());
+            if all_seen_feedback {
+                let wheel_rpms = self.kinematics.wheel_rpms(&twist);
+                for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) {
+                    motor.set_velocity(wheel_rpm);
+                }
+            } else {
+                for motor in &self.wheels {
+                    motor.disable();
+                }
+                if ticks.is_multiple_of(1000) {
+                    warn!("waiting for first feedback from all motors");
+                }
+            }
+
+            ticks += 1;
+            if ticks.is_multiple_of(1000) {
+                self.log_status();
+            }
+        }
+    }
+
+    fn log_status(&self) {
+        let (rotor_rpm, effort_raw): (Vec<f32>, Vec<i32>) = self
+            .wheels
+            .iter()
+            .map(|m| {
+                let st = m.state();
+                (st.rotor_speed_rpm, i32::from(st.effort_raw))
+            })
+            .unzip();
+        info!(?rotor_rpm, ?effort_raw, "chassis status");
+    }
+}
+
+/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。
+#[must_use]
+pub fn wrap_wheels<T: Clone, const N: usize>(
+    motors: [T; N],
+    gear_ratios: [f32; N],
+) -> [OutputShaft<T>; N] {
+    let mut out = Vec::with_capacity(N);
+    let mut gears = gear_ratios.into_iter();
+    for m in motors {
+        let g = gears.next().unwrap_or(1.0);
+        out.push(OutputShaft::new(m, g));
+    }
+    out.try_into().unwrap_or_else(|_| unreachable!())
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use motor_core::{Command, MockMotor, MotorState};
+
+    const GEAR: f32 = 19.203;
+
+    fn mock_wheel(id: u8) -> MockMotor {
+        let m = MockMotor::new(id);
+        m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
+        m
+    }
+
+    /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。
+    #[tokio::test(start_paused = true)]
+    async fn controller_drives_wheels_through_capability_traits() {
+        let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
+        let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)];
+        let ctrl = ChassisController::new(
+            kinematics,
+            wrap_wheels(mocks.clone(), [GEAR; 4]),
+            (3.0, 3.0, 6.0),
+            Duration::from_millis(200),
+        );
+        let (tx, rx) = watch::channel(Twist::ZERO);
+        let handle = ctrl.spawn(rx, Duration::from_millis(1));
+
+        let twist = Twist { vx: 1.0, ..Twist::ZERO };
+        tx.send_replace(twist);
+        tokio::time::sleep(Duration::from_millis(5)).await;
+
+        let expected = kinematics.wheel_rpms(&twist);
+        for (i, mock) in mocks.iter().enumerate() {
+            match mock.last_command() {
+                Command::Velocity(rotor_rpm) => {
+                    let want = expected[i] * GEAR;
+                    assert!(
+                        (rotor_rpm - want).abs() < 1e-2,
+                        "wheel {i}: rotor_rpm {rotor_rpm} != {want}"
+                    );
+                }
+                ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
+            }
+        }
+
+        handle.abort();
+    }
+
+    /// 反馈未到齐时必须全部停用，不得下发速度。
+    #[tokio::test(start_paused = true)]
+    async fn disables_wheels_until_all_feedback_seen() {
+        let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
+        let ready = mock_wheel(1);
+        let missing = MockMotor::new(2); // 无反馈
+        let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
+        let _ = mocks_ready;
+        let wheels = wrap_wheels(
+            [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)],
+            [GEAR; 4],
+        );
+        let ctrl = ChassisController::new(
+            kinematics,
+            wheels,
+            (3.0, 3.0, 6.0),
+            Duration::from_millis(200),
+        );
+        let (tx, rx) = watch::channel(Twist::ZERO);
+        let handle = ctrl.spawn(rx, Duration::from_millis(1));
+
+        tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+        tokio::time::sleep(Duration::from_millis(5)).await;
+
+        assert_eq!(ready.last_command(), Command::Disabled);
+        assert_eq!(missing.last_command(), Command::Disabled);
+
+        handle.abort();
+    }
+}

crates/chassisd/src/lib.rs

@@ -0,0 +1,15 @@
+//! `chassisd` 库部分：配置模型、底盘控制循环与指令源抽象。
+//!
+//! 二进制入口见 `src/main.rs`。
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+pub mod config;
+pub mod controller;
+pub mod source;
+
+pub use config::{
+    CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig,
+    ProtocolFamily,
+};
+pub use controller::{ChassisController, wrap_wheels};
+pub use source::{ChannelSource, TwistSource, ZeroSource, pump_source};

crates/chassisd/src/main.rs

@@ -0,0 +1,161 @@
+//! `chassisd`：麦轮底盘守护进程（4 × GM3508 / C620，SocketCAN）。
+
+use std::path::PathBuf;
+use std::process::ExitCode;
+use std::time::Duration;
+
+use clap::Parser;
+use mecanum::Twist;
+use tokio::sync::watch;
+use tracing::{error, info};
+use tracing_subscriber::EnvFilter;
+
+use chassisd::config::Config;
+use chassisd::controller::{ChassisController, wrap_wheels};
+use motor_core::Disable;
+
+/// 命令行参数。
+#[derive(Debug, Parser)]
+#[command(name = "chassisd", version, about = "麦轮底盘守护进程（4×GM3508/C620）")]
+struct Args {
+    /// 配置文件路径（实车模式必填；`--mock` 下可省略，使用内置演示配置）。
+    #[arg(short, long)]
+    config: Option<PathBuf>,
+
+    /// 覆盖配置文件中的 CAN 接口名。
+    #[arg(short, long)]
+    interface: Option<String>,
+
+    /// 无硬件仿真模式：进程内电机模型，全链路可离线运行。
+    #[arg(long)]
+    mock: bool,
+}
+
+fn main() -> ExitCode {
+    let filter =
+        EnvFilter::try_from_default_env().unwrap_or_else(|_| EnvFilter::new("info,chassisd=debug"));
+    tracing_subscriber::fmt().with_env_filter(filter).init();
+
+    let args = Args::parse();
+
+    let config = match load_config(&args) {
+        Ok(config) => config,
+        Err(e) => {
+            error!("{e}");
+            return ExitCode::FAILURE;
+        }
+    };
+    let interface = args.interface.clone().unwrap_or_else(|| config.can.interface.clone());
+
+    let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() {
+        Ok(runtime) => runtime,
+        Err(e) => {
+            error!("failed to start tokio runtime: {e}");
+            return ExitCode::FAILURE;
+        }
+    };
+    match runtime.block_on(run(config, &interface, args.mock)) {
+        Ok(()) => ExitCode::SUCCESS,
+        Err(e) => {
+            error!("{e}");
+            ExitCode::FAILURE
+        }
+    }
+}
+
+fn load_config(args: &Args) -> Result<Config, String> {
+    match (&args.config, args.mock) {
+        (Some(path), _) => Config::load(path).map_err(|e| e.to_string()),
+        (None, true) => {
+            info!("mock 模式未提供 --config，使用内置演示配置");
+            Ok(Config::mock_default())
+        }
+        (None, false) => {
+            Err("实车模式需要 --config 指向配置文件（样例见 config/chassis.toml.example）".into())
+        }
+    }
+}
+
+async fn run(
+    config: Config,
+    interface: &str,
+    mock: bool,
+) -> Result<(), Box<dyn std::error::Error>> {
+    let bus_options = config.bus_options();
+
+    // 1) CAN 链路：仿真或真实 SocketCAN。
+    let link = if mock {
+        let (bus_side, plant_side) = dji_esc::CanLink::pair(512);
+        let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
+        dji_esc::spawn_plant(plant_side, &esc_ids, bus_options.period);
+        info!(?esc_ids, "mock 模式：进程内电机仿真");
+        bus_side
+    } else {
+        open_real_link(interface)?
+    };
+
+    // 2) 电调总线。
+    let period = bus_options.period;
+    let bus = dji_esc::EscBus::spawn(link, config.to_esc_configs(), bus_options)?;
+    let names: Vec<&str> = bus.motors().iter().map(dji_esc::EscMotor::name).collect();
+    info!(
+        interface,
+        mock,
+        motors = ?names,
+        period_ms = period.as_millis(),
+        "chassisd 启动"
+    );
+
+    // 3) 控制循环（当前无指令源：watch 保持 ZERO，安全静止；
+    //    遥控 / 自主导航后续经 chassisd::source 接入）。
+    let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
+    let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega);
+    let motors: [dji_esc::EscMotor; 4] =
+        bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let gears: [f32; 4] = config
+        .motors
+        .iter()
+        .map(|m| m.gear_ratio)
+        .collect::<Vec<_>>()
+        .try_into()
+        .unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let controller = ChassisController::new(
+        config.kinematics(),
+        wrap_wheels(motors, gears),
+        limits,
+        Duration::from_millis(config.chassis.command_timeout_ms),
+    )
+    .spawn(twist_rx, period);
+    let _twist_tx = twist_tx; // 保留发送端供后续指令源使用
+
+    // 4) 优雅退出：ctrl-c 或控制任务意外退出。
+    tokio::select! {
+        _ = tokio::signal::ctrl_c() => info!("收到 ctrl-c，正在停车…"),
+        result = controller => match result {
+            Ok(()) => error!("控制任务意外退出"),
+            Err(e) => error!("控制任务 panic: {e}"),
+        },
+    }
+
+    // 5) 停车：显式停用电机并等若干节拍把 0 电流帧刷入总线。
+    for motor in bus.motors() {
+        motor.disable();
+    }
+    tokio::time::sleep(Duration::from_millis(20)).await;
+    bus.shutdown().await;
+    info!("chassisd 已退出");
+    Ok(())
+}
+
+/// 打开真实总线（仅 Linux；其他平台给出明确错误）。
+fn open_real_link(interface: &str) -> Result<dji_esc::CanLink, Box<dyn std::error::Error>> {
+    #[cfg(target_os = "linux")]
+    {
+        Ok(dji_esc_socketcan::open_link(interface)?)
+    }
+    #[cfg(not(target_os = "linux"))]
+    {
+        let _ = interface;
+        Err("实车模式仅支持 Linux（SocketCAN）；请使用 --mock 演示".into())
+    }
+}

crates/chassisd/src/source.rs

@@ -0,0 +1,81 @@
+//! 上层速度指令源（twist source）。
+//!
+//! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中
+//! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现
+//! [`TwistSource`] 即可。
+
+use std::future::Future;
+
+use mecanum::Twist;
+use tokio::sync::{mpsc, watch};
+
+/// 速度指令源。
+pub trait TwistSource: Send {
+    /// 取下一条指令；`None` 表示源关闭。
+    fn next_twist(&mut self) -> impl Future<Output = Option<Twist>> + Send;
+}
+
+/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。
+pub struct ZeroSource;
+
+impl TwistSource for ZeroSource {
+    async fn next_twist(&mut self) -> Option<Twist> {
+        std::future::pending().await
+    }
+}
+
+/// 通道指令源（测试 / 简单进程内注入用）。
+pub struct ChannelSource {
+    rx: mpsc::Receiver<Twist>,
+}
+
+impl ChannelSource {
+    /// 由 mpsc 接收端构造。
+    #[must_use]
+    pub const fn new(rx: mpsc::Receiver<Twist>) -> Self {
+        Self { rx }
+    }
+
+    /// 创建一对（发送端, 源）。
+    #[must_use]
+    pub fn pair(bound: usize) -> (mpsc::Sender<Twist>, Self) {
+        let (tx, rx) = mpsc::channel(bound);
+        (tx, Self { rx })
+    }
+}
+
+impl TwistSource for ChannelSource {
+    async fn next_twist(&mut self) -> Option<Twist> {
+        self.rx.recv().await
+    }
+}
+
+/// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。
+pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender<Twist>) {
+    while let Some(twist) = source.next_twist().await {
+        if tx.send(twist).is_err() {
+            break;
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[tokio::test(start_paused = true)]
+    async fn channel_source_pumps_into_watch() {
+        let (tx, source) = ChannelSource::pair(8);
+        let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO);
+        let pump = tokio::spawn(pump_source(source, watch_tx));
+
+        tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap();
+        tokio::time::sleep(std::time::Duration::from_millis(1)).await;
+        assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 });
+
+        // 源关闭 → 泵任务退出。
+        drop(tx);
+        pump.await.unwrap();
+        assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值
+    }
+}

crates/chassisd/tests/daemon.rs

@@ -0,0 +1,169 @@
+//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机
+//! → 控制循环 → 麦轮运动学），全平台可跑。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use chassisd::config::Config;
+use chassisd::controller::{ChassisController, wrap_wheels};
+use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant};
+use mecanum::Twist;
+use motor_core::MotorFeedback;
+use tokio::sync::watch;
+use tokio::time::sleep;
+
+/// 搭建与 `chassisd --mock` 完全同构的软件栈。
+///
+/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的
+/// 指令源），超时测试用配置原值。
+fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender<Twist>) {
+    let (bus_side, plant_side) = CanLink::pair(512);
+    let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
+    spawn_plant(plant_side, &esc_ids, config.bus_options().period);
+
+    let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap();
+
+    let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
+    let motors: [dji_esc::EscMotor; 4] =
+        bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let gears: [f32; 4] = config
+        .motors
+        .iter()
+        .map(|m| m.gear_ratio)
+        .collect::<Vec<_>>()
+        .try_into()
+        .unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let controller = ChassisController::new(
+        config.kinematics(),
+        wrap_wheels(motors, gears),
+        (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
+        Duration::from_millis(command_timeout_ms),
+    );
+    controller.spawn(twist_rx, config.bus_options().period);
+    (bus, twist_tx)
+}
+
+async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
+    let mut waited = 0u64;
+    while waited < total_ms {
+        if cond() {
+            return true;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+    cond()
+}
+
+fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] {
+    bus.motors()
+        .iter()
+        .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio)
+        .collect::<Vec<_>>()
+        .try_into()
+        .unwrap()
+}
+
+/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。
+const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762);
+
+#[tokio::test(start_paused = true)]
+async fn forward_command_drives_all_wheels_equally() {
+    let config = Config::mock_default();
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+    let target = RPM_PER_MPS;
+    let ok = wait_until(3000, || {
+        wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
+
+    // 停车：指令归零后各轮减速到停。
+    twist_tx.send_replace(Twist::ZERO);
+    let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
+    let rpm = wheel_rpms(&bus);
+    assert!(stopped, "未停车: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn rotation_command_respects_wheel_signs() {
+    let config = Config::mock_default();
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
+    // a·ω = (0.2 + 0.25) m/s 的轮线速度。
+    let target = 0.45 * RPM_PER_MPS;
+    let ok = wait_until(3000, || {
+        let rpm = wheel_rpms(&bus);
+        // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。
+        [
+            (rpm[0] + target).abs(),
+            (rpm[1] - target).abs(),
+            (rpm[2] + target).abs(),
+            (rpm[3] - target).abs(),
+        ]
+        .iter()
+        .all(|e| *e < 0.05 * target)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn command_timeout_stops_chassis() {
+    let config = Config::mock_default();
+    // 指令超时使用配置原值（200ms）。
+    let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
+
+    // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。
+    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+    let moving =
+        wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await;
+    assert!(moving, "指令后底盘应当运动");
+
+    sleep(Duration::from_millis(400)).await; // 超过 200ms 超时
+    let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
+    let rpm = wheel_rpms(&bus);
+    assert!(stopped, "指令超时后未停车: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn speed_limits_scale_motion() {
+    let mut config = Config::mock_default();
+    config.chassis.max_vx = 1.0; // 限速 1 m/s
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist {
+        vx: 3.0, // 超限指令
+        vy: 2.0,
+        omega: 0.0,
+    });
+    // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy：
+    // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。
+    let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
+    let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS;
+    let tol = 0.08 * diag_fast;
+    let ok = wait_until(3000, || {
+        let rpm = wheel_rpms(&bus);
+        [
+            (rpm[0] - diag_slow).abs(),
+            (rpm[1] - diag_fast).abs(),
+            (rpm[2] - diag_fast).abs(),
+            (rpm[3] - diag_slow).abs(),
+        ]
+        .iter()
+        .all(|e| *e < tol)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}");
+    bus.shutdown().await;
+}

crates/dji-esc-socketcan/Cargo.toml

@@ -0,0 +1,25 @@
+[package]
+name = "dji-esc-socketcan"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "SocketCAN backend bridging real Linux CAN interfaces to can-link channels"
+
+[lints]
+workspace = true
+
+[dependencies]
+can-link = { workspace = true }
+tokio = { workspace = true }
+tracing = { workspace = true }
+
+[target.'cfg(target_os = "linux")'.dependencies]
+socketcan = { workspace = true }
+
+[target.'cfg(target_os = "linux")'.dev-dependencies]
+can-link = { workspace = true }
+dji-esc = { workspace = true }
+motor-core = { workspace = true }
+socketcan = { workspace = true }
+tokio = { workspace = true }

crates/dji-esc-socketcan/examples/spin.rs

@@ -0,0 +1,110 @@
+//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
+//!
+//! ```sh
+//! spin [interface] [esc_id] [rotor_rpm] [duration_secs]
+//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
+//! cargo run --release -p dji-esc-socketcan --example spin -- can0 1 1000 10
+//! ```
+//!
+//! 关键点在 [`spin_one`]：它只依赖 `motor_core::Motor` 抽象，完全不知道
+//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
+//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（本 crate）在此汇合。
+//!
+//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+#[cfg(target_os = "linux")]
+use std::time::Duration;
+
+#[cfg(target_os = "linux")]
+use dji_esc::{BusOptions, EscBus, EscConfig};
+#[cfg(target_os = "linux")]
+use dji_esc_socketcan::open_link;
+#[cfg(target_os = "linux")]
+use motor_core::{Disable, EffortDrive, Motor, MotorFeedback, VelocityDrive};
+#[cfg(target_os = "linux")]
+use tokio::time::{MissedTickBehavior, interval};
+
+#[cfg(target_os = "linux")]
+#[tokio::main]
+async fn main() {
+    let mut args = std::env::args().skip(1);
+    let interface = args.next().unwrap_or_else(|| "can0".into());
+    let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
+    let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
+    let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
+
+    println!("=== 单电机抽象验证 ===");
+    println!(
+        "接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s"
+    );
+
+    let link = open_link(&interface).unwrap_or_else(|e| {
+        eprintln!(
+            "打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）"
+        );
+        std::process::exit(1);
+    });
+    let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
+    println!(
+        "电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}",
+        config.model, config.gear_ratio, config.current_limit
+    );
+
+    let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法");
+    let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
+
+    spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
+
+    bus.shutdown().await;
+    println!("已退出");
+}
+
+/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
+/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
+#[cfg(target_os = "linux")]
+async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
+    motor.set_velocity(target_rpm);
+    println!("已下发目标转速，开始观测（100ms 周期）……\n");
+
+    let mut ticker = interval(Duration::from_millis(100));
+    ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+    let deadline = tokio::time::Instant::now() + duration;
+
+    loop {
+        tokio::select! {
+            _ = tokio::signal::ctrl_c() => {
+                println!("\n收到 ctrl-c");
+                break;
+            }
+            _ = ticker.tick() => {}
+        }
+        if tokio::time::Instant::now() >= deadline {
+            break;
+        }
+        let st = motor.state();
+        if st.has_feedback() {
+            println!(
+                "转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
+                st.rotor_speed_rpm,
+                st.effort_raw,
+                st.temperature_c,
+                st.rotor_revolutions,
+                st.feedback_count
+            );
+        } else {
+            println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
+        }
+    }
+
+    motor.disable();
+    // 让 0 电流帧至少发出一拍再关总线。
+    tokio::time::sleep(Duration::from_millis(50)).await;
+    println!("已停机（0 电流）");
+}
+
+#[cfg(not(target_os = "linux"))]
+fn main() {
+    eprintln!("spin 仅支持 Linux（需要 SocketCAN）");
+}

crates/dji-esc-socketcan/src/bridge.rs

@@ -0,0 +1,96 @@
+//! 真实总线桥接实现（Linux）。
+
+use std::io;
+use std::sync::Arc;
+
+use can_link::{CanFrame as LinkFrame, CanLink};
+use socketcan::tokio::CanSocket;
+use socketcan::{
+    CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _,
+    SocketOptions as _,
+};
+use tokio::sync::mpsc;
+use tracing::{debug, error};
+
+/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。
+const CHANNEL_BOUND: usize = 512;
+
+/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。
+///
+/// 内部：
+/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208，
+///   非反馈帧再由协议层解码拒绝，双保险）；
+/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。
+///
+/// # Errors
+///
+/// 接口不存在 / 权限不足等 `io::Error`。
+pub fn open_link(interface: &str) -> io::Result<CanLink> {
+    let socket = CanSocket::open(interface)?;
+    socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?;
+    let socket = Arc::new(socket);
+
+    let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
+    let CanLink { outgoing_tx, incoming_rx } = bridge_side;
+
+    tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx));
+    tokio::spawn(tx_bridge(socket, incoming_rx));
+    Ok(bus_side)
+}
+
+/// socket → 通道：真实总线反馈进入协议 bus。
+async fn rx_bridge(socket: Arc<CanSocket>, tx: mpsc::Sender<LinkFrame>) {
+    loop {
+        match socket.read_frame().await {
+            Ok(frame) => {
+                if let Some(frame) = to_link_frame(&frame) {
+                    if tx.send(frame).await.is_err() {
+                        debug!("bus side closed, rx bridge exiting");
+                        return;
+                    }
+                }
+            }
+            Err(e) => {
+                // 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程，
+                // 由 systemd 拉起重连。
+                error!(error = %e, "CAN read failed, rx bridge exiting");
+                return;
+            }
+        }
+    }
+}
+
+/// 通道 → socket：协议 bus 命令写入真实总线。
+async fn tx_bridge(socket: Arc<CanSocket>, mut rx: mpsc::Receiver<LinkFrame>) {
+    while let Some(frame) = rx.recv().await {
+        match to_socketcan_frame(&frame) {
+            Some(sc_frame) => {
+                if let Err(e) = socket.write_frame(sc_frame).await {
+                    error!(error = %e, "CAN write failed, tx bridge exiting");
+                    return;
+                }
+            }
+            None => {
+                debug!(id = frame.id, "drop frame with non-standard CAN id");
+            }
+        }
+    }
+    debug!("bus side closed, tx bridge exiting");
+}
+
+/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。
+fn to_link_frame(frame: &ScFrame) -> Option<LinkFrame> {
+    match frame {
+        ScFrame::Data(df) if df.data().len() == 8 => {
+            Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?))
+        }
+        _ => None,
+    }
+}
+
+/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。
+fn to_socketcan_frame(frame: &LinkFrame) -> Option<ScFrame> {
+    let id = socketcan::StandardId::new(frame.id as u16)?;
+    let df = CanDataFrame::new(id, &frame.data)?;
+    Some(ScFrame::Data(df))
+}

crates/dji-esc-socketcan/src/lib.rs

@@ -0,0 +1,11 @@
+//! SocketCAN 桥接：把真实 Linux CAN 接口接入 [`can_link::CanLink`] 通道对。
+//!
+//! 本 crate 主体仅在 Linux 下编译（依赖 socketcan）。其他平台编译为空壳，
+//! 使整个 workspace 可以在任意开发机上构建与测试；实车部署目标板均为
+//! Linux。
+
+#[cfg(target_os = "linux")]
+mod bridge;
+
+#[cfg(target_os = "linux")]
+pub use bridge::open_link;

crates/dji-esc-socketcan/tests/vcan.rs

@@ -0,0 +1,132 @@
+//! vcan 集成测试：在真实 Linux 内核 CAN 栈上验证桥接。
+//!
+//! 仅 Linux 下编译；默认 `#[ignore]`，需要 vcan0（见 justfile `vcan-test`）。
+
+#![cfg(target_os = "linux")]
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use dji_esc::{CanFrame, CanLink};
+use motor_core::Motor;
+use socketcan::tokio::CanSocket;
+use socketcan::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId};
+use tokio::time::timeout;
+
+const IFACE: &str = "vcan0";
+
+fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame {
+    ScFrame::Data(CanDataFrame::new(StandardId::new(id).unwrap(), &data).unwrap())
+}
+
+#[tokio::test]
+#[ignore = "requires vcan0 (see `just vcan-test`)"]
+async fn bridge_relays_command_to_bus() {
+    let link = dji_esc_socketcan::open_link(IFACE).unwrap();
+    let observer = CanSocket::open(IFACE).unwrap();
+
+    let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
+    link.outgoing_tx.send(command).await.unwrap();
+
+    let got = timeout(Duration::from_secs(2), observer.read_frame())
+        .await
+        .expect("timeout waiting for command frame")
+        .unwrap();
+    match got {
+        ScFrame::Data(df) => {
+            assert_eq!(df.raw_id(), 0x200);
+            assert_eq!(df.data(), &[0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
+        }
+        other => panic!("expected data frame, got {other:?}"),
+    }
+}
+
+#[tokio::test]
+#[ignore = "requires vcan0 (see `just vcan-test`)"]
+async fn bridge_relays_feedback_from_bus() {
+    let CanLink { outgoing_tx: _, mut incoming_rx } = dji_esc_socketcan::open_link(IFACE).unwrap();
+    let simulator = CanSocket::open(IFACE).unwrap();
+
+    // 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。
+    let feedback = sc_data_frame(0x201, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
+    simulator.write_frame(feedback).await.unwrap();
+
+    let got = timeout(Duration::from_secs(2), incoming_rx.recv())
+        .await
+        .expect("timeout waiting for feedback frame")
+        .unwrap();
+    assert_eq!(got.id, 0x201);
+    assert_eq!(got.data, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
+}
+
+#[tokio::test]
+#[ignore = "requires vcan0 (see `just vcan-test`)"]
+async fn esc_bus_end_to_end_over_vcan() {
+    // EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。
+    let link = dji_esc_socketcan::open_link(IFACE).unwrap();
+    let mut esc = EscSim::new(IFACE, &[1]);
+
+    let bus = dji_esc::EscBus::spawn(
+        link,
+        vec![dji_esc::EscConfig::gm3508("m1", 1, 1.0)],
+        dji_esc::BusOptions::default(),
+    )
+    .unwrap();
+    bus.motor(1).unwrap().set_velocity(2000.0);
+
+    // 仿真 2s：模拟电调按命令回反馈，速度环应收敛。
+    let motor = bus.motor(1).unwrap().clone();
+    let deadline = tokio::time::Instant::now() + Duration::from_secs(2);
+    loop {
+        esc.step_once().await;
+        if motor.state().rotor_speed_rpm > 1900.0 {
+            break;
+        }
+        assert!(
+            tokio::time::Instant::now() < deadline,
+            "not converged: {}",
+            motor.state().rotor_speed_rpm
+        );
+        tokio::time::sleep(Duration::from_millis(5)).await;
+    }
+    bus.shutdown().await;
+}
+
+/// 最小电调模拟：收命令帧 → 更新转速 → 回反馈帧。
+struct EscSim {
+    socket: CanSocket,
+    speed_rpm: f32,
+    angle: u16,
+}
+
+impl EscSim {
+    fn new(iface: &str, _esc_ids: &[u8]) -> Self {
+        Self { socket: CanSocket::open(iface).unwrap(), speed_rpm: 0.0, angle: 0 }
+    }
+
+    /// 排空当前命令帧并推进模型一步（dt = 5ms）。
+    async fn step_once(&mut self) {
+        const DT: f32 = 0.005;
+        let mut current = 0i16;
+        while let Ok(frame) = self.socket.try_read_frame() {
+            if let ScFrame::Data(df) = frame {
+                if df.raw_id() == 0x200 {
+                    current = i16::from_be_bytes([df.data()[0], df.data()[1]]);
+                }
+            }
+        }
+        let target = f32::from(current) * 9254.0 / 16384.0;
+        self.speed_rpm += (target - self.speed_rpm) * (1.0 - (-DT / 0.12).exp());
+        self.angle =
+            (u32::from(self.angle) + (self.speed_rpm / 60.0 * 8192.0 * DT) as u32) as u16 % 8192;
+        let data = {
+            let mut d = [0u8; 8];
+            d[0..2].copy_from_slice(&self.angle.to_be_bytes());
+            d[2..4].copy_from_slice(&(self.speed_rpm as i16).to_be_bytes());
+            d[4..6].copy_from_slice(&current.to_be_bytes());
+            d[6] = 30;
+            d
+        };
+        self.socket.write_frame(sc_data_frame(0x201, data)).await.unwrap();
+    }
+}

crates/dji-esc/Cargo.toml

@@ -0,0 +1,20 @@
+[package]
+name = "dji-esc"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "DJI RoboMaster ESC (C620/C610) CAN protocol: codec, multi-turn tracking, and async bus driver"
+
+[lints]
+workspace = true
+
+[dependencies]
+motor-core = { workspace = true }
+can-link = { workspace = true }
+tokio = { workspace = true }
+thiserror = { workspace = true }
+tracing = { workspace = true }
+
+[dev-dependencies]
+tokio = { workspace = true, features = ["test-util"] }

crates/dji-esc/src/bus.rs

@@ -0,0 +1,512 @@
+//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。
+//!
+//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务：
+//!
+//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）；
+//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发
+//!   effort），按 `0x200`/`0x1FF` 分组组帧发送。
+//!
+//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。
+//! 命令模式切换时同样复位控制器，避免积分冲击。
+
+use std::sync::{Arc, Mutex, MutexGuard};
+use std::time::Duration;
+
+use motor_core::{
+    Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController,
+    PositionDrive, VelocityController, VelocityDrive,
+};
+use thiserror::Error;
+use tokio::sync::{mpsc, watch};
+use tokio::task::JoinHandle;
+use tokio::time::{Instant, Interval, MissedTickBehavior};
+use tracing::{debug, info, warn};
+
+use crate::codec::{
+    EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
+};
+use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
+use crate::turn::TurnTracker;
+
+/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌，
+/// 状态本身仍一致，驱动机不应因此停摆）。
+fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
+    mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
+}
+
+/// 单台电机的配置。
+#[derive(Debug, Clone)]
+pub struct EscConfig {
+    /// 便于日志辨认的名字（如 `front_left`）。
+    pub name: String,
+    /// 电调 ID（1–8，即 CAN 拨码）。
+    pub esc_id: u8,
+    /// 电调型号。
+    pub model: EscModel,
+    /// 减速比（电机转子 : 输出轴）。
+    pub gear_ratio: f32,
+    /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
+    /// 上层永远以"输出轴正转 = 前进方向"工作。
+    pub direction: f32,
+    /// effort 限幅（raw，`1..=型号满量程`）。
+    pub current_limit: i16,
+    /// 速度环比例增益（raw / rpm）。
+    pub kp: f32,
+    /// 速度环积分增益（raw / (rpm·s)）。
+    pub ki: f32,
+    /// 速度环微分增益。
+    pub kd: f32,
+    /// 速度前馈（raw / rpm）。
+    pub feedforward: f32,
+}
+
+impl EscConfig {
+    /// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值，需按整车整定。
+    #[must_use]
+    pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
+        Self {
+            name: name.into(),
+            esc_id,
+            model: EscModel::C620,
+            // 3591 / 187 ≈ 19.203
+            gear_ratio: 3591.0 / 187.0,
+            direction,
+            current_limit: 12_000,
+            kp: 10.0,
+            ki: 0.5,
+            kd: 0.0,
+            feedforward: 1.5,
+        }
+    }
+
+    /// M2006 + C610（拨弹轮）的常用配置。
+    ///
+    /// 减速比按 P36 起步（36.0），接硬件前需核对；电流标度见 [`EscModel::C610`]。
+    #[must_use]
+    pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
+        Self {
+            name: name.into(),
+            esc_id,
+            model: EscModel::C610,
+            // M2006 P36 减速比（接入前核对）
+            gear_ratio: 36.0,
+            direction,
+            current_limit: 8_000,
+            kp: 8.0,
+            ki: 0.4,
+            kd: 0.0,
+            feedforward: 1.0,
+        }
+    }
+
+    fn validate(&self) -> Result<(), String> {
+        if command_frame_id(self.esc_id).is_none() {
+            return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
+        }
+        if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
+            return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
+        }
+        // 逐位比较：direction 只允许精确的 ±1.0。
+        let dir_bits = self.direction.to_bits();
+        if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() {
+            return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction));
+        }
+        if !(1..=self.model.max_current_raw()).contains(&self.current_limit) {
+            return Err(format!(
+                "current_limit {} out of range 1..={} for {:?}",
+                self.current_limit,
+                self.model.max_current_raw(),
+                self.model
+            ));
+        }
+        Ok(())
+    }
+}
+
+/// 总线运行参数。
+#[derive(Debug, Clone)]
+pub struct BusOptions {
+    /// 控制节拍（C620 期望 1kHz 命令）。
+    pub period: Duration,
+    /// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。
+    pub feedback_timeout: Duration,
+}
+
+impl Default for BusOptions {
+    fn default() -> Self {
+        Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) }
+    }
+}
+
+/// 启动错误。
+#[derive(Debug, Clone, PartialEq, Eq, Error)]
+pub enum EscBusError {
+    /// 两台电机配置了同一电调 ID。
+    #[error("duplicate ESC id {0}")]
+    DuplicateEscId(u8),
+    /// 电机配置非法（原因见 `reason`）。
+    #[error("invalid config for motor {name}: {reason}")]
+    InvalidConfig {
+        /// 电机名。
+        name: String,
+        /// 非法原因。
+        reason: String,
+    },
+    /// 一台电机都没有配置。
+    #[error("no motors configured")]
+    NoMotors,
+}
+
+/// 单电机共享状态（驱动任务与 [`EscMotor`] 句柄之间）。
+struct MotorShared {
+    cfg: EscConfig,
+    state: Mutex<MotorState>,
+    cmd: Mutex<Command>,
+    last_mode: Mutex<CommandMode>,
+    vel_ctrl: Mutex<VelocityController>,
+    pos_ctrl: Mutex<PositionController>,
+    turn: Mutex<TurnTracker>,
+    last_feedback: Mutex<Option<Instant>>,
+    offline_warned: std::sync::atomic::AtomicBool,
+}
+
+impl MotorShared {
+    fn new(cfg: EscConfig) -> Self {
+        let limit = cfg.current_limit;
+        let pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
+        let pos_pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
+        Self {
+            vel_ctrl: Mutex::new(VelocityController::new(pid, cfg.feedforward)),
+            pos_ctrl: Mutex::new(PositionController::direct(pos_pid)),
+            cfg,
+            state: Mutex::new(MotorState::default()),
+            cmd: Mutex::new(Command::Disabled),
+            last_mode: Mutex::new(CommandMode::Disabled),
+            turn: Mutex::new(TurnTracker::new()),
+            last_feedback: Mutex::new(None),
+            offline_warned: std::sync::atomic::AtomicBool::new(false),
+        }
+    }
+
+    /// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
+    fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
+        let esc_rev = lock(&self.turn).update(fb.angle_raw);
+        let dir = self.cfg.direction;
+        {
+            let mut st = lock(&self.state);
+            st.rotor_angle_raw = fb.angle_raw;
+            st.rotor_revolutions = dir * esc_rev;
+            st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm);
+            st.effort_raw = (dir * f32::from(fb.current_raw)) as i16;
+            st.temperature_c = fb.temperature_c;
+            st.feedback_count = st.feedback_count.saturating_add(1);
+        }
+        *lock(&self.last_feedback) = Some(now);
+        self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
+    }
+
+    /// 复位主机侧控制器（离线或模式切换时）。
+    fn reset_controllers(&self) {
+        lock(&self.vel_ctrl).reset();
+        lock(&self.pos_ctrl).reset();
+    }
+
+    /// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。
+    fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 {
+        let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout);
+        if !online {
+            if !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
+                warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current");
+            }
+            // 复位控制器，避免重新上线时旧积分冲击。
+            self.reset_controllers();
+            *lock(&self.last_mode) = CommandMode::Disabled;
+            return 0;
+        }
+        let cmd = *lock(&self.cmd);
+        // 模式切换复位：避免积分/微分记忆冲击。
+        {
+            let mut last_mode = lock(&self.last_mode);
+            if *last_mode != cmd.mode() {
+                debug!(motor = %self.cfg.name, from = ?*last_mode, to = ?cmd.mode(), "command mode switch, resetting controllers");
+                self.reset_controllers();
+                *last_mode = cmd.mode();
+            }
+        }
+        let dir = self.cfg.direction;
+        let raw = match cmd {
+            Command::Disabled => 0,
+            Command::EffortRaw(raw) => (f32::from(raw) * dir) as i16,
+            Command::Velocity(target) => {
+                let measured = lock(&self.state).rotor_speed_rpm;
+                let out = lock(&self.vel_ctrl).update(target, measured, dt);
+                (out * dir) as i16
+            }
+            Command::Position(target) => {
+                let (measured_rev, measured_rpm) = {
+                    let st = lock(&self.state);
+                    (st.rotor_revolutions, st.rotor_speed_rpm)
+                };
+                let out = lock(&self.pos_ctrl).update(target, measured_rev, measured_rpm, dt);
+                (out * dir) as i16
+            }
+        };
+        let limit = self.cfg.current_limit;
+        clamp_current(self.cfg.model, raw.clamp(-limit, limit))
+    }
+}
+
+/// 电机的公共句柄：可克隆，跨任务共享。
+#[derive(Clone)]
+pub struct EscMotor {
+    shared: Arc<MotorShared>,
+}
+
+impl EscMotor {
+    /// 电机名。
+    #[must_use]
+    pub fn name(&self) -> &str {
+        &self.shared.cfg.name
+    }
+
+    /// 电机配置。
+    #[must_use]
+    pub fn config(&self) -> &EscConfig {
+        &self.shared.cfg
+    }
+
+    /// 当前命令。
+    #[must_use]
+    pub fn command(&self) -> Command {
+        *lock(&self.shared.cmd)
+    }
+
+    /// 最近一帧反馈的时刻（诊断用）。
+    #[must_use]
+    pub fn last_feedback(&self) -> Option<Instant> {
+        *lock(&self.shared.last_feedback)
+    }
+}
+
+impl MotorFeedback for EscMotor {
+    fn id(&self) -> u8 {
+        self.shared.cfg.esc_id
+    }
+
+    fn state(&self) -> MotorState {
+        *lock(&self.shared.state)
+    }
+}
+
+impl VelocityDrive for EscMotor {
+    fn set_velocity(&self, rotor_rpm: f32) {
+        *lock(&self.shared.cmd) = Command::Velocity(rotor_rpm);
+    }
+}
+
+impl EffortDrive for EscMotor {
+    fn set_effort_raw(&self, raw: i16) {
+        *lock(&self.shared.cmd) = Command::EffortRaw(raw);
+    }
+}
+
+impl PositionDrive for EscMotor {
+    fn set_position(&self, rotor_rev: f32) {
+        *lock(&self.shared.cmd) = Command::Position(rotor_rev);
+    }
+}
+
+impl Disable for EscMotor {
+    fn disable(&self) {
+        *lock(&self.shared.cmd) = Command::Disabled;
+    }
+}
+
+/// 运行中的电调总线。
+pub struct EscBus;
+
+impl EscBus {
+    /// 启动收发任务，返回句柄。
+    ///
+    /// # Errors
+    ///
+    /// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
+    pub fn spawn(
+        link: CanLink,
+        configs: Vec<EscConfig>,
+        options: BusOptions,
+    ) -> Result<EscBusHandle, EscBusError> {
+        if configs.is_empty() {
+            return Err(EscBusError::NoMotors);
+        }
+        let mut motors = Vec::with_capacity(configs.len());
+        let mut seen = std::collections::HashSet::new();
+        for cfg in configs {
+            cfg.validate()
+                .map_err(|reason| EscBusError::InvalidConfig { name: cfg.name.clone(), reason })?;
+            if !seen.insert(cfg.esc_id) {
+                return Err(EscBusError::DuplicateEscId(cfg.esc_id));
+            }
+            motors.push(EscMotor { shared: Arc::new(MotorShared::new(cfg)) });
+        }
+
+        let (shutdown_tx, shutdown_rx) = watch::channel(false);
+        let CanLink { outgoing_tx, incoming_rx } = link;
+        let motors = Arc::new(motors);
+        let rx_handle =
+            tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
+        let tx_handle =
+            tokio::spawn(tx_loop(outgoing_tx, Arc::clone(&motors), options, shutdown_rx));
+
+        Ok(EscBusHandle { motors, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
+    }
+}
+
+/// `EscBus` 的控制句柄。
+pub struct EscBusHandle {
+    motors: Arc<Vec<EscMotor>>,
+    shutdown_tx: watch::Sender<bool>,
+    tasks: Vec<JoinHandle<()>>,
+}
+
+impl EscBusHandle {
+    /// 按电调 ID 取电机句柄。
+    #[must_use]
+    pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> {
+        self.motors.iter().find(|m| m.id() == esc_id)
+    }
+
+    /// 全部电机（按配置顺序）。
+    #[must_use]
+    pub fn motors(&self) -> &[EscMotor] {
+        &self.motors
+    }
+
+    /// 停止收发任务并等待退出。
+    pub async fn shutdown(self) {
+        let _ = self.shutdown_tx.send(true);
+        for task in self.tasks {
+            let _ = task.await;
+        }
+    }
+}
+
+async fn rx_loop(
+    mut incoming: mpsc::Receiver<CanFrame>,
+    motors: Arc<Vec<EscMotor>>,
+    mut shutdown: watch::Receiver<bool>,
+) {
+    loop {
+        let frame = tokio::select! {
+            _ = shutdown.changed() => break,
+            frame = incoming.recv() => match frame {
+                Some(frame) => frame,
+                None => break,
+            },
+        };
+        match decode_feedback(&frame) {
+            Ok((esc_id, fb)) => {
+                if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) {
+                    motor.shared.apply_feedback(fb, Instant::now());
+                }
+            }
+            Err(e) => {
+                debug!(error = %e, id = frame.id, "ignore non-feedback frame");
+            }
+        }
+    }
+}
+
+async fn tx_loop(
+    outgoing: mpsc::Sender<CanFrame>,
+    motors: Arc<Vec<EscMotor>>,
+    options: BusOptions,
+    mut shutdown: watch::Receiver<bool>,
+) {
+    let dt = options.period.as_secs_f32();
+    let mut ticker = tick_every(options.period);
+    loop {
+        tokio::select! {
+            _ = shutdown.changed() => break,
+            _ = ticker.tick() => {}
+        }
+        let now = Instant::now();
+        for (frame_id, group_start) in
+            [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
+        {
+            let group: Vec<&EscMotor> = motors
+                .iter()
+                .filter(|m| (group_start..group_start + 4).contains(&m.id()))
+                .collect();
+            if group.is_empty() {
+                continue;
+            }
+            let mut currents = [0i16; 4];
+            for motor in &group {
+                let slot = usize::from(motor.id() - group_start);
+                currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now);
+            }
+            if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
+                info!("can link closed, tx loop exiting");
+                return;
+            }
+        }
+    }
+}
+
+fn tick_every(period: Duration) -> Interval {
+    let mut ticker = tokio::time::interval(period);
+    // 错过节拍后直接跳到当前节拍，避免补发帧风暴。
+    ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+    ticker
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn config_validate_rejects_bad_values() {
+        let mut cfg = EscConfig::gm3508("m", 1, 1.0);
+
+        cfg.esc_id = 0;
+        assert!(cfg.validate().is_err());
+        cfg.esc_id = 9;
+        assert!(cfg.validate().is_err());
+        cfg.esc_id = 5;
+
+        cfg.gear_ratio = 0.0;
+        assert!(cfg.validate().is_err());
+        cfg.gear_ratio = 19.2;
+
+        cfg.direction = 0.5;
+        assert!(cfg.validate().is_err());
+        cfg.direction = -1.0;
+
+        cfg.current_limit = 0;
+        assert!(cfg.validate().is_err());
+        cfg.current_limit = 16385;
+        assert!(cfg.validate().is_err());
+        cfg.current_limit = 12_000;
+
+        assert!(cfg.validate().is_ok());
+    }
+
+    #[test]
+    fn gm3508_defaults_are_sane() {
+        let cfg = EscConfig::gm3508("front_left", 1, -1.0);
+        assert_eq!(cfg.model, EscModel::C620);
+        assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
+        assert_eq!(cfg.current_limit, 12_000);
+        assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits());
+        assert!(cfg.validate().is_ok());
+    }
+
+    #[test]
+    fn m2006_defaults_are_sane() {
+        let cfg = EscConfig::m2006("feeder", 5, 1.0);
+        assert_eq!(cfg.model, EscModel::C610);
+        assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
+        assert!(cfg.validate().is_ok());
+    }
+}

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);
+    }
+}

crates/dji-esc/src/frame.rs

@@ -0,0 +1,13 @@
+//! DJI 电调协议帧 ID 常量。
+//!
+//! 帧与链路类型（[`CanFrame`] / [`CanLink`]）由 [`can_link`] 提供，
+//! 本模块只保留 **C620/C610 电流协议族**的 ID 约定。
+
+pub use can_link::{CanFrame, CanLink};
+
+/// 控制帧 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;

crates/dji-esc/src/lib.rs

@@ -0,0 +1,29 @@
+//! DJI RoboMaster 电调（C620/C610）CAN 协议与异步驱动。
+//!
+//! 分层：
+//!
+//! - [`codec`]：纯函数帧编解码（C620 手册协议）；
+//! - [`turn`]：单圈编码器 → 连续多圈角；
+//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）；
+//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全；
+//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。
+//!
+//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction`
+//! 完成方向修正。
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+pub mod bus;
+pub mod codec;
+pub mod frame;
+pub mod sim;
+pub mod turn;
+
+pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor};
+pub use can_link::{CanFrame, CanLink};
+pub use codec::{
+    DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id,
+    decode_feedback, encode_current_frame, encode_feedback_frame,
+};
+pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE};
+pub use sim::{PlantMotor, spawn_plant};
+pub use turn::{ENCODER_TICKS, TurnTracker};

crates/dji-esc/src/sim.rs

@@ -0,0 +1,116 @@
+//! 电机被控对象仿真：一阶转速模型 + 编码器积分。
+//!
+//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环
+//! （测试、`chassisd --mock` 均复用）。
+
+use std::collections::HashMap;
+use std::time::Duration;
+
+use tokio::task::JoinHandle;
+use tokio::time::{MissedTickBehavior, interval};
+use tracing::trace;
+
+use crate::codec::{EscFeedback, encode_feedback_frame};
+use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
+
+/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。
+///
+/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数
+/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。
+#[derive(Debug, Clone)]
+pub struct PlantMotor {
+    gain_rpm_per_raw: f32,
+    tau: f32,
+    max_rpm: f32,
+    speed_rpm: f32,
+    angle_raw: f32,
+}
+
+impl PlantMotor {
+    /// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。
+    #[must_use]
+    pub fn gm3508() -> Self {
+        Self {
+            gain_rpm_per_raw: 9254.0 / 16384.0,
+            tau: 0.12,
+            max_rpm: 9254.0,
+            speed_rpm: 0.0,
+            angle_raw: 0.0,
+        }
+    }
+
+    /// 当前转子转速（rpm，电调侧）。
+    #[must_use]
+    pub fn speed_rpm(&self) -> f32 {
+        self.speed_rpm
+    }
+
+    /// 推进一个时间步，返回本步反馈帧内容。
+    pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
+        let target =
+            (f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm);
+        let alpha = 1.0 - (-dt / self.tau).exp();
+        self.speed_rpm += (target - self.speed_rpm) * alpha;
+        self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0);
+        EscFeedback {
+            angle_raw: self.angle_raw as u16,
+            speed_rpm: self.speed_rpm as i16,
+            current_raw,
+            temperature_c: 30,
+        }
+    }
+}
+
+/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。
+///
+/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。
+pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
+    let mut plants: HashMap<u8, PlantMotor> =
+        esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect();
+    let mut commands: HashMap<u8, i16> = esc_ids.iter().map(|&id| (id, 0)).collect();
+    let dt = tick.as_secs_f32();
+    let mut ticker = interval(tick);
+    ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+
+    tokio::spawn(async move {
+        loop {
+            tokio::select! {
+                frame = link.incoming_rx.recv() => match frame {
+                    Some(frame) => apply_command_frame(&frame, &mut commands),
+                    None => break,
+                },
+                _ = ticker.tick() => {
+                    for (&id, plant) in &mut plants {
+                        let cmd = commands.get(&id).copied().unwrap_or(0);
+                        let feedback = plant.step(cmd, dt);
+                        if link
+                            .outgoing_tx
+                            .send(encode_feedback_frame(id, &feedback))
+                            .await
+                            .is_err()
+                        {
+                            return;
+                        }
+                    }
+                }
+            }
+        }
+        trace!("plant exiting");
+    })
+}
+
+fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap<u8, i16>) {
+    let group_start = match frame.id {
+        COMMAND_FRAME_ID_PRIMARY => 1u8,
+        COMMAND_FRAME_ID_SECONDARY => 5u8,
+        _ => return,
+    };
+    for slot in 0..4u8 {
+        if let Some(cmd) = commands.get_mut(&(group_start + slot)) {
+            *cmd = i16::from_be_bytes([
+                frame.data[(slot * 2) as usize],
+                frame.data[(slot * 2 + 1) as usize],
+            ]);
+        }
+    }
+}

crates/dji-esc/src/turn.rs

@@ -0,0 +1,134 @@
+//! 多圈累计：单圈编码器原始角 → 连续圈数。
+
+/// 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);
+    }
+}

crates/dji-esc/tests/closed_loop.rs

@@ -0,0 +1,275 @@
+//! 闭环集成测试：`EscBus` + 内存通道 + 一阶电机仿真。
+//!
+//! 全部可在任意平台（macOS / Linux / CI）运行，无需硬件。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use dji_esc::{
+    BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback,
+    encode_feedback_frame, spawn_plant,
+};
+use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
+use tokio::time::{sleep, timeout};
+
+/// 与 `PlantMotor::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
+fn tuned(esc_id: u8, direction: f32) -> EscConfig {
+    let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction);
+    cfg.kp = 8.0;
+    cfg.ki = 2.0;
+    cfg.kd = 0.0;
+    cfg.feedforward = 16384.0 / 9254.0;
+    cfg
+}
+
+/// 轮询直到条件满足或超时（毫秒，仿真时间）。
+async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
+    let mut waited = 0u64;
+    while waited < total_ms {
+        if cond() {
+            return true;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+    cond()
+}
+
+/// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。
+async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) {
+    for &id in esc_ids {
+        let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
+        peer.outgoing_tx.send(encode_feedback_frame(id, &fb)).await.unwrap();
+    }
+}
+
+async fn recv_command(peer: &mut CanLink) -> CanFrame {
+    timeout(Duration::from_millis(100), peer.incoming_rx.recv())
+        .await
+        .expect("timed out waiting for command frame")
+        .expect("link closed")
+}
+
+fn slot_current(frame: &CanFrame, slot: usize) -> i16 {
+    i16::from_be_bytes([frame.data[slot * 2], frame.data[slot * 2 + 1]])
+}
+
+// ---------------------------------------------------------------- 收敛性
+
+#[tokio::test(start_paused = true)]
+async fn velocity_loop_converges() {
+    let (bus_link, plant_link) = CanLink::pair(512);
+    let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+    let motor = bus.motor(1).unwrap().clone();
+
+    motor.set_velocity(3000.0);
+    let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 2950.0).await;
+    assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
+
+    // 稳态保持（速度无漂移、反馈持续流入）。
+    let count_before = motor.state().feedback_count;
+    sleep(Duration::from_millis(300)).await;
+    let st = motor.state();
+    assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}");
+    assert!(
+        st.feedback_count >= count_before + 280,
+        "feedback should keep flowing: {} → {}",
+        count_before,
+        st.feedback_count
+    );
+
+    plant.abort();
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn reversed_motor_converges_in_physical_frame() {
+    // direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。
+    let (bus_link, plant_link) = CanLink::pair(512);
+    let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1));
+    let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
+    let motor = bus.motor(2).unwrap().clone();
+
+    motor.set_velocity(2000.0);
+    let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 1950.0).await;
+    assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
+
+    let st = motor.state();
+    assert!((1950.0..=2050.0).contains(&st.rotor_speed_rpm), "{st:?}");
+    // 物理转速为正 ⇒ 电调电流为负 ⇒ 物理电流 = dir × esc > 0。
+    assert!(st.effort_raw > 0, "{st:?}");
+
+    plant.abort();
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 安全
+
+#[tokio::test(start_paused = true)]
+async fn motor_without_feedback_commands_zero_current() {
+    let (bus_link, mut peer) = CanLink::pair(64);
+    // 不接 plant → 永远没有反馈。
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+
+    bus.motor(1).unwrap().set_velocity(5000.0);
+    for _ in 0..5 {
+        let frame = recv_command(&mut peer).await;
+        assert_eq!(frame.id, 0x200);
+        assert_eq!(frame.data, [0; 8], "must send zero current while offline");
+    }
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn current_limit_clamps_saturation() {
+    let (bus_link, mut peer) = CanLink::pair(64);
+    let mut cfg = tuned(1, 1.0);
+    cfg.current_limit = 5000;
+    let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).unwrap();
+
+    send_feedback(&peer, &[1]).await;
+    bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和
+    sleep(Duration::from_millis(5)).await;
+
+    let mut saw_saturated = false;
+    for _ in 0..5 {
+        let frame = recv_command(&mut peer).await;
+        let raw = slot_current(&frame, 0);
+        assert!(raw <= 5000, "current {raw} exceeds limit 5000");
+        saw_saturated |= raw == 5000;
+    }
+    assert!(saw_saturated, "loop should reach the configured limit");
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 组帧与路由
+
+#[tokio::test(start_paused = true)]
+async fn current_commands_route_to_group_slots() {
+    let (bus_link, mut peer) = CanLink::pair(256);
+    let configs = (1..=5).map(|id| tuned(id, 1.0)).collect();
+    let bus = EscBus::spawn(bus_link, configs, BusOptions::default()).unwrap();
+
+    bus.motor(3).unwrap().set_effort_raw(1234);
+    bus.motor(5).unwrap().set_effort_raw(-567);
+    send_feedback(&peer, &(1..=5).collect::<Vec<_>>()).await;
+    sleep(Duration::from_millis(5)).await;
+
+    let mut saw_200 = false;
+    let mut saw_1ff = false;
+    for _ in 0..20 {
+        let frame = recv_command(&mut peer).await;
+        match frame.id {
+            0x200 if !saw_200 => {
+                assert_eq!(frame.data, [0, 0, 0, 0, 0x04, 0xD2, 0, 0], "esc3 slot = 1234");
+                saw_200 = true;
+            }
+            0x1FF if !saw_1ff => {
+                // -567 = 0xFDC9（i16 补码，大端）。
+                assert_eq!(frame.data, [0xFD, 0xC9, 0, 0, 0, 0, 0, 0], "esc5 slot = -567");
+                saw_1ff = true;
+            }
+            _ => {}
+        }
+        if saw_200 && saw_1ff {
+            break;
+        }
+    }
+    assert!(saw_200, "no 0x200 frame observed");
+    assert!(saw_1ff, "no 0x1FF frame observed");
+
+    // disable 后槽位清零。
+    bus.motor(3).unwrap().disable();
+    let zeroed = wait_until(200, || {
+        try_find_frame(&mut peer, |frame| frame.id == 0x200 && slot_current(frame, 2) == 0)
+    })
+    .await;
+    assert!(zeroed, "esc3 slot should return to 0 after disable");
+    bus.shutdown().await;
+}
+
+/// 非阻塞地扫一眼通道里已有的帧。
+fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool {
+    while let Ok(frame) = peer.incoming_rx.try_recv() {
+        if pred(&frame) {
+            return true;
+        }
+    }
+    false
+}
+
+// ---------------------------------------------------------------- 反馈 → 状态
+
+#[tokio::test]
+async fn feedback_updates_state_with_direction() {
+    let (bus_link, peer) = CanLink::pair(64);
+    let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
+
+    // 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。
+    let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 };
+    peer.outgoing_tx.send(encode_feedback_frame(2, &fb)).await.unwrap();
+    let fb2 = EscFeedback { angle_raw: 100, ..fb };
+    peer.outgoing_tx.send(encode_feedback_frame(2, &fb2)).await.unwrap();
+    sleep(Duration::from_millis(50)).await;
+
+    let st = bus.motor(2).unwrap().state();
+    assert_eq!(st.rotor_angle_raw, 100);
+    assert!((st.rotor_speed_rpm + 600.0).abs() < 1e-3); // direction = -1
+    assert_eq!(st.effort_raw, 300);
+    assert_eq!(st.temperature_c, 47);
+    assert_eq!(st.feedback_count, 2);
+    let esc_rev = 1.0 + 100.0 / 8192.0;
+    assert!(
+        (st.rotor_revolutions + esc_rev).abs() < 1e-3,
+        "revolutions = {}",
+        st.rotor_revolutions
+    );
+    // 输出轴换算由 `OutputShaft` 负责（见 motor-core gearbox 单测）。
+    bus.shutdown().await;
+}
+
+// ---------------------------------------------------------------- 生命周期
+
+#[tokio::test(start_paused = true)]
+async fn shutdown_stops_tasks_and_closes_link() {
+    let (bus_link, mut peer) = CanLink::pair(128);
+    // 不接 plant，直接验证任务生命周期。
+    let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
+    bus.motor(1).unwrap().set_velocity(1000.0);
+    sleep(Duration::from_millis(20)).await;
+    // 有命令帧在流动。
+    assert!(peer.incoming_rx.recv().await.is_some());
+
+    bus.shutdown().await;
+    // 排空残留缓冲帧后应立即得到 None（发送端已全部退出）；
+    // 若任务仍在运行，帧会持续流入并触发超时。
+    loop {
+        match timeout(Duration::from_millis(100), peer.incoming_rx.recv()).await {
+            Ok(None) => break,
+            Ok(Some(_)) => {} // 排空残留缓冲帧
+            Err(elapsed) => panic!("bus tasks still running after shutdown (waited {elapsed:?})"),
+        }
+    }
+}
+
+#[test]
+fn spawn_rejects_bad_configs() {
+    let link = || CanLink::pair(8).0;
+    assert_eq!(
+        EscBus::spawn(link(), vec![], BusOptions::default()).err(),
+        Some(EscBusError::NoMotors)
+    );
+    assert_eq!(
+        EscBus::spawn(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(),
+        Some(EscBusError::DuplicateEscId(2))
+    );
+    let mut bad = tuned(1, 1.0);
+    bad.direction = 0.0;
+    let err = EscBus::spawn(link(), vec![bad], BusOptions::default()).err();
+    assert!(
+        matches!(err, Some(EscBusError::InvalidConfig { .. })),
+        "expected InvalidConfig, got {err:?}"
+    );
+}

crates/mecanum/Cargo.toml

@@ -0,0 +1,16 @@
+[package]
+name = "mecanum"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "Mecanum wheel chassis forward/inverse kinematics"
+
+[lints]
+workspace = true
+
+[dependencies]
+
+[dev-dependencies]
+proptest = { workspace = true }
+approx = { workspace = true }

crates/mecanum/proptest-regressions/lib.txt

@@ -0,0 +1,7 @@
+# Seeds for failure cases proptest has generated in the past. It is
+# automatically read and these particular cases re-run before any
+# novel cases are generated.
+#
+# It is recommended to check this file in to source control so that
+# everyone who runs the test benefits from these saved cases.
+cc 698ba63bf948a3702145ece17ed0f014133de0d0b3797fd8bdda514c5d2480a7 # shrinks to vx = 0.0, vy = 0.0, omega = -2.3901656

crates/mecanum/src/lib.rs

@@ -0,0 +1,284 @@
+//! 麦克纳姆轮底盘正 / 逆运动学。
+//!
+//! # 约定
+//!
+//! - 机器人坐标系（ISO）：x 前、y 左、ω 绕 z 逆时针为正；
+//! - 轮序固定为 `[前左, 前右, 后左, 后右]`（常量 [`FL`] / [`FR`] / [`RL`] /
+//!   [`RR`]）；
+//! - 辊子 45°、俯视呈 X 型布置（RoboMaster 底盘标准布局）；
+//! - 角速度单位 rad/s，线速度 m/s，换算见 [`wheel_rpm_from_angular`]。
+//!
+//! 四轮线速度（`a = half_length + half_width`）：
+//!
+//! ```text
+//! v_FL = vx - vy - a·ω
+//! v_FR = vx + vy + a·ω
+//! v_RL = vx + vy - a·ω
+//! v_RR = vx - vy + a·ω
+//! ```
+//!
+//! 正解为逆解的左伪逆（JᵀJ 对角，可解析求平均）。
+
+use std::f32::consts::TAU;
+
+/// 前左轮下标。
+pub const FL: usize = 0;
+/// 前右轮下标。
+pub const FR: usize = 1;
+/// 后左轮下标。
+pub const RL: usize = 2;
+/// 后右轮下标。
+pub const RR: usize = 3;
+
+/// 底盘平面速度：x 前进（m/s）、y 左移（m/s）、ω 逆时针（rad/s）。
+#[derive(Debug, Clone, Copy, PartialEq, Default)]
+pub struct Twist {
+    /// 前进速度（m/s，前正）。
+    pub vx: f32,
+    /// 横移速度（m/s，左正）。
+    pub vy: f32,
+    /// 角速度（rad/s，逆时针正）。
+    pub omega: f32,
+}
+
+impl Twist {
+    /// 全零速度。
+    pub const ZERO: Self = Self { vx: 0.0, vy: 0.0, omega: 0.0 };
+
+    /// 幅值限幅：任一分量超界时按比例收缩整体，保持运动方向不变。
+    ///
+    /// `max_forward` / `max_strafe` 单位 m/s，`max_spin` 单位 rad/s。
+    #[must_use]
+    pub fn clamped(self, max_forward: f32, max_strafe: f32, max_spin: f32) -> Self {
+        let scale = [
+            (self.vx.abs() / max_forward).max(f32::EPSILON),
+            (self.vy.abs() / max_strafe).max(f32::EPSILON),
+            (self.omega.abs() / max_spin).max(f32::EPSILON),
+        ]
+        .into_iter()
+        .fold(1.0f32, f32::max);
+        Self { vx: self.vx / scale, vy: self.vy / scale, omega: self.omega / scale }
+    }
+}
+
+/// 麦轮底盘几何参数。
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct Mecanum {
+    /// 前后轮轴线到底盘中心的纵向半距 lx（米，正）。
+    pub half_length: f32,
+    /// 左右轮到底盘中心的横向半距 ly（米，正）。
+    pub half_width: f32,
+    /// 车轮半径（米，正）。
+    pub wheel_radius: f32,
+}
+
+impl Mecanum {
+    /// 按几何参数构造。
+    ///
+    /// # Panics
+    ///
+    /// debug 构建下参数非正时 panic（配置层应先行校验）。
+    #[must_use]
+    pub fn new(half_length: f32, half_width: f32, wheel_radius: f32) -> Self {
+        debug_assert!(
+            half_length > 0.0 && half_width > 0.0 && wheel_radius > 0.0,
+            "mecanum geometry must be positive"
+        );
+        Self { half_length, half_width, wheel_radius }
+    }
+
+    /// 逆解：底盘速度 → 四轮角速度（rad/s，顺序 FL/FR/RL/RR）。
+    #[must_use]
+    pub fn wheel_speeds(&self, twist: &Twist) -> [f32; 4] {
+        let a = self.half_length + self.half_width;
+        [
+            twist.vx - twist.vy - a * twist.omega,
+            twist.vx + twist.vy + a * twist.omega,
+            twist.vx + twist.vy - a * twist.omega,
+            twist.vx - twist.vy + a * twist.omega,
+        ]
+        .map(|v| v / self.wheel_radius)
+    }
+
+    /// 正解：四轮角速度（rad/s）→ 底盘速度（最小二乘意义）。
+    #[must_use]
+    pub fn twist(&self, wheels: &[f32; 4]) -> Twist {
+        let w = wheels.map(|x| x * self.wheel_radius);
+        let a = self.half_length + self.half_width;
+        Twist {
+            vx: (w[FL] + w[FR] + w[RL] + w[RR]) / 4.0,
+            vy: (-w[FL] + w[FR] + w[RL] - w[RR]) / 4.0,
+            omega: (-w[FL] + w[FR] - w[RL] + w[RR]) / (4.0 * a),
+        }
+    }
+
+    /// 逆解并直接输出四轮转速（rpm）。
+    #[must_use]
+    pub fn wheel_rpms(&self, twist: &Twist) -> [f32; 4] {
+        self.wheel_speeds(twist).map(wheel_rpm_from_angular)
+    }
+
+    /// 由四轮转速（rpm）正解底盘速度。
+    #[must_use]
+    pub fn twist_from_wheel_rpms(&self, wheel_rpms: &[f32; 4]) -> Twist {
+        self.twist(&wheel_rpms.map(wheel_angular_from_rpm))
+    }
+}
+
+/// 轮角速度（rad/s）→ 转速（rpm）。
+#[must_use]
+pub const fn wheel_rpm_from_angular(rad_per_s: f32) -> f32 {
+    rad_per_s * 60.0 / TAU
+}
+
+/// 轮转速（rpm）→ 角速度（rad/s）。
+#[must_use]
+pub const fn wheel_angular_from_rpm(rpm: f32) -> f32 {
+    rpm * TAU / 60.0
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use approx::assert_relative_eq;
+
+    /// 0.2m 半轴距 / 0.25m 半轮距 / 76.2mm 轮半径的典型 RM 底盘。
+    fn chassis() -> Mecanum {
+        Mecanum::new(0.2, 0.25, 0.0762)
+    }
+
+    fn assert_twist_eq(a: &Twist, b: &Twist) {
+        assert_relative_eq!(a.vx, b.vx, epsilon = 1e-5);
+        assert_relative_eq!(a.vy, b.vy, epsilon = 1e-5);
+        assert_relative_eq!(a.omega, b.omega, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn pure_forward_drives_all_wheels_equally() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vx: 1.0, ..Twist::ZERO });
+        for x in w {
+            assert_relative_eq!(x, 1.0 / 0.0762, epsilon = 1e-5);
+        }
+    }
+
+    #[test]
+    fn pure_strafe_left_opposes_diagonal_wheels() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vy: 2.0, ..Twist::ZERO });
+        let v = 2.0 / 0.0762;
+        assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[FR], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RR], -v, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn pure_rotation_splits_left_right() {
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { omega: 1.0, ..Twist::ZERO });
+        let v = (0.2 + 0.25) / 0.0762;
+        // 逆时针：左侧后退、右侧前进。
+        assert_relative_eq!(w[FL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[FR], v, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], -v, epsilon = 1e-5);
+        assert_relative_eq!(w[RR], v, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn diagonal_motion_idles_two_wheels() {
+        // 前左 45° 对角线：vx = vy ⇒ FL、RR 停转。
+        let m = chassis();
+        let w = m.wheel_speeds(&Twist { vx: 1.0, vy: 1.0, omega: 0.0 });
+        assert_relative_eq!(w[FL], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(w[RR], 0.0, epsilon = 1e-6);
+        assert_relative_eq!(w[FR], 2.0 / 0.0762, epsilon = 1e-5);
+        assert_relative_eq!(w[RL], 2.0 / 0.0762, epsilon = 1e-5);
+    }
+
+    #[test]
+    fn zero_twist_zero_wheels() {
+        let wheels = chassis().wheel_speeds(&Twist::ZERO);
+        assert!(wheels.iter().all(|w| f64::from(*w).abs() < 1e-12), "{wheels:?}");
+    }
+
+    #[test]
+    fn inverse_then_forward_recovers_twist() {
+        let m = chassis();
+        let t = Twist { vx: 1.5, vy: -0.8, omega: 2.2 };
+        assert_twist_eq(&m.twist(&m.wheel_speeds(&t)), &t);
+    }
+
+    #[test]
+    fn rpm_helpers_roundtrip() {
+        assert_relative_eq!(
+            wheel_rpm_from_angular(wheel_angular_from_rpm(482.0)),
+            482.0,
+            epsilon = 1e-4
+        );
+        assert_relative_eq!(wheel_angular_from_rpm(60.0 / TAU), 1.0, epsilon = 1e-6);
+    }
+
+    #[test]
+    fn rpm_path_matches_angular_path() {
+        let m = chassis();
+        let t = Twist { vx: 0.3, vy: 1.1, omega: -0.7 };
+        assert_twist_eq(&m.twist_from_wheel_rpms(&m.wheel_rpms(&t)), &t);
+    }
+
+    #[test]
+    fn clamp_preserves_direction_and_limits() {
+        let t = Twist { vx: 4.0, vy: 1.0, omega: 0.5 }.clamped(2.0, 2.0, 3.0);
+        assert_relative_eq!(t.vx, 2.0, epsilon = 1e-6);
+        assert_relative_eq!(t.vy, 0.5, epsilon = 1e-6);
+        assert_relative_eq!(t.omega, 0.25, epsilon = 1e-6);
+        // 限内不缩放。
+        let keep = Twist { vx: 1.0, vy: -1.0, omega: 2.0 }.clamped(2.0, 2.0, 3.0);
+        assert_eq!(keep, Twist { vx: 1.0, vy: -1.0, omega: 2.0 });
+    }
+}
+
+#[cfg(test)]
+mod proptests {
+    use super::*;
+    use approx::assert_relative_eq;
+    use proptest::prelude::*;
+
+    proptest! {
+        /// 正逆解互逆：forward(inverse(t)) == t（对任意合理几何与速度）。
+        #[test]
+        fn kinematics_roundtrip(
+            lx in 0.05f32..0.5,
+            ly in 0.05f32..0.5,
+            r in 0.03f32..0.2,
+            vx in -3.0f32..3.0,
+            vy in -3.0f32..3.0,
+            omega in -6.0f32..6.0,
+        ) {
+            let m = Mecanum::new(lx, ly, r);
+            let t = Twist { vx, vy, omega };
+            let back = m.twist(&m.wheel_speeds(&t));
+            assert_relative_eq!(back.vx, t.vx, epsilon = 1e-4);
+            assert_relative_eq!(back.vy, t.vy, epsilon = 1e-4);
+            assert_relative_eq!(back.omega, t.omega, epsilon = 1e-4);
+        }
+
+        /// 左右镜像对称：vy、ω 取反（镜像底盘）等价于 FL↔FR、RL↔RR 对调。
+        #[test]
+        fn strafe_symmetry(
+            vx in -2.0f32..2.0,
+            vy in -2.0f32..2.0,
+            omega in -4.0f32..4.0,
+        ) {
+            let m = Mecanum::new(0.2, 0.25, 0.0762);
+            let t = Twist { vx, vy, omega };
+            let mirrored = Twist { vx, vy: -vy, omega: -omega };
+            let w = m.wheel_speeds(&t);
+            let w2 = m.wheel_speeds(&mirrored);
+            assert_relative_eq!(w[FL], w2[FR], epsilon = 1e-4);
+            assert_relative_eq!(w[FR], w2[FL], epsilon = 1e-4);
+            assert_relative_eq!(w[RL], w2[RR], epsilon = 1e-4);
+            assert_relative_eq!(w[RR], w2[RL], epsilon = 1e-4);
+        }
+    }
+}

crates/motor-core/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "motor-core"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+description = "Protocol-agnostic motor abstraction: Motor trait, feedback state, and PID controllers"
+
+[lints]
+workspace = true
+
+[dependencies]

crates/motor-core/src/command.rs

@@ -0,0 +1,41 @@
+//! 驱动层命令寄存器（最后写入胜出）。
+
+/// 发给驱动层的电机命令。
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub enum Command {
+    /// 停用：目标清零，下发 0 输出。
+    Disabled,
+    /// 直接下发开环 raw（协议量程，物理方向），绕过闭环。
+    /// ESC = 电流 raw；GM6020 = 电压 raw。
+    EffortRaw(i16),
+    /// 速度环跟踪目标转子转速（rpm，物理方向）。
+    Velocity(f32),
+    /// 位置环跟踪目标转子多圈位置（圈，物理方向）。
+    Position(f32),
+}
+
+/// 命令模式标签（不含设定值）——用于切换时复位控制器。
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum CommandMode {
+    /// 停用。
+    Disabled,
+    /// 开环 raw。
+    EffortRaw,
+    /// 速度环。
+    Velocity,
+    /// 位置环。
+    Position,
+}
+
+impl Command {
+    /// 模式标签。
+    #[must_use]
+    pub const fn mode(self) -> CommandMode {
+        match self {
+            Self::Disabled => CommandMode::Disabled,
+            Self::EffortRaw(_) => CommandMode::EffortRaw,
+            Self::Velocity(_) => CommandMode::Velocity,
+            Self::Position(_) => CommandMode::Position,
+        }
+    }
+}

crates/motor-core/src/gearbox.rs

@@ -0,0 +1,173 @@
+//! 输出轴单位适配器：减速比换算。
+//!
+//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
+//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
+
+use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
+
+/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
+pub struct OutputShaft<T> {
+    inner: T,
+    gear_ratio: f32,
+}
+
+impl<T> OutputShaft<T> {
+    /// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。
+    ///
+    /// # Panics
+    ///
+    /// `gear_ratio` 非有限或 ≤ 0 时 panic。
+    #[must_use]
+    pub fn new(inner: T, gear_ratio: f32) -> Self {
+        assert!(
+            gear_ratio.is_finite() && gear_ratio > 0.0,
+            "gear_ratio must be > 0, got {gear_ratio}"
+        );
+        Self { inner, gear_ratio }
+    }
+
+    /// 减速比。
+    #[must_use]
+    pub fn gear_ratio(&self) -> f32 {
+        self.gear_ratio
+    }
+
+    /// 底层电机引用。
+    #[must_use]
+    pub fn inner(&self) -> &T {
+        &self.inner
+    }
+
+    /// 转子量 → 输出轴量。
+    #[must_use]
+    pub fn to_output(&self, rotor: f32) -> f32 {
+        rotor / self.gear_ratio
+    }
+
+    /// 输出轴量 → 转子量。
+    #[must_use]
+    pub fn to_rotor(&self, output: f32) -> f32 {
+        output * self.gear_ratio
+    }
+
+    /// 由反馈快照推算输出轴转速（rpm）。
+    #[must_use]
+    pub fn output_speed_rpm(&self, state: &MotorState) -> f32 {
+        state.rotor_speed_rpm / self.gear_ratio
+    }
+}
+
+impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
+    fn id(&self) -> u8 {
+        self.inner.id()
+    }
+
+    fn state(&self) -> MotorState {
+        // 只透传转子量；不伪造输出轴字段。
+        self.inner.state()
+    }
+}
+
+impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
+    /// `output_rpm` 为**输出轴**转速。
+    fn set_velocity(&self, output_rpm: f32) {
+        self.inner.set_velocity(self.to_rotor(output_rpm));
+    }
+}
+
+impl<T: EffortDrive> EffortDrive for OutputShaft<T> {
+    fn set_effort_raw(&self, raw: i16) {
+        self.inner.set_effort_raw(raw);
+    }
+}
+
+impl<T: PositionDrive> PositionDrive for OutputShaft<T> {
+    /// `output_rev` 为**输出轴**圈数。
+    fn set_position(&self, output_rev: f32) {
+        self.inner.set_position(self.to_rotor(output_rev));
+    }
+}
+
+impl<T: Disable> Disable for OutputShaft<T> {
+    fn disable(&self) {
+        self.inner.disable();
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::Command;
+
+    struct Stub {
+        cmd: std::sync::Mutex<Command>,
+    }
+
+    impl Stub {
+        fn new() -> Self {
+            Self { cmd: std::sync::Mutex::new(Command::Disabled) }
+        }
+    }
+
+    impl MotorFeedback for Stub {
+        fn id(&self) -> u8 {
+            1
+        }
+        fn state(&self) -> MotorState {
+            MotorState::default()
+        }
+    }
+    impl VelocityDrive for Stub {
+        fn set_velocity(&self, rotor_rpm: f32) {
+            *self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm);
+        }
+    }
+    impl EffortDrive for Stub {
+        fn set_effort_raw(&self, raw: i16) {
+            *self.cmd.lock().unwrap() = Command::EffortRaw(raw);
+        }
+    }
+    impl PositionDrive for Stub {
+        fn set_position(&self, rotor_rev: f32) {
+            *self.cmd.lock().unwrap() = Command::Position(rotor_rev);
+        }
+    }
+    impl Disable for Stub {
+        fn disable(&self) {
+            *self.cmd.lock().unwrap() = Command::Disabled;
+        }
+    }
+
+    #[test]
+    fn velocity_scales_by_gear() {
+        let shaft = OutputShaft::new(Stub::new(), 19.203);
+        shaft.set_velocity(100.0);
+        match *shaft.inner().cmd.lock().unwrap() {
+            Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"),
+            ref c => panic!("unexpected {c:?}"),
+        }
+    }
+
+    #[test]
+    fn position_scales_by_gear() {
+        let shaft = OutputShaft::new(Stub::new(), 36.0);
+        shaft.set_position(2.0);
+        match *shaft.inner().cmd.lock().unwrap() {
+            Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"),
+            ref c => panic!("unexpected {c:?}"),
+        }
+    }
+
+    #[test]
+    fn effort_passes_through() {
+        let shaft = OutputShaft::new(Stub::new(), 10.0);
+        shaft.set_effort_raw(-1234);
+        assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234));
+    }
+
+    #[test]
+    #[should_panic = "gear_ratio"]
+    fn rejects_nonpositive_gear() {
+        let _ = OutputShaft::new(Stub::new(), 0.0);
+    }
+}

crates/motor-core/src/heading.rs

@@ -0,0 +1,21 @@
+//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。
+//!
+//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。
+//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。
+
+use std::future::Future;
+
+/// 绝对航向观测源。
+pub trait HeadingSource: Send {
+    /// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。
+    fn heading_rad(&self) -> impl Future<Output = f32> + Send;
+}
+
+/// 云台目标角的参考系。
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum HeadingFrame {
+    /// 电机转子多圈（上电相对）。
+    RotorMultiTurn,
+    /// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。
+    AbsoluteField,
+}

crates/motor-core/src/lib.rs

@@ -0,0 +1,93 @@
+//! 协议无关的电机抽象层。
+//!
+//! 本 crate 不做任何 I/O：它定义电机能力 trait（[`MotorFeedback`] /
+//! [`EffortDrive`] / [`VelocityDrive`] / [`PositionDrive`] / [`Disable`]）、
+//! 公共数据类型（[`MotorState`]、[`Command`]）以及纯控制算法（[`Pid`]、
+//! [`VelocityController`]、[`PositionController`]）。具体总线协议在其上实现。
+//!
+//! # 单一职责
+//!
+//! - [`traits`]：能力面（小而正交）；
+//! - [`state`] / [`command`]：反馈快照与命令寄存器；
+//! - [`pid`] / [`velocity`] / [`position`]：纯控制律；
+//! - [`gearbox`]：减速比单位适配（输出轴 ↔ 转子）；
+//! - [`heading`]：航向观测 seam（云台绝对角融合用）；
+//! - [`testing`]：测试替身。
+//!
+//! # 单位与方向约定
+//!
+//! - 所有呈现给上层的量都是**安装方向修正后**的物理量：`direction = -1`
+//!   的电机由驱动层在读写两个方向上统一翻转符号。
+//! - 转速一律为电机**转子**转速（rpm）；输出轴换算见 [`OutputShaft`]。
+//! - 开环控制量以协议原始值（raw）为主（C620 电流：±16384 ↔ ±20 A；
+//!   GM6020 电压：±30000 ≈ 满电压）。
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+pub mod command;
+pub mod gearbox;
+pub mod heading;
+pub mod pid;
+pub mod position;
+pub mod state;
+pub mod testing;
+pub mod traits;
+pub mod velocity;
+
+pub use command::{Command, CommandMode};
+pub use gearbox::OutputShaft;
+pub use heading::{HeadingFrame, HeadingSource};
+pub use pid::Pid;
+pub use position::{PositionController, PositionLoop};
+pub use state::MotorState;
+pub use testing::{MockHeading, MockMotor};
+pub use traits::{Disable, EffortDrive, Motor, MotorFeedback, PositionDrive, VelocityDrive};
+pub use velocity::VelocityController;
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn default_state_has_no_feedback() {
+        let state = MotorState::default();
+        assert!(!state.has_feedback());
+        assert_eq!(state.feedback_count, 0);
+    }
+
+    #[test]
+    fn feedback_gate_flips_on_first_frame() {
+        let state = MotorState { feedback_count: 1, ..MotorState::default() };
+        assert!(state.has_feedback());
+    }
+
+    #[test]
+    fn command_variants_roundtrip_by_value() {
+        let cmds = [
+            Command::Disabled,
+            Command::EffortRaw(-16384),
+            Command::Velocity(4820.5),
+            Command::Position(2.5),
+        ];
+        assert_eq!(cmds[0], Command::Disabled);
+        assert_eq!(cmds[1], Command::EffortRaw(-16384));
+        assert_eq!(cmds[2], Command::Velocity(4820.5));
+        assert_eq!(cmds[3], Command::Position(2.5));
+    }
+
+    #[test]
+    fn command_mode_labels() {
+        assert_eq!(Command::Disabled.mode(), CommandMode::Disabled);
+        assert_eq!(Command::EffortRaw(0).mode(), CommandMode::EffortRaw);
+        assert_eq!(Command::Velocity(0.0).mode(), CommandMode::Velocity);
+        assert_eq!(Command::Position(0.0).mode(), CommandMode::Position);
+    }
+
+    #[test]
+    fn mock_motor_is_full_motor() {
+        fn takes_motor(m: &impl Motor) {
+            m.set_velocity(1.0);
+            m.disable();
+        }
+        takes_motor(&MockMotor::new(1));
+    }
+}

crates/motor-core/src/pid.rs

@@ -0,0 +1,264 @@
+//! 通用 PID 控制器。
+
+/// 并联式 PID 控制器（f32，显式时间步）。
+///
+/// 特性：
+/// - 输出限幅 `[out_min, out_max]`；
+/// - 积分抗饱和（clamping：输出已饱和且误差继续同向时暂停积分）；
+/// - 微分作用于**测量值**（避免设定值阶跃的微分冲击），可选一阶低通滤波。
+///
+/// 时间步 `dt` 由调用方显式传入（秒）。`dt <= 0` 或非有限值时本次调用
+/// 直接返回上次输出且不更新内部状态（防御跳帧 / 首帧场景）。
+#[derive(Debug, Clone)]
+pub struct Pid {
+    kp: f32,
+    ki: f32,
+    kd: f32,
+    out_min: f32,
+    out_max: f32,
+    /// 微分低通时间常数（秒）；`0` 关闭滤波。
+    derivative_tau: f32,
+    integral: f32,
+    prev_measurement: Option<f32>,
+    derivative_filtered: f32,
+    last_output: f32,
+}
+
+impl Pid {
+    /// 创建 PID。`out_min` 必须小于 `out_max`。
+    ///
+    /// # Panics
+    ///
+    /// 参数非有限或 `out_min >= out_max` 时 panic。
+    #[must_use]
+    pub fn new(kp: f32, ki: f32, kd: f32, out_min: f32, out_max: f32) -> Self {
+        assert!(
+            out_min < out_max && [kp, ki, kd, out_min, out_max].iter().all(|v| v.is_finite()),
+            "invalid PID parameters"
+        );
+        Self {
+            kp,
+            ki,
+            kd,
+            out_min,
+            out_max,
+            derivative_tau: 0.0,
+            integral: 0.0,
+            prev_measurement: None,
+            derivative_filtered: 0.0,
+            last_output: 0.0,
+        }
+    }
+
+    /// 设置微分低通时间常数（秒），`0` 关闭滤波。链式调用。
+    #[must_use]
+    pub fn with_derivative_tau(mut self, tau: f32) -> Self {
+        self.derivative_tau = tau.max(0.0);
+        self
+    }
+
+    /// 单步更新，返回限幅后的输出。
+    pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 {
+        // NaN 与 +inf 的比较结果均为 false，故 `dt > 0` 已隐含有限性。
+        let input_valid = dt > 0.0 && setpoint.is_finite() && measurement.is_finite();
+        if !input_valid {
+            return self.last_output;
+        }
+        let error = setpoint - measurement;
+
+        // 微分作用于测量值（反向），一阶低通滤波。
+        let d_raw = match self.prev_measurement {
+            Some(prev) => -(measurement - prev) / dt,
+            None => 0.0,
+        };
+        if self.derivative_tau > 0.0 {
+            let alpha = dt / (self.derivative_tau + dt);
+            self.derivative_filtered += alpha * (d_raw - self.derivative_filtered);
+        } else {
+            self.derivative_filtered = d_raw;
+        }
+        let d_term = self.kd * self.derivative_filtered;
+
+        // 积分抗饱和（clamping）：仅在不会把输出进一步推入饱和时积分。
+        let i_candidate = self.integral + self.ki * error * dt;
+        let unsat = self.kp * error + i_candidate + d_term;
+        let saturating_up = unsat > self.out_max && error > 0.0;
+        let saturating_down = unsat < self.out_min && error < 0.0;
+        if !saturating_up && !saturating_down {
+            self.integral = i_candidate;
+        }
+
+        let output = (self.kp * error + self.integral + d_term).clamp(self.out_min, self.out_max);
+        self.prev_measurement = Some(measurement);
+        self.last_output = output;
+        output
+    }
+
+    /// 清空积分、微分记忆与上次输出。
+    pub fn reset(&mut self) {
+        self.integral = 0.0;
+        self.prev_measurement = None;
+        self.derivative_filtered = 0.0;
+        self.last_output = 0.0;
+    }
+
+    /// 当前积分累加项。
+    #[must_use]
+    pub fn integral(&self) -> f32 {
+        self.integral
+    }
+
+    /// 比例增益。
+    #[must_use]
+    pub fn kp(&self) -> f32 {
+        self.kp
+    }
+
+    /// 积分增益。
+    #[must_use]
+    pub fn ki(&self) -> f32 {
+        self.ki
+    }
+
+    /// 微分增益。
+    #[must_use]
+    pub fn kd(&self) -> f32 {
+        self.kd
+    }
+
+    /// 输出下限。
+    #[must_use]
+    pub fn output_min(&self) -> f32 {
+        self.out_min
+    }
+
+    /// 输出上限。
+    #[must_use]
+    pub fn output_max(&self) -> f32 {
+        self.out_max
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    const DT: f32 = 0.001;
+
+    #[test]
+    fn pure_proportional() {
+        let mut pid = Pid::new(2.0, 0.0, 0.0, -100.0, 100.0);
+        let out = pid.update(10.0, 4.0, DT);
+        assert!((out - 12.0).abs() < 1e-4, "out = {out}");
+    }
+
+    #[test]
+    fn output_clamps_both_ends() {
+        let mut pid = Pid::new(100.0, 0.0, 0.0, -10.0, 10.0);
+        assert!((pid.update(1.0, 0.0, DT) - 10.0).abs() < 1e-5);
+        assert!((pid.update(-1.0, 0.0, DT) - (-10.0)).abs() < 1e-5);
+    }
+
+    #[test]
+    fn integral_accumulates() {
+        let mut pid = Pid::new(0.0, 10.0, 0.0, -1e6, 1e6);
+        pid.update(1.0, 0.0, DT); // integral += 10 * 1 * 0.001
+        pid.update(1.0, 0.0, DT);
+        let out = pid.update(1.0, 0.0, DT);
+        let expected = 10.0 * 1.0 * DT * 3.0;
+        assert!((out - expected).abs() < 1e-4, "out = {out}, expected {expected}");
+    }
+
+    #[test]
+    fn anti_windup_bounds_integral_when_saturated() {
+        // 大误差持续饱和 200 步：kp = 0 时输出即积分项。clamping 抗饱和要求
+        // 积分不超过 out_max，且进入保持区后输出不再增长。
+        let ki = 5.0;
+        let mut pid = Pid::new(0.0, ki, 0.0, -16384.0, 16384.0);
+        for _ in 0..200 {
+            pid.update(1e6, 0.0, DT);
+        }
+        assert!(pid.integral() <= 16384.0, "integral = {}", pid.integral());
+        let frozen = pid.update(1e6, 0.0, DT);
+        let still = pid.update(1e6, 0.0, DT);
+        assert!(
+            (frozen - still).abs() < 1e-3,
+            "saturated output must stop growing: {frozen} vs {still}"
+        );
+    }
+
+    #[test]
+    fn anti_windup_recovers_quickly_on_reverse() {
+        // 饱和后目标反向，输出应在一个反向误差步内脱离正饱和。
+        let mut pid = Pid::new(1.0, 10.0, 0.0, -100.0, 100.0);
+        for _ in 0..500 {
+            pid.update(1000.0, 0.0, DT);
+        }
+        assert!((pid.update(1000.0, 0.0, DT) - 100.0).abs() < 1e-3);
+        // 一步反向大误差（kp=1, e=-1000）足以击穿下限，说明无深度饱和。
+        let out = pid.update(-1000.0, 0.0, DT);
+        assert!(out <= 0.0, "expected non-positive output, got {out}");
+    }
+
+    #[test]
+    fn derivative_acts_on_measurement() {
+        // 设定值恒 0，测量值阶跃：输出应为负向微分冲击（-kd*Δ/dt）。
+        let mut pid = Pid::new(0.0, 0.0, 0.01, -1e6, 1e6);
+        pid.update(0.0, 0.0, DT); // 建立初值，首帧微分为 0
+        let out = pid.update(0.0, 0.5, DT);
+        let expected = -0.01 * (0.5 / DT);
+        assert!((out - expected).abs() < 1.0, "out = {out}, expected {expected}");
+    }
+
+    #[test]
+    fn derivative_filter_softens_spike() {
+        let step = |tau: f32| {
+            let mut pid = Pid::new(0.0, 0.0, 10.0, -1e9, 1e9).with_derivative_tau(tau);
+            pid.update(0.0, 0.0, DT);
+            pid.update(0.0, 1.0, DT).abs()
+        };
+        let unfiltered = step(0.0);
+        let filtered = step(0.05);
+        assert!(
+            filtered < unfiltered * 0.05,
+            "filtered {filtered} should be far below unfiltered {unfiltered}"
+        );
+    }
+
+    #[test]
+    fn nonpositive_dt_returns_last_output_and_keeps_state() {
+        let mut pid = Pid::new(1.0, 1.0, 0.0, -100.0, 100.0);
+        let first = pid.update(10.0, 0.0, DT);
+        let integral_before = pid.integral().to_bits();
+        // 逐位相等：无效输入必须原样返回上次输出。
+        assert_eq!(pid.update(99.0, 0.0, 0.0).to_bits(), first.to_bits());
+        assert_eq!(pid.update(99.0, 0.0, f32::NAN).to_bits(), first.to_bits());
+        assert_eq!(pid.integral().to_bits(), integral_before);
+    }
+
+    #[test]
+    fn nan_input_returns_last_output() {
+        let mut pid = Pid::new(1.0, 0.0, 0.0, -100.0, 100.0);
+        let first = pid.update(1.0, 0.0, DT);
+        assert_eq!(pid.update(f32::NAN, 0.0, DT).to_bits(), first.to_bits());
+        assert_eq!(pid.update(1.0, f32::INFINITY, DT).to_bits(), first.to_bits());
+    }
+
+    #[test]
+    fn reset_clears_history() {
+        let mut pid = Pid::new(1.0, 10.0, 1.0, -1e6, 1e6);
+        pid.update(5.0, 1.0, DT);
+        pid.update(5.0, 1.0, DT);
+        pid.reset();
+        assert_eq!(pid.integral().to_bits(), 0.0f32.to_bits());
+        // 重置后首帧微分为 0，输出 = P 项 + 本步积分（ki*e*dt = 0.04）。
+        let out = pid.update(5.0, 1.0, DT);
+        assert!((out - 4.04).abs() < 1e-4, "out = {out}");
+    }
+
+    #[test]
+    #[should_panic = "invalid PID parameters"]
+    fn new_rejects_inverted_limits() {
+        let _pid = Pid::new(1.0, 0.0, 0.0, 1.0, -1.0);
+    }
+}

crates/motor-core/src/position.rs

@@ -0,0 +1,103 @@
+//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。
+
+use crate::pid::Pid;
+use crate::velocity::VelocityController;
+
+/// 位置环拓扑。
+#[derive(Debug, Clone)]
+pub enum PositionLoop {
+    /// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。
+    Direct(Pid),
+    /// 级联：位置 → 目标转速 → 速度环 → effort_raw。
+    Cascade {
+        /// 外环位置 PID（输出限幅 = 最大 rpm）。
+        pos: Pid,
+        /// 内环速度控制器（输出限幅 = 最大 effort）。
+        vel: VelocityController,
+    },
+}
+
+/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。
+#[derive(Debug, Clone)]
+pub struct PositionController {
+    inner: PositionLoop,
+}
+
+impl PositionController {
+    /// 单环位置控制器。
+    #[must_use]
+    pub fn direct(pid: Pid) -> Self {
+        Self { inner: PositionLoop::Direct(pid) }
+    }
+
+    /// 级联位置控制器（限速）。
+    #[must_use]
+    pub fn cascade(pos: Pid, vel: VelocityController) -> Self {
+        Self { inner: PositionLoop::Cascade { pos, vel } }
+    }
+
+    /// 单步更新，返回 effort_raw（已含限幅）。
+    ///
+    /// `measured_rpm` 仅级联内环使用；单环忽略该参数。
+    pub fn update(
+        &mut self,
+        target_rev: f32,
+        measured_rev: f32,
+        measured_rpm: f32,
+        dt: f32,
+    ) -> f32 {
+        match &mut self.inner {
+            PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt),
+            PositionLoop::Cascade { pos, vel } => {
+                let vel_target = pos.update(target_rev, measured_rev, dt);
+                vel.update(vel_target, measured_rpm, dt)
+            }
+        }
+    }
+
+    /// 清空内部状态（切换模式 / 重新使能时调用）。
+    pub fn reset(&mut self) {
+        match &mut self.inner {
+            PositionLoop::Direct(pid) => pid.reset(),
+            PositionLoop::Cascade { pos, vel } => {
+                pos.reset();
+                vel.reset();
+            }
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    const DT: f32 = 0.001;
+
+    #[test]
+    fn direct_outputs_pid() {
+        let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0));
+        let out = pc.update(1.0, 0.5, 0.0, DT);
+        assert!((out - 1.0).abs() < 1e-4, "out = {out}");
+    }
+
+    #[test]
+    fn cascade_limits_velocity_then_effort() {
+        // 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。
+        let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0);
+        let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0);
+        let mut pc = PositionController::cascade(pos, vel);
+        let out = pc.update(1.0, 0.0, 0.0, DT);
+        // 外环饱和到 5 rpm，内环 ff=2 → 10 raw。
+        assert!((out - 10.0).abs() < 1e-3, "out = {out}");
+    }
+
+    #[test]
+    fn reset_clears_state() {
+        let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6));
+        pc.update(1.0, 0.0, 0.0, DT);
+        pc.reset();
+        let out = pc.update(1.0, 0.0, 0.0, DT);
+        // 重置后只剩本步积分：10 * 1 * 0.001。
+        assert!((out - 0.01).abs() < 1e-5, "out = {out}");
+    }
+}

crates/motor-core/src/state.rs

@@ -0,0 +1,29 @@
+//! 电机反馈快照（协议无关，方向修正后的物理量）。
+//!
+//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`]
+//! 或上层完成，不在此泄漏底盘语义。
+
+/// 电机反馈快照。
+#[derive(Debug, Clone, Copy, PartialEq, Default)]
+pub struct MotorState {
+    /// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。
+    pub rotor_angle_raw: u16,
+    /// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。
+    pub rotor_revolutions: f32,
+    /// 转子转速（rpm，物理方向）。
+    pub rotor_speed_rpm: f32,
+    /// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。
+    pub effort_raw: i16,
+    /// 电机温度（℃）。
+    pub temperature_c: i8,
+    /// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。
+    pub feedback_count: u64,
+}
+
+impl MotorState {
+    /// 是否已收到过反馈（上电后首帧门控）。
+    #[must_use]
+    pub fn has_feedback(&self) -> bool {
+        self.feedback_count > 0
+    }
+}

crates/motor-core/src/testing.rs

@@ -0,0 +1,116 @@
+//! 测试替身：内存电机与航向源。
+
+use std::future::Future;
+use std::sync::{Arc, Mutex, MutexGuard};
+
+use crate::{
+    Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
+    VelocityDrive,
+};
+
+/// 锁辅助：中毒锁直接恢复内部数据。
+fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
+    mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
+}
+
+/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。
+///
+/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。
+#[derive(Clone)]
+pub struct MockMotor {
+    id: u8,
+    published: Arc<Mutex<(MotorState, Command)>>,
+}
+
+impl MockMotor {
+    /// 创建指定 ID 的空电机。
+    #[must_use]
+    pub fn new(id: u8) -> Self {
+        Self { id, published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))) }
+    }
+
+    /// 注入反馈快照。
+    pub fn inject_state(&self, state: MotorState) {
+        lock(&self.published).0 = state;
+    }
+
+    /// 最近一次命令。
+    #[must_use]
+    pub fn last_command(&self) -> Command {
+        lock(&self.published).1
+    }
+}
+
+impl MotorFeedback for MockMotor {
+    fn id(&self) -> u8 {
+        self.id
+    }
+    fn state(&self) -> MotorState {
+        lock(&self.published).0
+    }
+}
+
+impl VelocityDrive for MockMotor {
+    fn set_velocity(&self, rotor_rpm: f32) {
+        lock(&self.published).1 = Command::Velocity(rotor_rpm);
+    }
+}
+
+impl EffortDrive for MockMotor {
+    fn set_effort_raw(&self, raw: i16) {
+        lock(&self.published).1 = Command::EffortRaw(raw);
+    }
+}
+
+impl PositionDrive for MockMotor {
+    fn set_position(&self, rotor_rev: f32) {
+        lock(&self.published).1 = Command::Position(rotor_rev);
+    }
+}
+
+impl Disable for MockMotor {
+    fn disable(&self) {
+        lock(&self.published).1 = Command::Disabled;
+    }
+}
+
+/// 恒定航向测试源。
+pub struct MockHeading {
+    rad: f32,
+}
+
+impl MockHeading {
+    /// 创建固定航向源。
+    #[must_use]
+    pub const fn new(rad: f32) -> Self {
+        Self { rad }
+    }
+}
+
+impl HeadingSource for MockHeading {
+    fn heading_rad(&self) -> impl Future<Output = f32> + Send {
+        std::future::ready(self.rad)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn mock_motor_records_commands() {
+        let m = MockMotor::new(3);
+        m.set_velocity(1000.0);
+        assert_eq!(m.last_command(), Command::Velocity(1000.0));
+        m.disable();
+        assert_eq!(m.last_command(), Command::Disabled);
+    }
+
+    #[test]
+    fn mock_motor_injects_state() {
+        let m = MockMotor::new(1);
+        assert!(!m.state().has_feedback());
+        m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
+        assert!(m.state().has_feedback());
+    }
+}

crates/motor-core/src/traits.rs

@@ -0,0 +1,47 @@
+//! 能力面 trait：小而正交，按硬件事实拆分。
+//!
+//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）：
+//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`]
+//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]）
+//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
+//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
+
+use crate::MotorState;
+
+/// 读反馈。所有可观察设备的最小面。
+pub trait MotorFeedback: Send + Sync {
+    /// 控制器内的电机 ID（对 DJI 电调即电调 ID）。
+    fn id(&self) -> u8;
+
+    /// 最新反馈快照。
+    fn state(&self) -> MotorState;
+}
+
+/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。
+pub trait EffortDrive: Send + Sync {
+    /// 直接下发开环 raw（协议量程，物理方向）。
+    fn set_effort_raw(&self, raw: i16);
+}
+
+/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。
+pub trait VelocityDrive: Send + Sync {
+    /// 设定目标转子转速。
+    fn set_velocity(&self, rotor_rpm: f32);
+}
+
+/// 位置目标（转子多圈，圈；物理方向）。
+pub trait PositionDrive: Send + Sync {
+    /// 设定目标转子多圈位置。
+    fn set_position(&self, rotor_rev: f32);
+}
+
+/// 停用（零输出）。
+pub trait Disable: Send + Sync {
+    /// 停用电机。
+    fn disable(&self);
+}
+
+/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。
+pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
+
+impl<T> Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {}

crates/motor-core/src/velocity.rs

@@ -0,0 +1,87 @@
+//! 速度环控制器：PID + 速度前馈。
+
+use crate::pid::Pid;
+
+/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
+///
+/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
+/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。
+/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量
+/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。
+///
+/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。
+#[derive(Debug, Clone)]
+pub struct VelocityController {
+    pid: Pid,
+    feedforward: f32,
+}
+
+impl VelocityController {
+    /// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。
+    #[must_use]
+    pub fn new(pid: Pid, feedforward: f32) -> Self {
+        Self { pid, feedforward }
+    }
+
+    /// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
+    pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
+        let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
+        (pid_out + self.feedforward * target_rpm).clamp(
+            // 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。
+            self.pid.output_min(),
+            self.pid.output_max(),
+        )
+    }
+
+    /// 清空 PID 内部状态（切换模式 / 重新使能时调用）。
+    pub fn reset(&mut self) {
+        self.pid.reset();
+    }
+
+    /// 前馈系数（raw / rpm）。
+    #[must_use]
+    pub fn feedforward(&self) -> f32 {
+        self.feedforward
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn pure_feedforward_tracks_target() {
+        let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0);
+        assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3);
+        // 前馈 -18000 被限幅到 -16384。
+        assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3);
+    }
+
+    #[test]
+    fn feedforward_plus_pid_respects_limit() {
+        let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0);
+        // 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。
+        let out = vc.update(1000.0, 0.0, 0.001);
+        assert!((out - 12000.0).abs() < 1e-3, "out = {out}");
+    }
+
+    #[test]
+    fn zero_error_yields_pure_feedforward() {
+        // 误差为 0 时 PID 贡献为 0，输出只剩前馈。
+        let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0);
+        let out = vc.update(1000.0, 1000.0, 0.001);
+        assert!((out - 9000.0).abs() < 1e-4, "out = {out}");
+    }
+
+    #[test]
+    fn reset_clears_pid_state() {
+        let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0);
+        for _ in 0..10 {
+            vc.update(100.0, 0.0, 0.001);
+        }
+        vc.reset();
+        // 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。
+        let out = vc.update(100.0, 0.0, 0.001);
+        assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
+    }
+}