refactor: 重构架构并整理仓库
This commit is contained in:
@@ -0,0 +1,26 @@
|
||||
[package]
|
||||
name = "bench"
|
||||
description = "Hardware bench links, safety envelope, and trial runner"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,947 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod link {
|
||||
//! 链路打开:把两种运输后端收敛成一个门面。
|
||||
//!
|
||||
//! 台架工具与自动整定的真机后端都需要"给个设备名,拿回一条 [`CanLink`]",
|
||||
//! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处,上层就不必
|
||||
//! 同时依赖两个运输 crate。
|
||||
|
||||
use std::io::Result;
|
||||
use std::time::Duration;
|
||||
|
||||
use can::CanLink;
|
||||
|
||||
/// slcan 波特率档位(对应协议 `Sn` 命令)。
|
||||
pub use slcan::CanSpeed;
|
||||
/// slcan 打开参数。
|
||||
pub use slcan::SlcanOptions;
|
||||
|
||||
/// 打开 slcan 串口设备(CANable 等),桥接为链路端点。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。
|
||||
pub fn open_slcan(device: &str, options: SlcanOptions) -> Result<CanLink> {
|
||||
slcan::open_link_with(device, options)
|
||||
}
|
||||
|
||||
/// 打开 SocketCAN 接口(Linux)。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 未 up / 权限不足时的 `io::Error`;非 Linux 平台恒为错误。
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
socketcan::open_link(interface)
|
||||
}
|
||||
|
||||
/// 非 Linux 平台没有 SocketCAN。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 恒返回 `Unsupported`。
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
Err(std::io::Error::new(
|
||||
std::io::ErrorKind::Unsupported,
|
||||
format!("SocketCAN ({interface}) 仅支持 Linux"),
|
||||
))
|
||||
}
|
||||
|
||||
/// 默认 slcan 参数(1 Mbps、串口超时 50 ms)。
|
||||
#[must_use]
|
||||
pub fn default_slcan_options(serial_baud: u32, speed: CanSpeed) -> SlcanOptions {
|
||||
SlcanOptions { serial_baud, speed, read_timeout: Duration::from_millis(50) }
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn socketcan_is_unavailable_off_linux() {
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
{
|
||||
let err = open_socketcan("can0").expect_err("非 Linux 必须报错");
|
||||
assert_eq!(err.kind(), std::io::ErrorKind::Unsupported);
|
||||
}
|
||||
#[cfg(target_os = "linux")]
|
||||
{
|
||||
// Linux 上接口名不存在时也必须是 Err,而不是 panic。
|
||||
let _ = open_socketcan("definitely-not-a-can-interface");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn slcan_failure_is_an_error_not_a_panic() {
|
||||
let result = open_slcan(
|
||||
"/definitely/not/a/device",
|
||||
default_slcan_options(2_000_000, CanSpeed::M1),
|
||||
);
|
||||
assert!(result.is_err());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_options_carry_the_requested_baud() {
|
||||
let options = default_slcan_options(115_200, CanSpeed::K500);
|
||||
assert_eq!(options.serial_baud, 115_200);
|
||||
assert_eq!(options.speed, CanSpeed::K500);
|
||||
assert_eq!(options.read_timeout, Duration::from_millis(50));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod safety {
|
||||
//! 台架安全包络:把"什么时候必须立刻停机"变成可复用的判定。
|
||||
//!
|
||||
//! 台架工具(`chassis-spin`)与自动整定的真机后端(`chassis-tune --backend bench`)
|
||||
//! 共用同一套判定,避免安全逻辑在两个地方各写一遍、然后慢慢漂移。
|
||||
//!
|
||||
//! 所有阈值都是 `Option`:只开需要的那个。`chassis-spin` 只开过速保护
|
||||
//! (保持历史行为),自动整定则开启全部——因为搜索会主动试错,必须由
|
||||
//! 包络兜住"试错到失控"。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use motor::MotorFeedback;
|
||||
// 用 tokio 的时钟:`start_paused` 下虚拟时间可控,安全包络(宽限期 / 超时)
|
||||
// 才能被确定性地测试;生产构建里它就是 `std::time::Instant`。
|
||||
use thiserror::Error;
|
||||
use tokio::time::Instant;
|
||||
|
||||
/// 触发停机的原因。
|
||||
#[derive(Debug, Clone, PartialEq, Error)]
|
||||
pub enum Abort {
|
||||
/// 转速超过上限并持续超过宽限期。
|
||||
#[error("overspeed: {peak_rpm:.0} rpm > {limit_rpm:.0} rpm for over {grace_ms}ms")]
|
||||
Overspeed {
|
||||
/// 观测到的峰值转速(rpm)。
|
||||
peak_rpm: f32,
|
||||
/// 配置的上限(rpm)。
|
||||
limit_rpm: f32,
|
||||
/// 宽限期(毫秒)。
|
||||
grace_ms: u64,
|
||||
},
|
||||
/// 温度超过上限。
|
||||
#[error("overtemperature: {temperature_c}℃ > {limit_c}℃")]
|
||||
OverTemperature {
|
||||
/// 观测温度(℃)。
|
||||
temperature_c: i8,
|
||||
/// 上限(℃)。
|
||||
limit_c: i8,
|
||||
},
|
||||
/// 反馈超出新鲜度阈值(掉线)。
|
||||
#[error("esc {esc_id} feedback lost: age {age_ms:?}ms > {timeout_ms}ms")]
|
||||
FeedbackLost {
|
||||
/// 电调 ID。
|
||||
esc_id: u8,
|
||||
/// 实测反馈年龄(毫秒)。
|
||||
age_ms: Option<u128>,
|
||||
/// 阈值(毫秒)。
|
||||
timeout_ms: u64,
|
||||
},
|
||||
/// 整个试验超时。
|
||||
#[error("trial timeout after {elapsed_ms}ms")]
|
||||
Timeout {
|
||||
/// 已耗时(毫秒)。
|
||||
elapsed_ms: u128,
|
||||
},
|
||||
/// 上层主动取消(ctrl-c)。
|
||||
#[error("cancelled by operator")]
|
||||
Cancelled,
|
||||
}
|
||||
|
||||
/// 安全包络(阈值 + 宽限期)。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct SafetyEnvelope {
|
||||
/// 转速上限(rpm)。
|
||||
pub max_rpm: Option<f32>,
|
||||
/// 温度上限(℃)。
|
||||
pub max_temperature_c: Option<i8>,
|
||||
/// 反馈新鲜度阈值:超过它即视为掉线。
|
||||
pub feedback_timeout: Option<Duration>,
|
||||
/// 过速需要持续多久才判定为失控(抑制单帧噪声)。
|
||||
pub overspeed_grace: Duration,
|
||||
/// 单次试验的总时长上限。
|
||||
pub trial_timeout: Option<Duration>,
|
||||
}
|
||||
|
||||
impl Default for SafetyEnvelope {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_rpm: None,
|
||||
max_temperature_c: None,
|
||||
feedback_timeout: None,
|
||||
overspeed_grace: Duration::from_millis(60),
|
||||
trial_timeout: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SafetyEnvelope {
|
||||
/// 空包络(不判定任何条件)。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 只开过速保护(`chassis-spin` 的历史行为)。
|
||||
#[must_use]
|
||||
pub fn overspeed_only(max_rpm: f32, grace: Duration) -> Self {
|
||||
Self { max_rpm: Some(max_rpm), overspeed_grace: grace, ..Self::default() }
|
||||
}
|
||||
|
||||
/// 开过速保护。
|
||||
#[must_use]
|
||||
pub fn with_overspeed(mut self, max_rpm: f32, grace: Duration) -> Self {
|
||||
self.max_rpm = Some(max_rpm);
|
||||
self.overspeed_grace = grace;
|
||||
self
|
||||
}
|
||||
|
||||
/// 开温度保护。
|
||||
#[must_use]
|
||||
pub fn with_temperature_max(mut self, max_temperature_c: i8) -> Self {
|
||||
self.max_temperature_c = Some(max_temperature_c);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开掉线保护。
|
||||
#[must_use]
|
||||
pub fn with_feedback_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.feedback_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开试验总时长保护。
|
||||
#[must_use]
|
||||
pub fn with_trial_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.trial_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 是否需要任何判定(空包络时调用方可以跳过整段检查)。
|
||||
#[must_use]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.max_rpm.is_none()
|
||||
&& self.max_temperature_c.is_none()
|
||||
&& self.feedback_timeout.is_none()
|
||||
&& self.trial_timeout.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 带状态的安全监视器(过速宽限与总时长需要记忆)。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SafetyMonitor {
|
||||
envelope: SafetyEnvelope,
|
||||
started: Instant,
|
||||
overspeed_since: Option<Instant>,
|
||||
}
|
||||
|
||||
impl SafetyMonitor {
|
||||
/// 创建监视器并记录起点。
|
||||
#[must_use]
|
||||
pub fn new(envelope: SafetyEnvelope, now: Instant) -> Self {
|
||||
Self { envelope, started: now, overspeed_since: None }
|
||||
}
|
||||
|
||||
/// 当前包络。
|
||||
#[must_use]
|
||||
pub const fn envelope(&self) -> &SafetyEnvelope {
|
||||
&self.envelope
|
||||
}
|
||||
|
||||
/// 检查一组电机在当前时刻是否触碰包络。
|
||||
///
|
||||
/// 返回 `Some(Abort)` 即调用方**必须立刻停电流并结束本次试验**。
|
||||
/// 每次调用**不分配**(1 kHz 台架循环里每拍都要过一遍)。
|
||||
pub fn check<M: MotorFeedback>(&mut self, motors: &[M], now: Instant) -> Option<Abort> {
|
||||
if let Some(timeout) = self.envelope.trial_timeout {
|
||||
let elapsed = now.saturating_duration_since(self.started);
|
||||
if elapsed > timeout {
|
||||
return Some(Abort::Timeout { elapsed_ms: elapsed.as_millis() });
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(timeout) = self.envelope.feedback_timeout {
|
||||
for motor in motors {
|
||||
let age = motor.feedback_age();
|
||||
if age.is_none_or(|age| age > timeout) {
|
||||
return Some(Abort::FeedbackLost {
|
||||
esc_id: motor.id(),
|
||||
age_ms: age.map(|age| age.as_millis()),
|
||||
timeout_ms: timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_temperature_c {
|
||||
for motor in motors {
|
||||
let temperature = motor.state().temperature_c;
|
||||
if temperature > limit {
|
||||
return Some(Abort::OverTemperature {
|
||||
temperature_c: temperature,
|
||||
limit_c: limit,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_rpm {
|
||||
let peak = motors
|
||||
.iter()
|
||||
.map(|motor| motor.state().rotor_speed_rpm.abs())
|
||||
.fold(0.0f32, f32::max);
|
||||
if peak > limit {
|
||||
match self.overspeed_since {
|
||||
None => self.overspeed_since = Some(now),
|
||||
Some(since)
|
||||
if now.saturating_duration_since(since)
|
||||
>= self.envelope.overspeed_grace =>
|
||||
{
|
||||
return Some(Abort::Overspeed {
|
||||
peak_rpm: peak,
|
||||
limit_rpm: limit,
|
||||
grace_ms: self.envelope.overspeed_grace.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Some(_) => {}
|
||||
}
|
||||
} else {
|
||||
self.overspeed_since = None;
|
||||
}
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor::{MockMotor, MotorState};
|
||||
|
||||
fn motor_with(id: u8, rpm: f32, temperature_c: i8, age: Option<Duration>) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
let state = MotorState {
|
||||
rotor_speed_rpm: rpm,
|
||||
temperature_c,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
};
|
||||
match age {
|
||||
Some(age) => motor.inject_state_with_age(state, age),
|
||||
None => motor.inject_state(state),
|
||||
}
|
||||
motor
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_envelope_never_aborts() {
|
||||
let motors = vec![motor_with(1, 1.0e6, 120, Some(Duration::from_secs(5)))];
|
||||
let mut monitor = SafetyMonitor::new(SafetyEnvelope::new(), Instant::now());
|
||||
assert!(monitor.envelope().is_empty());
|
||||
assert_eq!(monitor.check(&motors, Instant::now()), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_needs_the_grace_period() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None, "宽限期内不得触发");
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(30)), None);
|
||||
let abort = monitor.check(&motors, now + grace);
|
||||
assert!(matches!(abort, Some(Abort::Overspeed { .. })), "{abort:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_timer_resets_when_back_under_limit() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None);
|
||||
let calm = vec![motor_with(1, 100.0, 30, None)];
|
||||
assert_eq!(monitor.check(&calm, now + Duration::from_millis(30)), None);
|
||||
// 重新超速:计时必须从头开始。
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(40)), None);
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(90)).is_none());
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(110)).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_and_feedback_loss_abort_immediately() {
|
||||
let envelope = SafetyEnvelope::new()
|
||||
.with_temperature_max(70)
|
||||
.with_feedback_timeout(Duration::from_millis(100));
|
||||
let hot = vec![motor_with(1, 0.0, 80, None)];
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&hot, Instant::now()),
|
||||
Some(Abort::OverTemperature { .. })
|
||||
));
|
||||
|
||||
let stale = vec![motor_with(1, 0.0, 30, Some(Duration::from_millis(250)))];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&stale, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 1, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_without_any_feedback_is_lost() {
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
// 从未收到过反馈:age = None。
|
||||
let silent = vec![MockMotor::new(3)];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&silent, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 3, age_ms: None, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn trial_timeout_aborts() {
|
||||
let envelope = SafetyEnvelope::new().with_trial_timeout(Duration::from_millis(500));
|
||||
let motors = vec![motor_with(1, 0.0, 30, None)];
|
||||
let start = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope, start);
|
||||
assert_eq!(monitor.check(&motors, start + Duration::from_millis(100)), None);
|
||||
let abort = monitor.check(&motors, start + Duration::from_millis(600));
|
||||
assert!(matches!(abort, Some(Abort::Timeout { .. })), "{abort:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod sampler {
|
||||
//! 真机试验:把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。
|
||||
//!
|
||||
//! 与 `sim-harness` 的离线 harness 是**同一套场景词汇**:整定工具先用离线
|
||||
//! harness 搜索,再用本模块在台架上复核,两条路径的指标口径因此一致。
|
||||
//!
|
||||
//! # 安全契约
|
||||
//!
|
||||
//! - 每个采样拍都重发设定值——**命令是租约**,不重发会在 50ms 后失电;
|
||||
//! - 每个采样拍都过一遍 [`SafetyMonitor`];
|
||||
//! - 无论正常结束、触发包络还是被取消,退出前一律 `disable()`(0 电流)。
|
||||
|
||||
use std::future::Future;
|
||||
use std::time::Duration;
|
||||
|
||||
use experiment::{Scenario, Trace};
|
||||
use motor::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval};
|
||||
|
||||
use crate::safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
|
||||
/// 一次真机试验的结果。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TrialOutcome {
|
||||
/// 采集到的轨迹(触发中止时只含中止前的采样)。
|
||||
pub trace: Trace,
|
||||
/// 触发安全包络 / 被取消的原因;`None` 表示按场景跑完。
|
||||
pub abort: Option<Abort>,
|
||||
}
|
||||
|
||||
impl TrialOutcome {
|
||||
/// 是否完整跑完场景(没有被安全包络打断)。
|
||||
#[must_use]
|
||||
pub fn completed(&self) -> bool {
|
||||
self.abort.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 采样节拍下限(防止 `dt_s` 给出病态值把 CPU 打满)。
|
||||
const MIN_TICK: Duration = Duration::from_micros(50);
|
||||
|
||||
/// 在单台电机上重放一条场景。
|
||||
///
|
||||
/// - `effort_limit_raw`:本次试运行的 effort 限幅,仅用于标注 `saturated`
|
||||
/// (控制器真正的钳位在驱动层按 `EscConfig::current_limit` 完成);
|
||||
/// - `lease_period`:命令租约的续期节拍(比 `scenario.dt_s` 更稀时以采样节拍为准);
|
||||
/// - `cancel`:外部取消源(ctrl-c / 搜索预算耗尽)。
|
||||
pub async fn run_step_trial<M, F>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
cancel: F,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
F: Future<Output = ()>,
|
||||
{
|
||||
let dt = Duration::from_secs_f32(scenario.dt_s.max(1.0e-6)).max(MIN_TICK);
|
||||
let total = Duration::from_secs_f32(scenario.total_s().max(0.0));
|
||||
// 续租节拍不得稀于采样节拍,否则会短暂失电。
|
||||
let lease_period = lease_period.min(dt);
|
||||
|
||||
let started = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), started);
|
||||
let mut ticker = interval(dt);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut lease = interval(lease_period);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
let mut trace = Trace::with_capacity(scenario.samples());
|
||||
let mut setpoint = scenario.setpoint_at(0.0);
|
||||
let mut abort = None;
|
||||
tokio::pin!(cancel);
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
() = &mut cancel => {
|
||||
abort = Some(Abort::Cancelled);
|
||||
break;
|
||||
}
|
||||
_ = lease.tick() => {
|
||||
// 续租:命令是租约,不重发就失电。
|
||||
motor.set_velocity(setpoint);
|
||||
}
|
||||
_ = ticker.tick() => {
|
||||
let now = Instant::now();
|
||||
let elapsed = now.saturating_duration_since(started);
|
||||
if elapsed > total {
|
||||
break;
|
||||
}
|
||||
let t_s = elapsed.as_secs_f32();
|
||||
setpoint = scenario.setpoint_at(t_s);
|
||||
motor.set_velocity(setpoint);
|
||||
|
||||
let state = motor.state();
|
||||
let saturated =
|
||||
i32::from(state.effort_raw).abs() >= i32::from(effort_limit_raw);
|
||||
trace.push(t_s, setpoint, state.rotor_speed_rpm, state.effort_raw, saturated);
|
||||
|
||||
if let Some(reason) = monitor.check(std::slice::from_ref(motor), now) {
|
||||
abort = Some(reason);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 安全收尾:任何退出路径都先归零,绝不把非零给定留在总线上。
|
||||
motor.disable();
|
||||
TrialOutcome { trace, abort }
|
||||
}
|
||||
|
||||
/// 以 ctrl-c 作为取消源的 [`run_step_trial`] 包装。
|
||||
pub async fn run_step_trial_until_ctrl_c<M>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
{
|
||||
run_step_trial(motor, scenario, envelope, effort_limit_raw, lease_period, async {
|
||||
let _ = tokio::signal::ctrl_c().await;
|
||||
})
|
||||
.await
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor::{Command, MockMotor, MotorState};
|
||||
|
||||
fn ready_motor(id: u8) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
motor.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
|
||||
motor
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn trial_follows_scenario_and_disables_at_the_end() {
|
||||
let motor = ready_motor(1);
|
||||
let scenario = Scenario::ramp_and_hold(1000.0, 0.1, 0.2);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(outcome.completed(), "{:?}", outcome.abort);
|
||||
assert!(outcome.trace.is_consistent());
|
||||
assert!(outcome.trace.len() > 250, "len = {}", outcome.trace.len());
|
||||
// 斜坡中点的设定值应约为一半。
|
||||
let mid = outcome.trace.window(0.049, 0.052).setpoint_rpm()[0];
|
||||
assert!((mid - 500.0).abs() < 30.0, "mid setpoint = {mid}");
|
||||
// 收尾必须归零。
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn overspeed_aborts_and_zeroes_output() {
|
||||
let motor = ready_motor(1);
|
||||
// 反馈恒为 5000 rpm,远超 3000 rpm 上限。
|
||||
motor.inject_state(MotorState {
|
||||
rotor_speed_rpm: 5000.0,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
});
|
||||
let envelope = SafetyEnvelope::overspeed_only(3000.0, Duration::from_millis(20));
|
||||
let scenario = Scenario::step(1000.0, 2.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(matches!(outcome.abort, Some(Abort::Overspeed { .. })), "{:?}", outcome.abort);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
// 中止得足够早,不该跑满场景。
|
||||
assert!(outcome.trace.len() < 200, "len = {}", outcome.trace.len());
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn stalled_feedback_aborts() {
|
||||
let motor = MockMotor::new(4);
|
||||
motor.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(500),
|
||||
);
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
let scenario = Scenario::step(500.0, 1.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(
|
||||
matches!(outcome.abort, Some(Abort::FeedbackLost { esc_id: 4, .. })),
|
||||
"{:?}",
|
||||
outcome.abort
|
||||
);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn cancellation_stops_and_zeroes_output() {
|
||||
let motor = ready_motor(2);
|
||||
let scenario = Scenario::step(800.0, 5.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
async { tokio::time::sleep(Duration::from_millis(50)).await },
|
||||
)
|
||||
.await;
|
||||
|
||||
assert_eq!(outcome.abort, Some(Abort::Cancelled));
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
assert!(!outcome.trace.is_empty());
|
||||
assert!(outcome.trace.len() < 200, "取消后不得继续采样");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod hil {
|
||||
//! 真机(HIL)评估后端:在台架上逐次试验一组增益。
|
||||
//!
|
||||
//! 与 [`SimEvaluator`](crate::SimEvaluator) 实现同一个 [`Evaluator`],
|
||||
//! 因此 `sweep` / `validate` 的全部逻辑(粗扫、精修、报告)都不需要改动,
|
||||
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
|
||||
//!
|
||||
//! # 安全设计
|
||||
//!
|
||||
//! 每次试验都:① 先确认目标电调反馈新鲜;② 逐拍重发设定值(命令租约);
|
||||
//! ③ 每拍过一遍 [`SafetyEnvelope`];④ 无论正常结束、触发包络还是 ctrl-c,
|
||||
//! 退出前一律 `disable()`;⑤ 试验之间插入静默期,让轮子真正停下来再开始下一组。
|
||||
//!
|
||||
//! 评估是**串行**的(真机只有一条总线)。调用方应把搜索放进单线程 rayon 池
|
||||
//! (见 `main`),否则大量线程会在互斥锁上排队、白白浪费时间。
|
||||
|
||||
use std::sync::Mutex;
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{SafetyEnvelope, run_step_trial};
|
||||
use anyhow::Context as _;
|
||||
use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use experiment::Scenario;
|
||||
use motor::{Disable as _, MotorFeedback};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::sampler::TrialOutcome;
|
||||
|
||||
/// 真机后端错误。
|
||||
#[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 between_trials: Duration,
|
||||
/// 命令租约续期节拍。
|
||||
pub lease_period: Duration,
|
||||
/// 启动时等待反馈的超时。
|
||||
pub startup_timeout: Duration,
|
||||
}
|
||||
|
||||
impl BenchConfig {
|
||||
/// 合理默认:1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
|
||||
#[must_use]
|
||||
pub fn new(esc: EscConfig, scenario: Scenario, envelope: SafetyEnvelope) -> Self {
|
||||
Self {
|
||||
esc,
|
||||
bus: BusOptions::default(),
|
||||
scenario,
|
||||
envelope,
|
||||
between_trials: Duration::from_millis(500),
|
||||
lease_period: Duration::from_millis(5),
|
||||
startup_timeout: Duration::from_secs(5),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 真机评估器(持有总线与运行时)。
|
||||
pub struct BenchEvaluator {
|
||||
session: Mutex<Session>,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
between_trials: Duration,
|
||||
lease_period: Duration,
|
||||
}
|
||||
|
||||
struct Session {
|
||||
runtime: tokio::runtime::Runtime,
|
||||
bus: Option<EscBusHandle<MotorPolicy>>,
|
||||
motor: EscMotor,
|
||||
}
|
||||
|
||||
impl BenchEvaluator {
|
||||
/// 打开链路、启动总线并等待目标电调反馈。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路 / 总线启动失败,或启动超时(电调无反馈)。
|
||||
pub fn open(link: can::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
|
||||
let esc_id = config.esc.esc_id;
|
||||
let effort_limit_raw = config.esc.current_limit;
|
||||
let runtime = tokio::runtime::Builder::new_multi_thread()
|
||||
.worker_threads(2)
|
||||
.enable_all()
|
||||
.build()
|
||||
.map_err(|e| BenchError::Bus(e.to_string()))?;
|
||||
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(config.esc.esc_id, config.esc.current_limit, VelocityGains::ZERO);
|
||||
let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone(), policy)
|
||||
.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,
|
||||
effort_limit_raw,
|
||||
between_trials: config.between_trials,
|
||||
lease_period: config.lease_period,
|
||||
};
|
||||
if !ready {
|
||||
// 启动失败也必须走完整停机路径:先 0 电流,再关总线。
|
||||
evaluator.shutdown();
|
||||
return Err(BenchError::NoFeedback {
|
||||
esc_id,
|
||||
waited_ms: startup_timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Ok(evaluator)
|
||||
}
|
||||
|
||||
/// Scenario executed by each trial.
|
||||
#[must_use]
|
||||
pub fn scenario(&self) -> Scenario {
|
||||
self.scenario
|
||||
}
|
||||
|
||||
/// Stop outputs and close the bus.
|
||||
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;
|
||||
});
|
||||
}
|
||||
|
||||
/// Run one physical trial and return its raw trace and abort reason.
|
||||
pub fn run_trial(&self, gains: &VelocityGains) -> TrialOutcome {
|
||||
let mut session =
|
||||
self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
let Session { runtime, motor, bus, .. } = &mut *session;
|
||||
let esc_id = motor.id();
|
||||
if let Some(bus) = bus.as_ref() {
|
||||
bus.with_policy(|policy| policy.set_velocity_gains(esc_id, *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;
|
||||
|
||||
runtime.block_on(async {
|
||||
let trial = run_step_trial(
|
||||
motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
limit,
|
||||
lease,
|
||||
std::future::pending(),
|
||||
)
|
||||
.await;
|
||||
tokio::time::sleep(between).await;
|
||||
trial
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开真机链路:给了 `interface` 走 SocketCAN,否则走 slcan 串口。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备 / 接口打开失败(路径、权限、接口未 up、平台不支持)时返回错误。
|
||||
pub fn open_link(
|
||||
device: &str,
|
||||
serial_baud: u32,
|
||||
can_speed: crate::CanSpeed,
|
||||
interface: Option<&str>,
|
||||
) -> Result<can::CanLink, BenchError> {
|
||||
let result = match interface {
|
||||
Some(interface) => crate::open_socketcan(interface),
|
||||
None => crate::open_slcan(device, crate::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::*;
|
||||
|
||||
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 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)
|
||||
}
|
||||
|
||||
#[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.between_trials >= Duration::from_millis(200));
|
||||
assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn link_open_failures_are_errors() {
|
||||
// 不存在的设备:必须返回 Err 而不是 panic(真机前最常见的失败模式)。
|
||||
let result =
|
||||
open_link("/definitely/not/a/device", 2_000_000, crate::CanSpeed::M1, None);
|
||||
assert!(result.is_err(), "不存在的串口设备必须报错");
|
||||
let missing_interface =
|
||||
open_link("ignored", 2_000_000, crate::CanSpeed::M1, Some("can-not-real"));
|
||||
assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use hil::*;
|
||||
pub use link::*;
|
||||
pub use safety::*;
|
||||
pub use sampler::*;
|
||||
Reference in New Issue
Block a user