feat: 初始化

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