feat: 初始化

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