384 lines
14 KiB
Rust
384 lines
14 KiB
Rust
//! 真机（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 接口必须报错");
|
||
}
|
||
}
|