feat: 添加了自动超参数整定

crates/motor-core/src/gearbox.rs

@@ -3,6 +3,8 @@
 //! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
 //! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
 
+use std::time::Duration;
+
 use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
 
 /// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
@@ -66,6 +68,10 @@ impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
         // 只透传转子量；不伪造输出轴字段。
         self.inner.state()
     }
+
+    fn feedback_age(&self) -> Option<Duration> {
+        self.inner.feedback_age()
+    }
 }
 
 impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
@@ -116,6 +122,9 @@ mod tests {
         fn state(&self) -> MotorState {
             MotorState::default()
         }
+        fn feedback_age(&self) -> Option<Duration> {
+            Some(Duration::ZERO)
+        }
     }
     impl VelocityDrive for Stub {
         fn set_velocity(&self, rotor_rpm: f32) {

crates/motor-core/src/lib.rs

@@ -41,7 +41,7 @@ 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;
+pub use velocity::{VelocityController, VelocityGains};
 
 #[cfg(test)]
 mod tests {

crates/motor-core/src/testing.rs

@@ -2,6 +2,7 @@
 
 use std::future::Future;
 use std::sync::{Arc, Mutex, MutexGuard};
+use std::time::{Duration, Instant};
 
 use crate::{
     Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
@@ -20,18 +21,30 @@ fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
 pub struct MockMotor {
     id: u8,
     published: Arc<Mutex<(MotorState, Command)>>,
+    last_feedback: Arc<Mutex<Option<Instant>>>,
 }
 
 impl MockMotor {
     /// 创建指定 ID 的空电机。
     #[must_use]
     pub fn new(id: u8) -> Self {
-        Self { id, published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))) }
+        Self {
+            id,
+            published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))),
+            last_feedback: Arc::new(Mutex::new(None)),
+        }
     }
 
-    /// 注入反馈快照。
+    /// 注入反馈快照（记为「刚刚收到」）。
     pub fn inject_state(&self, state: MotorState) {
+        self.inject_state_with_age(state, Duration::ZERO);
+    }
+
+    /// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。
+    pub fn inject_state_with_age(&self, state: MotorState, age: Duration) {
         lock(&self.published).0 = state;
+        let at = Instant::now().checked_sub(age);
+        *lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now));
     }
 
     /// 最近一次命令。
@@ -48,6 +61,10 @@ impl MotorFeedback for MockMotor {
     fn state(&self) -> MotorState {
         lock(&self.published).0
     }
+    fn feedback_age(&self) -> Option<Duration> {
+        let at = *lock(&self.last_feedback);
+        Some(at?.elapsed())
+    }
 }
 
 impl VelocityDrive for MockMotor {
@@ -110,7 +127,19 @@ mod tests {
     fn mock_motor_injects_state() {
         let m = MockMotor::new(1);
         assert!(!m.state().has_feedback());
+        assert!(m.feedback_age().is_none());
         m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
         assert!(m.state().has_feedback());
+        assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50)));
+    }
+
+    #[test]
+    fn mock_motor_feedback_age_can_be_stale() {
+        let m = MockMotor::new(1);
+        m.inject_state_with_age(
+            MotorState { feedback_count: 1, ..MotorState::default() },
+            Duration::from_millis(250),
+        );
+        assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200)));
     }
 }

crates/motor-core/src/traits.rs

@@ -6,6 +6,8 @@
 //! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
 //! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
 
+use std::time::Duration;
+
 use crate::MotorState;
 
 /// 读反馈。所有可观察设备的最小面。
@@ -15,6 +17,12 @@ pub trait MotorFeedback: Send + Sync {
 
     /// 最新反馈快照。
     fn state(&self) -> MotorState;
+
+    /// 距最近一帧反馈的时长；`None` 表示从未收到。
+    ///
+    /// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`]
+    /// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。
+    fn feedback_age(&self) -> Option<Duration>;
 }
 
 /// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。

crates/motor-core/src/velocity.rs

@@ -2,6 +2,43 @@
 
 use crate::pid::Pid;
 
+/// 速度环增益组（可整定参数）。
+///
+/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
+/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
+/// 传递、落盘与热更新，而无需重建整个速度环。
+#[derive(Debug, Clone, Copy, PartialEq)]
+pub struct VelocityGains {
+    /// 比例增益（raw / rpm）。
+    pub kp: f32,
+    /// 积分增益（raw / (rpm·s)）。
+    pub ki: f32,
+    /// 微分增益（raw / (rpm/s)）。
+    pub kd: f32,
+    /// 速度前馈（raw / rpm）。
+    pub feedforward: f32,
+}
+
+impl VelocityGains {
+    /// 全零增益。
+    pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
+
+    /// 由四个分量构造。
+    #[must_use]
+    pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
+        Self { kp, ki, kd, feedforward }
+    }
+
+    /// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
+    #[must_use]
+    pub fn is_finite(&self) -> bool {
+        self.kp.is_finite()
+            && self.ki.is_finite()
+            && self.kd.is_finite()
+            && self.feedforward.is_finite()
+    }
+}
+
 /// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
 ///
 /// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
@@ -23,6 +60,29 @@ impl VelocityController {
         Self { pid, feedforward }
     }
 
+    /// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
+    ///
+    /// # Panics
+    ///
+    /// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
+    #[must_use]
+    pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
+        let limit = f32::from(output_limit_raw);
+        let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
+        Self { pid, feedforward: gains.feedforward }
+    }
+
+    /// 当前增益组（含前馈），用于诊断与热更新前的读取。
+    #[must_use]
+    pub fn gains(&self) -> VelocityGains {
+        VelocityGains {
+            kp: self.pid.kp(),
+            ki: self.pid.ki(),
+            kd: self.pid.kd(),
+            feedforward: self.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);
@@ -84,4 +144,37 @@ mod tests {
         let out = vc.update(100.0, 0.0, 0.001);
         assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
     }
+
+    #[test]
+    fn from_gains_round_trips_through_gains() {
+        let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
+        let mut from_data = VelocityController::from_gains(12_000, gains);
+        let mut by_hand =
+            VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
+
+        assert_eq!(from_data.gains(), gains);
+        // 两条构造路径必须逐位一致。
+        for step in 0..16 {
+            let target = 1000.0 + step as f32;
+            let measured = 900.0 + step as f32 * 3.0;
+            assert_eq!(
+                from_data.update(target, measured, 0.001).to_bits(),
+                by_hand.update(target, measured, 0.001).to_bits()
+            );
+        }
+    }
+
+    #[test]
+    fn from_gains_clamps_to_output_limit() {
+        let mut vc = VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
+        assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
+    }
+
+    #[test]
+    fn gains_finiteness_is_checkable() {
+        assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
+        assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
+        assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
+        assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
+    }
 }