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

crates/chassis-tune/src/bench.rs

@@ -0,0 +1,383 @@
+//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
+//!
+//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]，
+//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
+//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
+//!
+//! # 安全设计
+//!
+//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
+//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
+//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
+//!
+//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
+//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。
+
+use std::sync::Mutex;
+use std::time::Duration;
+
+use anyhow::Context as _;
+use chassis_bench::{Abort, SafetyEnvelope, run_step_trial};
+use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
+use dji_esc_sim::{LoopLimits, Scenario, Trace};
+use motor_core::{Disable as _, MotorFeedback, VelocityGains};
+use thiserror::Error;
+
+use crate::backend::Evaluator;
+use crate::cost::{Objective, Score, Verdict};
+
+/// 真机后端错误。
+#[derive(Debug, Error)]
+pub enum BenchError {
+    /// 无法打开总线。
+    #[error("cannot open CAN link: {0}")]
+    Link(String),
+    /// 电调总线启动失败。
+    #[error("cannot start ESC bus: {0}")]
+    Bus(String),
+    /// 启动超时：目标电调没有反馈。
+    #[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")]
+    NoFeedback {
+        /// 目标电调 ID。
+        esc_id: u8,
+        /// 等待时长（毫秒）。
+        waited_ms: u64,
+    },
+    /// 搜索期间因安全事件中止且未要求继续。
+    #[error("aborted by safety envelope: {reason}")]
+    Aborted {
+        /// 触发原因。
+        reason: String,
+    },
+}
+
+/// 真机评估配置。
+#[derive(Debug, Clone)]
+pub struct BenchConfig {
+    /// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
+    pub esc: EscConfig,
+    /// 总线运行参数。
+    pub bus: BusOptions,
+    /// 评估场景。
+    pub scenario: Scenario,
+    /// 安全包络。
+    pub envelope: SafetyEnvelope,
+    /// 硬约束与权重。
+    pub objective: Objective,
+    /// 试验之间的静默期（等轮子停稳）。
+    pub between_trials: Duration,
+    /// 命令租约续期节拍。
+    pub lease_period: Duration,
+    /// 启动时等待反馈的超时。
+    pub startup_timeout: Duration,
+    /// 安全检查触发后是否继续搜索（默认 false = 立即停）。
+    pub continue_on_abort: bool,
+}
+
+impl BenchConfig {
+    /// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
+    #[must_use]
+    pub fn new(
+        esc: EscConfig,
+        scenario: Scenario,
+        envelope: SafetyEnvelope,
+        objective: Objective,
+    ) -> Self {
+        Self {
+            esc,
+            bus: BusOptions::default(),
+            scenario,
+            envelope,
+            objective,
+            between_trials: Duration::from_millis(500),
+            lease_period: Duration::from_millis(5),
+            startup_timeout: Duration::from_secs(5),
+            continue_on_abort: false,
+        }
+    }
+}
+
+/// 真机评估器（持有总线与运行时）。
+pub struct BenchEvaluator {
+    session: Mutex<Session>,
+    scenario: Scenario,
+    envelope: SafetyEnvelope,
+    objective: Objective,
+    effort_limit_raw: i16,
+    between_trials: Duration,
+    lease_period: Duration,
+    continue_on_abort: bool,
+    /// 目前见过的最优轨迹（报告用）。
+    best: Mutex<Option<(f64, Trace)>>,
+    /// 中止记录（报告用）。
+    aborts: Mutex<Vec<String>>,
+    describe: String,
+}
+
+struct Session {
+    runtime: tokio::runtime::Runtime,
+    bus: Option<EscBusHandle>,
+    motor: EscMotor,
+}
+
+impl BenchEvaluator {
+    /// 打开链路、启动总线并等待目标电调反馈。
+    ///
+    /// # Errors
+    ///
+    /// 链路 / 总线启动失败，或启动超时（电调无反馈）。
+    pub fn open(link: can_link::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
+        let esc_id = config.esc.esc_id;
+        let limits =
+            LoopLimits { current_limit_raw: config.esc.current_limit, model: config.esc.model };
+        let runtime = tokio::runtime::Builder::new_multi_thread()
+            .worker_threads(2)
+            .enable_all()
+            .build()
+            .map_err(|e| BenchError::Bus(e.to_string()))?;
+
+        let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone())
+            .map_err(|e| BenchError::Bus(e.to_string()))?;
+        let motor = bus
+            .motor(esc_id)
+            .ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))?
+            .clone();
+
+        let startup_timeout = config.startup_timeout;
+        let ready = runtime.block_on(async {
+            let deadline = tokio::time::Instant::now() + startup_timeout;
+            loop {
+                if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) {
+                    return true;
+                }
+                if tokio::time::Instant::now() >= deadline {
+                    return false;
+                }
+                tokio::time::sleep(Duration::from_millis(20)).await;
+            }
+        });
+        let evaluator = Self {
+            session: Mutex::new(Session { runtime, bus: Some(bus), motor }),
+            scenario: config.scenario,
+            envelope: config.envelope,
+            objective: config.objective,
+            effort_limit_raw: limits.current_limit_raw,
+            between_trials: config.between_trials,
+            lease_period: config.lease_period,
+            continue_on_abort: config.continue_on_abort,
+            best: Mutex::new(None),
+            aborts: Mutex::new(Vec::new()),
+            describe: format!(
+                "bench(esc={esc_id}, name={}, limit={} raw, target={} rpm, trial={:.1}s, gap={}ms)",
+                config.esc.name,
+                limits.current_limit_raw,
+                config.scenario.target_rpm,
+                config.scenario.total_s(),
+                config.between_trials.as_millis()
+            ),
+        };
+        if !ready {
+            // 启动失败也必须走完整停机路径：先 0 电流，再关总线。
+            evaluator.shutdown();
+            return Err(BenchError::NoFeedback {
+                esc_id,
+                waited_ms: startup_timeout.as_millis() as u64,
+            });
+        }
+        Ok(evaluator)
+    }
+
+    /// 最优轨迹（若有）。
+    #[must_use]
+    pub fn best_trace(&self) -> Option<Trace> {
+        self.best
+            .lock()
+            .unwrap_or_else(std::sync::PoisonError::into_inner)
+            .as_ref()
+            .map(|(_, trace)| trace.clone())
+    }
+
+    /// 搜索期间的安全中止记录。
+    #[must_use]
+    pub fn aborts(&self) -> Vec<String> {
+        self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).clone()
+    }
+
+    /// 停机并关闭总线（**必须**在结束前调用）。
+    pub fn shutdown(&self) {
+        let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
+        let Some(bus) = session.bus.take() else {
+            return;
+        };
+        for motor in bus.motors() {
+            motor.disable();
+        }
+        session.runtime.block_on(async {
+            tokio::time::sleep(Duration::from_millis(50)).await;
+            bus.shutdown().await;
+        });
+    }
+
+    /// 一次真机试验（含试验间隔静默）。
+    fn run_trial(&self, gains: &VelocityGains) -> Score {
+        let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
+        let Session { runtime, motor, .. } = &mut *session;
+        motor.set_velocity_gains(*gains);
+        let scenario = self.scenario;
+        let envelope = self.envelope.clone();
+        let limit = self.effort_limit_raw;
+        let lease = self.lease_period;
+        let between = self.between_trials;
+
+        let outcome = runtime.block_on(async {
+            let trial =
+                run_step_trial(motor, &scenario, &envelope, limit, lease, std::future::pending())
+                    .await;
+            // 试验间隔：让轮子真正停下来，再让下一次试验的起点一致。
+            tokio::time::sleep(between).await;
+            trial
+        });
+
+        if let Some(reason) = outcome.abort {
+            let verdict = verdict_for(&reason);
+            let message = format!("{}: {reason}", self.describe);
+            self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).push(message);
+            return Score::penalized(verdict);
+        }
+
+        let score = match crate::metrics::StepMetrics::from_trace(&outcome.trace, &self.scenario) {
+            Some(metrics) => self.objective.score_metrics(metrics),
+            None => Score::penalized(Verdict::Rejected),
+        };
+        // 记录目前最优的轨迹，供报告画图 / 复盘。
+        let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
+        if best.as_ref().is_none_or(|(cost, _)| score.cost < *cost) {
+            *best = Some((score.cost, outcome.trace));
+        }
+        score
+    }
+}
+
+impl Drop for BenchEvaluator {
+    fn drop(&mut self) {
+        self.shutdown();
+    }
+}
+
+impl Evaluator for BenchEvaluator {
+    fn evaluate(&self, gains: &VelocityGains) -> Score {
+        let score = self.run_trial(gains);
+        if !score.verdict.is_ok() && !self.continue_on_abort {
+            // 真机上出现安全事件时不"带伤继续搜索"：交给调用方在 sweep 之后
+            // 检查 `aborts()` 并决定是否收工。这里只记录，不 panic。
+            tracing::warn!(verdict = score.verdict.label(), "bench trial did not pass");
+        }
+        score
+    }
+
+    fn describe(&self) -> String {
+        self.describe.clone()
+    }
+}
+
+/// 安全事件 → 成本判定。
+///
+/// 过速 / 过温说明控制器失稳；掉线 / 超时 / 取消说明本次试验没做完，
+/// 两者的结果都不可用，但原因不同——报告要能区分。
+#[must_use]
+pub fn verdict_for(abort: &Abort) -> Verdict {
+    match abort {
+        Abort::Overspeed { .. } | Abort::OverTemperature { .. } => Verdict::Overspeed,
+        Abort::FeedbackLost { .. } | Abort::Timeout { .. } | Abort::Cancelled => Verdict::Rejected,
+    }
+}
+
+/// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。
+///
+/// # Errors
+///
+/// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。
+pub fn open_link(
+    device: &str,
+    serial_baud: u32,
+    can_speed: chassis_bench::CanSpeed,
+    interface: Option<&str>,
+) -> Result<can_link::CanLink, BenchError> {
+    let result = match interface {
+        Some(interface) => chassis_bench::open_socketcan(interface),
+        None => chassis_bench::open_slcan(
+            device,
+            chassis_bench::default_slcan_options(serial_baud, can_speed),
+        ),
+    };
+    result
+        .with_context(|| match interface {
+            Some(interface) => format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）"),
+            None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"),
+        })
+        .map_err(|e| BenchError::Link(e.to_string()))
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use crate::cost::Weights;
+
+    fn config() -> BenchConfig {
+        let mut esc = EscConfig::gm3508("bench", 1, 1.0);
+        esc.current_limit = 1500;
+        let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15);
+        let objective = Objective::new(
+            Weights::default(),
+            scenario,
+            LoopLimits { current_limit_raw: esc.current_limit, model: esc.model },
+        );
+        let envelope = SafetyEnvelope::new()
+            .with_overspeed(1200.0, Duration::from_millis(60))
+            .with_temperature_max(70)
+            .with_feedback_timeout(Duration::from_millis(200))
+            .with_trial_timeout(Duration::from_secs(2));
+        BenchConfig::new(esc, scenario, envelope, objective)
+    }
+
+    #[test]
+    fn config_defaults_are_safe() {
+        let config = config();
+        assert_eq!(config.bus.period, Duration::from_millis(1));
+        assert_eq!(config.bus.command_timeout, Duration::from_millis(50));
+        assert!(!config.continue_on_abort, "默认必须在安全事件后收工");
+        assert!(config.between_trials >= Duration::from_millis(200));
+        assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约");
+    }
+
+    #[test]
+    fn every_abort_maps_to_a_definite_verdict() {
+        let cases = [
+            Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 },
+            Abort::OverTemperature { temperature_c: 90, limit_c: 70 },
+            Abort::FeedbackLost { esc_id: 1, age_ms: None, timeout_ms: 200 },
+            Abort::Timeout { elapsed_ms: 6000 },
+            Abort::Cancelled,
+        ];
+        for abort in cases {
+            let verdict = verdict_for(&abort);
+            assert!(!verdict.is_ok(), "{abort:?} 不得被判为达标");
+        }
+        assert_eq!(
+            verdict_for(&Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 }),
+            Verdict::Overspeed
+        );
+        assert_eq!(verdict_for(&Abort::Cancelled), Verdict::Rejected);
+    }
+
+    #[test]
+    fn link_open_failures_are_errors() {
+        // 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。
+        let result =
+            open_link("/definitely/not/a/device", 2_000_000, chassis_bench::CanSpeed::M1, None);
+        assert!(result.is_err(), "不存在的串口设备必须报错");
+        let missing_interface =
+            open_link("ignored", 2_000_000, chassis_bench::CanSpeed::M1, Some("can-not-real"));
+        assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错");
+    }
+}