refactor: enforce layered crates and Rust audit gates

This commit is contained in:
Horldsence
2026-09-24 13:00:03 +08:00
parent af151525d1
commit 8a582fc102
67 changed files with 3918 additions and 581 deletions
+29
View File
@@ -0,0 +1,29 @@
[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"] }
[package.metadata.sentry-chassis]
layer = "application/bench"
+965
View File
@@ -0,0 +1,965 @@
#![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 runtime_guard = runtime.enter();
let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone(), policy)
.map_err(|e| BenchError::Bus(e.to_string()))?;
drop(runtime_guard);
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 接口必须报错");
}
#[test]
fn evaluator_spawns_bus_without_an_outer_tokio_runtime() {
let (link, peer) = can::CanLink::pair(8);
// 测试只关心 runtime 启动;立刻关闭对端,避免假链路堵住 1kHz 命令发送。
drop(peer);
let mut config = config();
config.startup_timeout = Duration::from_millis(1);
let result = BenchEvaluator::open(link, config);
assert!(
matches!(result, Err(BenchError::NoFeedback { esc_id: 1, .. })),
"应当在自有 runtime 中启动总线并因无反馈返回错误"
);
}
}
}
pub use hil::*;
pub use link::*;
pub use safety::*;
pub use sampler::*;
+33
View File
@@ -0,0 +1,33 @@
[package]
name = "chassisd"
description = "Mecanum chassis daemon"
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 }
motor-control = { workspace = true }
mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
chassis-config = { workspace = true }
sim-harness = { workspace = true }
socketcan = { workspace = true }
tokio = { workspace = true }
clap = { workspace = true }
tracing = { workspace = true }
tracing-subscriber = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/daemon"
File diff suppressed because it is too large Load Diff
+32
View File
@@ -0,0 +1,32 @@
[package]
name = "spin"
description = "Chassis and single-motor hardware bench tool"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
[lints]
workspace = true
[dependencies]
bench = { workspace = true }
motor = { workspace = true }
esc-bus = { workspace = true }
experiment = { workspace = true }
motor-control = { workspace = true }
mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
slcan = { workspace = true }
socketcan = { workspace = true }
tokio = { workspace = true }
clap = { workspace = true }
tracing = { workspace = true }
tracing-subscriber = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "application/tools"
+810
View File
@@ -0,0 +1,810 @@
mod chassis {
//! 实车台架工具:让**四个轮子同时**以固定**转子**转速旋转(默认 500 rpm)。
//!
//! ```sh
//! # T153 板载 CANable2(slcan 固件)→ /dev/ttyACM0,四轮 500 rpm 转子,缓升 1s 保持 10s
//! cargo run --release -p chassis-bench --bin chassis-spin -- \
//! --device /dev/ttyACM0 --rpm 500
//!
//! # 只验证链路与反馈,不发任何非零电流(接线排查用)
//! cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
//! ```
//!
//! # 抽象边界
//!
//! 本工具是**组合根**:`dji-esc-slcan`(链路桥接)→ `dji-esc`(C620 协议 +
//! 速度环)→ `motor-core`(`VelocityDrive` / `MotorFeedback` / `Disable` 能力
//! trait)。激励场景与安全包络来自同 crate 的 `chassis-bench` 库,与
//! `chassis-tune` 的真机后端**共用同一套实现**。
//!
//! # 命令租约(重要)
//!
//! `EscBus` 的命令是**租约**:`command_timeout`(默认 50ms)内没有重新下发命令,
//! 驱动会强制 0 电流。所以控制循环必须**逐拍重发**同一个设定值——本工具在 1kHz
//! 节拍里重发(与 `chassisd` 的控制循环一致)。只在开头下发一次是**不行的**。
//!
//! # 安全
//!
//! 轮子必须架空或整车固定,急停可及。默认低限流、有限时长、ctrl-c 任意时刻
//! 安全停机(先 0 电流再关总线);未收到全部反馈前绝不下发速度。
use std::process::ExitCode;
use std::time::Duration;
use bench::{SafetyEnvelope, SafetyMonitor};
use clap::{Parser, ValueEnum};
use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
use experiment::Scenario;
use motor::{Disable, MotorFeedback, VelocityDrive};
use motor_control::{MotorPolicy, VelocityGains};
use slcan::{CanSpeed, SlcanOptions, open_link_with};
use tokio::time::{Instant, MissedTickBehavior, interval, sleep};
/// 稳态判据:平均转子转速与目标的相对误差上限。
const TOLERANCE: f32 = 0.05;
/// 状态打印周期。
const REPORT_PERIOD: Duration = Duration::from_millis(200);
/// 反馈新鲜度阈值:`feedback_age` 超过它即视为该电调掉线。
const FRESH: Duration = Duration::from_millis(100);
/// 过速需要持续这么久才判定为失控并停机(避免单帧噪声误触发)。
const OVERSPEED_GRACE: Duration = Duration::from_millis(60);
/// 控制节拍(C620 期望 1kHz 命令)。
const CONTROL_PERIOD: Duration = Duration::from_millis(1);
/// 四轮默认名(顺序固定 FL、FR、RL、RR)。
const WHEEL_NAMES: [&str; 4] = ["front_left", "front_right", "rear_left", "rear_right"];
#[derive(Debug, Parser)]
#[command(name = "chassis-spin", version, about = "台架:四轮同时旋转(默认 500 rpm 转子)")]
struct Args {
/// slcan 串口设备(T153 板载 CANable2 为 /dev/ttyACM0)。
#[arg(long, default_value = "/dev/ttyACM0")]
device: String,
/// 串口波特率(CDC ACM 一般忽略;CH340 类适配器需与固件一致)。
#[arg(long, default_value_t = 2_000_000)]
serial_baud: u32,
/// 串口超时(毫秒)。读写共用:`serial` 的 `TTYPort::write` 也按它 poll(POLLOUT)。
#[arg(long, default_value_t = 50)]
serial_timeout_ms: u64,
/// CAN 波特率档位(C620 标准 = 1Mbps)。
#[arg(long, value_enum, default_value_t = CanSpeedArg::M1)]
can_speed: CanSpeedArg,
/// 电调 ID 列表,顺序 = FL、FR、RL、RR。
#[arg(long, value_delimiter = ',', default_value = "1,2,3,4")]
esc_ids: Vec<u8>,
/// 各轮安装方向 ±1(默认左右镜像 → 四轮物理同向)。
#[arg(
long,
value_delimiter = ',',
allow_hyphen_values = true,
default_value = "1,-1,1,-1"
)]
directions: Vec<f32>,
/// 目标**转子**转速(rpm);轮端 = 转子 ÷ 减速比。
#[arg(long, default_value_t = 500.0)]
rpm: f32,
/// 减速比(转子 : 输出轴,GM3508 ≈ 19.203)。
#[arg(long, default_value_t = 19.203)]
gear_ratio: f32,
/// 电流限幅(raw,C620 满量程 16384)。
///
/// 默认 1500 raw(≈1.8A):要越过静摩擦(实测约 400~600 raw)且留出整定余量。
/// 上限同时也是 PID 抗饱和的钳位点——**必须贴近实际所需电流**,
/// 否则积分会在堵转期间积累,一旦起动就冲过头。
#[arg(long, default_value_t = 1500)]
current_limit: i16,
/// 缓升时间:0 → 目标转速(毫秒)。
///
/// 默认 3000:空载 GM3508 的静摩擦需要约 400~600 raw 才能起动(stick-slip),
/// 缓升让积分在起动前平滑建立,避免"先憋住、一动就飞"。
#[arg(long, default_value_t = 3000)]
ramp_ms: u64,
/// 目标转速保持时长(秒)。
#[arg(long, default_value_t = 10.0)]
hold_secs: f32,
/// 等待全部电调反馈的超时(毫秒)。
#[arg(long, default_value_t = 3000)]
startup_timeout_ms: u64,
/// 只验证链路与反馈,不发出任何非零电流。
#[arg(long)]
dry_run: bool,
/// 允许缺失部分电调:仅驱动有反馈的电调继续测试(默认要求四个电调全部在线)。
#[arg(long)]
allow_partial: bool,
/// 速度环 kp(raw/rpm)。
///
/// 空载台架默认 2.0:被控对象近似积分器(≈12 rpm/s per raw),
/// 高 kp(如 8~18)会因反馈延迟震荡;低 kp 配合大 ki 才稳。
#[arg(long, default_value_t = 2.0)]
kp: f32,
/// 速度环 ki(raw/(rpm·s))。
///
/// 空载台架默认 10:静摩擦需要积分项缓慢"顶"到起动电流,ki 太小会长期
/// 停在 300~400 rpm 达不到目标;抗饱和由 `current_limit` 钳位兜底,
/// 所以大 ki 不会跑飞。
#[arg(long, default_value_t = 10.0)]
ki: f32,
/// 速度环 kd。
#[arg(long, default_value_t = 0.0)]
kd: f32,
/// 速度前馈(raw/rpm)。
///
/// **空载台架默认 0**:轮子架空时维持转速只需要克服摩擦的极小电流,
/// 任何接近 1/K(GM3508 空载 K ≈ 0.57 rpm/raw → 1/K ≈ 1.77)的前馈都会把
/// 空载轮子直接推到最高转速。只有带载工况才需要正前馈。
#[arg(long, default_value_t = 0.0)]
feedforward: f32,
/// 过速保护上限(rpm):任一参与测试的电调超过它即立即停机。
/// 默认 = 目标 × 2 + 200。
#[arg(long)]
max_rpm: Option<f32>,
}
/// CAN 波特率档位(对应 slcan 的 `Sn` 命令)。
#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)]
enum CanSpeedArg {
/// `S8`:1 Mbps。
#[value(name = "1m")]
M1,
/// `S6`:500 kbps。
#[value(name = "500k")]
K500,
}
impl From<CanSpeedArg> for CanSpeed {
fn from(value: CanSpeedArg) -> Self {
match value {
CanSpeedArg::M1 => Self::M1,
CanSpeedArg::K500 => Self::K500,
}
}
}
impl Args {
/// 过速保护上限:显式 `--max-rpm` 优先,否则目标 × 2 + 200。
fn overspeed_limit(&self) -> f32 {
self.max_rpm.unwrap_or(self.rpm * 2.0 + 200.0)
}
/// 参数自检:越界参数在碰硬件之前就拒绝。
fn validate(&self) -> Result<(), String> {
if self.esc_ids.is_empty() {
return Err("--esc-ids 不能为空".into());
}
if self.esc_ids.len() != self.directions.len() {
return Err(format!(
"--esc-ids({} 个)与 --directions({} 个)数量必须一致",
self.esc_ids.len(),
self.directions.len()
));
}
for &id in &self.esc_ids {
if !(1..=8).contains(&id) {
return Err(format!("电调 ID {id} 超出 1..=8"));
}
}
for &direction in &self.directions {
let bits = direction.to_bits();
if bits != 1.0f32.to_bits() && bits != (-1.0f32).to_bits() {
return Err(format!("方向必须是 +1 或 -1,得到 {direction}"));
}
}
if !(self.rpm.is_finite() && self.rpm >= 0.0) {
return Err(format!("--rpm 必须 ≥ 0,得到 {}", self.rpm));
}
if let Some(max) = self.max_rpm
&& !(max.is_finite() && max > self.rpm)
{
return Err(format!("--max-rpm 必须大于目标转速 {},得到 {max}", self.rpm));
}
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
return Err(format!("--gear-ratio 必须 > 0,得到 {}", self.gear_ratio));
}
if !(1..=16384).contains(&self.current_limit) {
return Err(format!(
"--current-limit 必须在 1..=16384,得到 {}",
self.current_limit
));
}
if self.hold_secs < 0.0 {
return Err(format!("--hold-secs 不能为负,得到 {}", self.hold_secs));
}
Ok(())
}
/// 四个电调配置(方向、减速比、电流限幅、PID 全部显式落地)。
fn esc_configs(&self) -> Vec<EscConfig> {
self.esc_ids
.iter()
.enumerate()
.map(|(index, &id)| {
let name = WHEEL_NAMES
.get(index)
.map_or_else(|| format!("wheel{id}"), |name| (*name).to_owned());
let mut cfg = EscConfig::gm3508(name, id, self.directions[index]);
cfg.gear_ratio = self.gear_ratio;
cfg.current_limit = self.current_limit;
cfg
})
.collect()
}
fn control_policy(&self) -> MotorPolicy {
let mut policy = MotorPolicy::new();
let gains = VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward);
for id in &self.esc_ids {
policy.configure(*id, self.current_limit, gains);
}
policy
}
}
fn bus_options() -> BusOptions {
BusOptions {
period: CONTROL_PERIOD,
feedback_timeout: Duration::from_millis(100),
// 命令租约 50ms;控制循环每 1ms 续租。
command_timeout: Duration::from_millis(50),
}
}
pub async fn main_with_args(parameters: Vec<String>) -> ExitCode {
init_tracing();
let args =
Args::parse_from(std::iter::once(String::from("spin-chassis")).chain(parameters));
if let Err(problem) = args.validate() {
eprintln!("✗ 参数错误:{problem}");
return ExitCode::FAILURE;
}
print_banner(&args);
match run(&args).await {
Ok(true) => ExitCode::SUCCESS,
Ok(false) => ExitCode::FAILURE,
Err(e) => {
eprintln!("✗ 运行失败:{e}");
ExitCode::FAILURE
}
}
}
/// 初始化日志:链路桥的读写失败/静默告警必须可见,
/// 否则现场只剩一句"没有反馈",无法区分适配器故障与整车接线问题。
fn init_tracing() {
let filter = tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| {
tracing_subscriber::EnvFilter::new("warn,slcan=debug,socketcan=debug")
});
let _ = tracing_subscriber::fmt().with_env_filter(filter).try_init();
}
fn print_banner(args: &Args) {
println!("=== 台架:四轮同时旋转(转子目标 {:.1} rpm)===", args.rpm);
println!(
"设备 {} | CAN {:?} | 电调 {:?} | 方向 {:?} | 减速比 {:.3}",
args.device, args.can_speed, args.esc_ids, args.directions, args.gear_ratio
);
println!(
"转子 {:.1} rpm ÷ {:.3} = 轮端 {:.2} rpm | 电流限幅 {} raw | 缓升 {}ms | 保持 {:.1}s{}",
args.rpm,
args.gear_ratio,
args.rpm / args.gear_ratio,
args.current_limit,
args.ramp_ms,
args.hold_secs,
if args.dry_run {
" | dry-run"
} else if args.allow_partial {
" | allow-partial"
} else {
""
}
);
if args.dry_run {
println!("ℹ dry-run:只读反馈,不发出非零电流。\n");
} else {
println!(
"速度环 kp={} ki={} kd={} ff={} | 过速保护 {:.0} rpm",
args.kp,
args.ki,
args.kd,
args.feedforward,
args.overspeed_limit()
);
println!("⚠ 轮子必须架空或整车固定,急停可及;ctrl-c 随时安全停机。\n");
}
}
/// 打开链路 → 装配协议 bus → 执行 → **任何路径都先停电流再关总线**。
async fn run(args: &Args) -> Result<bool, Box<dyn std::error::Error>> {
let options = SlcanOptions {
serial_baud: args.serial_baud,
speed: args.can_speed.into(),
// 读写共用该超时(serial crate 的 TTYPort::write 也先 poll(POLLOUT)):
// 设太小会把"这一拍写不出去"误判成写失败。总线繁忙时读立即返回,
// 放大它只影响总线完全静默时的下行延迟。
read_timeout: Duration::from_millis(args.serial_timeout_ms),
};
let link = open_link_with(&args.device, options).map_err(|e| {
format!("打开 slcan 设备 {} 失败:{e}(检查设备路径 / 权限 / 是否被占用)", args.device)
})?;
println!("✓ slcan 已打开:{}(CAN {:?})", args.device, args.can_speed);
let bus = EscBus::spawn(link, args.esc_configs(), bus_options(), args.control_policy())?;
let outcome = drive(args, &bus).await;
// 安全收尾:先 0 电流并让它至少发出一拍,再关总线。
for motor in bus.motors() {
motor.disable();
}
sleep(Duration::from_millis(50)).await;
bus.shutdown().await;
println!("已停机(0 电流)并关闭总线");
outcome
}
async fn drive(
args: &Args,
bus: &EscBusHandle<MotorPolicy>,
) -> Result<bool, Box<dyn std::error::Error>> {
let motors = bus.motors();
wait_for_feedback(motors, Duration::from_millis(args.startup_timeout_ms)).await;
let targets: Vec<EscMotor> = motors.iter().filter(|m| is_fresh(m)).cloned().collect();
let missing: Vec<u8> =
motors.iter().filter(|m| !is_fresh(m)).map(MotorFeedback::id).collect();
if !missing.is_empty() {
eprintln!(
"✗ 以下电调没有新鲜反馈:{missing:?}(收到 {:?})",
motors.iter().map(MotorFeedback::feedback_age).collect::<Vec<_>>()
);
eprintln!(
" 排查:CAN 波特率(C620 = 1Mbps = S8)| CANH/CANL 是否接反 | 120Ω 终端电阻 | 电调供电 | 电调 CAN ID 拨码 | 设备 {}",
args.device
);
if !args.allow_partial {
eprintln!(" 若确认台架只接了部分电调,可加 --allow-partial 降级测试。");
return Ok(false);
}
eprintln!("⚠ --allow-partial:仅驱动有反馈的电调继续测试。\n");
}
if targets.is_empty() {
eprintln!("✗ 没有任何电调在反馈,无法测试。");
return Ok(false);
}
println!(
"✓ 参与测试的电调:{:?}(新鲜度 < {FRESH:?})\n",
targets.iter().map(MotorFeedback::id).collect::<Vec<_>>()
);
if args.dry_run {
return Ok(dry_run(&targets).await);
}
Ok(spin(args, &targets).await)
}
/// 该电调是否有**新鲜**反馈(而非历史上有过反馈)。
fn is_fresh(motor: &EscMotor) -> bool {
motor.feedback_age().is_some_and(|age| age <= FRESH)
}
/// 上电门控:等到每个电调都有一帧**新鲜**反馈;超时即返回 `false`。
async fn wait_for_feedback(motors: &[EscMotor], timeout: Duration) -> bool {
let deadline = Instant::now() + timeout;
loop {
if motors.iter().all(is_fresh) {
return true;
}
if Instant::now() >= deadline {
return false;
}
sleep(Duration::from_millis(20)).await;
}
}
/// dry-run:只观察反馈,不下发任何非零电流(总线仍在发 0 电流帧)。
async fn dry_run(targets: &[EscMotor]) -> bool {
let mut ticker = interval(REPORT_PERIOD);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
let deadline = Instant::now() + Duration::from_secs(3);
while Instant::now() < deadline {
ticker.tick().await;
for motor in targets {
let st = motor.state();
println!(
" {:<11} esc{} | 转子 {:>7.1} rpm | 温度 {:>3}℃ | 反馈 {:>6} 帧 | 新鲜度 {:?}",
motor.name(),
motor.id(),
st.rotor_speed_rpm,
st.temperature_c,
st.feedback_count,
motor.feedback_age().map(|age| age.as_millis())
);
}
println!();
}
println!("✓ 链路与反馈验证完成(未发出非零电流)");
true
}
/// 本次台架的安全包络:只开过速保护(`chassis-spin` 的历史行为)。
fn envelope(args: &Args) -> SafetyEnvelope {
SafetyEnvelope::overspeed_only(args.overspeed_limit(), OVERSPEED_GRACE)
}
/// 缓升 → 保持 → 采样;返回稳态是否达标。
async fn spin(args: &Args, motors: &[EscMotor]) -> bool {
let ramp = Duration::from_millis(args.ramp_ms);
let hold = Duration::from_secs_f32(args.hold_secs);
let overspeed_limit = args.overspeed_limit();
// 设定值曲线与安全判定都来自 chassis-bench 库:与 chassis-tune 的真机
// 后端共用同一套实现,避免"台架测的行为"和"整定优化的行为"分叉。
let scenario = Scenario::ramp_and_hold(args.rpm, ramp.as_secs_f32(), hold.as_secs_f32());
let mut ticker = interval(CONTROL_PERIOD);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
let start = Instant::now();
let mut monitor = SafetyMonitor::new(envelope(args), start);
let mut next_report = REPORT_PERIOD;
let mut samples: Vec<Vec<f32>> = vec![Vec::new(); motors.len()];
let mut overspeed_warned = false;
loop {
tokio::select! {
_ = tokio::signal::ctrl_c() => {
println!("\n收到 ctrl-c,中断测试");
return false;
}
_ = ticker.tick() => {}
}
let elapsed = start.elapsed();
if elapsed >= ramp + hold {
break;
}
// 逐拍重发(= 命令租约续期),斜坡段目标线性上升。
let setpoint = scenario.setpoint_at(elapsed.as_secs_f32());
for motor in motors {
motor.set_velocity(setpoint);
}
// 过速保护:控制器失稳时立刻停机,别让轮子飞到最高转速。
let peak =
motors.iter().map(|m| m.state().rotor_speed_rpm.abs()).fold(0.0f32, f32::max);
if peak > overspeed_limit && !overspeed_warned {
overspeed_warned = true;
eprintln!("⚠ 过速 {peak:.0} rpm > 上限 {overspeed_limit:.0} rpm,观察中……");
}
if let Some(abort) = monitor.check(motors, Instant::now()) {
eprintln!(
"✗ 过速保护触发:{peak:.0} rpm 超过上限 {overspeed_limit:.0} rpm,立即停机({abort})。\n 速度环整定不当(kp/ki/前馈过大)——空载台架前馈应为 0。"
);
return false;
}
if elapsed >= ramp {
for (slot, motor) in samples.iter_mut().zip(motors) {
slot.push(motor.state().rotor_speed_rpm);
}
}
if elapsed >= next_report {
report_line(motors, setpoint, elapsed);
next_report += REPORT_PERIOD;
}
}
summarize(args, motors, &samples)
}
fn report_line(motors: &[EscMotor], setpoint: f32, elapsed: Duration) {
let rotor: Vec<String> =
motors.iter().map(|m| format!("{:>7.1}", m.state().rotor_speed_rpm)).collect();
let current: Vec<String> =
motors.iter().map(|m| format!("{:>6}", m.state().effort_raw)).collect();
let temp: Vec<String> =
motors.iter().map(|m| format!("{:>3}", m.state().temperature_c)).collect();
println!(
"[t={:>5.1}s] 目标 {:>7.1} | 转子 {} | 电流 {} | 温度 {}",
elapsed.as_secs_f32(),
setpoint,
rotor.join(" "),
current.join(" "),
temp.join(" ")
);
}
fn summarize(args: &Args, motors: &[EscMotor], samples: &[Vec<f32>]) -> bool {
if samples.iter().all(Vec::is_empty) {
println!("保持时长为 0,未采集到稳态样本");
return true;
}
println!(
"\n=== 稳态结论(目标转子 {:.1} rpm ≈ 轮端 {:.2} rpm)===",
args.rpm,
args.rpm / args.gear_ratio
);
let mut all_ok = true;
for (index, motor) in motors.iter().enumerate() {
let mean = mean(&samples[index]);
let error = if args.rpm.abs() > f32::EPSILON {
(mean - args.rpm).abs() / args.rpm.abs()
} else {
0.0
};
let ok = error <= TOLERANCE;
all_ok &= ok;
println!(
" {:<11} esc{} | 稳态 {:>7.1} rpm | 轮端 {:>6.2} rpm | 误差 {:>5.1}% | 电流 {:>6} raw | 温度 {:>3}℃ | {}",
motor.name(),
motor.id(),
mean,
mean / args.gear_ratio,
error * 100.0,
motor.state().effort_raw,
motor.state().temperature_c,
if ok { "✓" } else { "✗" }
);
}
if all_ok {
println!("\n✓ 通过:{} 个轮子均达到目标转速 ±{:.0}%", motors.len(), TOLERANCE * 100.0);
} else {
println!(
"\n✗ 未通过:存在轮速偏差 > {:.0}%,检查 PID / 机械阻力 / 电流限幅 / 方向配置",
TOLERANCE * 100.0
);
}
all_ok
}
fn mean(samples: &[f32]) -> f32 {
if samples.is_empty() {
return 0.0;
}
samples.iter().sum::<f32>() / samples.len() as f32
}
}
mod rc_monitor {
//! 只读 DR16 遥控监视器:不打开 CAN,不驱动任何电机。
use std::process::ExitCode;
use std::time::{Duration, Instant};
use clap::Parser;
use dr16::open;
use dr16_protocol::SwitchPosition;
#[derive(Debug, Parser)]
#[command(name = "spin-rc", version, about = "只读监视 DR16/D-BUS 遥控输入")]
struct Args {
/// DR16 D-BUS UART 设备。
#[arg(long, default_value = "/dev/ttyS2")]
device: String,
/// 显示打印周期(毫秒)。
#[arg(long, default_value_t = 100)]
report_ms: u64,
/// 映射显示用死区。
#[arg(long, default_value_t = 0.05)]
deadband: f32,
}
fn shaped(raw: i16, deadband: f32) -> f32 {
let normalized = (f32::from(raw) / 660.0).clamp(-1.0, 1.0);
let magnitude = normalized.abs();
if magnitude <= deadband {
return 0.0;
}
normalized.signum() * ((magnitude - deadband) / (1.0 - deadband)).powi(2)
}
pub async fn main_with_args(parameters: Vec<String>) -> ExitCode {
let args = Args::parse_from(std::iter::once(String::from("spin-rc")).chain(parameters));
let stream = match open(&args.device) {
Ok(stream) => stream,
Err(error) => {
eprintln!("✗ 打开 DR16 串口 {} 失败:{error}", args.device);
return ExitCode::FAILURE;
}
};
println!("=== DR16/D-BUS 只读监视({})===", args.device);
println!("不接触 CAN / 电机;ctrl-c 退出。满量程映射:vx=3.0 vy=3.0 omega=6.0(仅显示)\n");
let mut report = tokio::time::interval(Duration::from_millis(args.report_ms.max(1)));
report.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
loop {
tokio::select! {
_ = tokio::signal::ctrl_c() => {
println!("\n已退出");
return ExitCode::SUCCESS;
}
_ = report.tick() => {
let Some(stamped) = stream.latest() else {
println!("等待 DR16 帧……");
continue;
};
let sample = stamped.sample;
let age = Instant::now().saturating_duration_since(stamped.received_at);
let vx = shaped(sample.channel_t(), args.deadband) * 3.0;
let vy = -shaped(sample.channel_a(), args.deadband) * 3.0;
let omega = -shaped(sample.channel_r(), args.deadband) * 6.0;
println!(
"age {:>4}ms | A {:>4} E {:>4} T {:>4} R {:>4} | S1 {:?}({}) S2 {:?}({}) | vx {:+.2} vy {:+.2} omega {:+.2}",
age.as_millis(),
sample.channel_a(),
sample.channel_e(),
sample.channel_t(),
sample.channel_r(),
sample.s1,
sample.s1.wire_value(),
sample.s2,
sample.s2.wire_value(),
vx,
vy,
omega,
);
if sample.s1 != SwitchPosition::Up {
println!(" S1 非上档:底盘使能关闭");
}
}
}
}
}
}
mod motor_one {
//! 单电机验证工具:让一个 C620 / GM3508 转起来,验证电机抽象全链路。
//!
//! ```sh
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
//! # 例:can0 上电调 1 以 1000 rpm(转子)转 10 秒
//! cargo run --release -p bench --bin socketcan-spin -- can0 1 1000 10
//! ```
//!
//! 关键点在 `spin_one`:它只依赖 `motor::Motor` 抽象,完全不知道
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机,这段代码
//! 原样可用。抽象(`can / imu`)、协议实现(`motor-protocol`)、设备层(`motor / esc-bus`)与总线后端(`driver crates`)在此汇合。
//!
//! 仅 Linux(需要 SocketCAN);其他平台编译为报错桩。
#[cfg(target_os = "linux")]
use std::time::Duration;
#[cfg(target_os = "linux")]
use esc_bus::{BusOptions, EscBus, EscConfig};
#[cfg(target_os = "linux")]
use motor::Motor;
#[cfg(target_os = "linux")]
use motor_control::{MotorPolicy, VelocityGains};
#[cfg(target_os = "linux")]
use socketcan::open_link;
#[cfg(target_os = "linux")]
use tokio::time::{MissedTickBehavior, interval};
#[cfg(target_os = "linux")]
pub async fn main_with_args(argv: Vec<String>) {
let mut args = argv.into_iter();
let interface = args.next().unwrap_or_else(|| "can0".into());
let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
println!("=== 单电机抽象验证 ===");
println!(
"接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm(转子)| 时长 {duration_secs}s"
);
let link = open_link(&interface).unwrap_or_else(|e| {
eprintln!(
"打开 {interface} 失败:{e}(先 ip link set {interface} up type can bitrate 1000000)"
);
std::process::exit(1);
});
let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
println!(
"电机配置:{:?} | 减速比 {:.3} | 电流限幅 {}",
config.model, config.gear_ratio, config.current_limit
);
let mut policy = MotorPolicy::new();
policy.configure(esc_id, config.current_limit, VelocityGains::new(10.0, 0.5, 0.0, 1.5));
let bus = EscBus::spawn(link, vec![config], BusOptions::default(), policy)
.expect("EscBus 配置非法");
let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
bus.shutdown().await;
println!("已退出");
}
/// 验证目标:本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
/// 它不知道 CAN,不知道 C620,换任何 Motor 实现都原样工作。
///
/// 注意**命令租约**:`BusOptions::command_timeout` 默认 50ms,超过该时长未重发
/// 命令,驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值
/// (真实底盘的控制循环每 1ms 重发一次)。
#[cfg(target_os = "linux")]
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
const LEASE_PERIOD: Duration = Duration::from_millis(10);
const REPORT_PERIOD: Duration = Duration::from_millis(100);
motor.set_velocity(target_rpm);
println!("已下发目标转速,开始观测(100ms 周期)……\n");
let mut lease = interval(LEASE_PERIOD);
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
let mut report = interval(REPORT_PERIOD);
report.set_missed_tick_behavior(MissedTickBehavior::Skip);
let deadline = tokio::time::Instant::now() + duration;
loop {
tokio::select! {
_ = tokio::signal::ctrl_c() => {
println!("\n收到 ctrl-c");
break;
}
_ = lease.tick() => motor.set_velocity(target_rpm), // 续租
_ = report.tick() => {
let st = motor.state();
if st.has_feedback() {
println!(
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
st.rotor_speed_rpm,
st.effort_raw,
st.temperature_c,
st.rotor_revolutions,
st.feedback_count
);
} else {
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
}
}
}
if tokio::time::Instant::now() >= deadline {
break;
}
}
motor.disable();
// 让 0 电流帧至少发出一拍再关总线。
tokio::time::sleep(Duration::from_millis(50)).await;
println!("已停机(0 电流)");
}
#[cfg(not(target_os = "linux"))]
#[allow(clippy::unused_async)]
pub async fn main_with_args(_argv: Vec<String>) {
eprintln!("spin motor 仅支持 Linux(需要 SocketCAN)");
}
}
#[tokio::main]
async fn main() -> std::process::ExitCode {
let mut args = std::env::args().skip(1);
match args.next().as_deref() {
Some("chassis") => chassis::main_with_args(args.collect()).await,
Some("motor") => {
motor_one::main_with_args(args.collect()).await;
std::process::ExitCode::SUCCESS
}
Some("rc") => rc_monitor::main_with_args(args.collect()).await,
Some("--help" | "-h") => {
println!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::SUCCESS
}
_ => {
eprintln!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::from(2)
}
}
}
+50
View File
@@ -0,0 +1,50 @@
[package]
name = "chassis-tune"
description = "Automated chassis velocity-loop tuning tool"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
[lints]
workspace = true
[features]
plot = ['dep:plotters']
[dependencies]
bench = { workspace = true }
can = { workspace = true }
chassis-config = { workspace = true }
experiment = { workspace = true }
motor = { workspace = true }
esc-bus = { workspace = true }
motor-protocol = { workspace = true }
motor-control = { workspace = true }
motor-model = { workspace = true }
sim-harness = { workspace = true }
tuning = { workspace = true }
socketcan = { workspace = true }
slcan = { workspace = true }
tokio = { workspace = true }
thiserror = { workspace = true }
tracing = { workspace = true }
tracing-subscriber = { workspace = true }
clap = { workspace = true }
serde = { workspace = true }
serde_json = { workspace = true }
toml = { workspace = true }
csv = { workspace = true }
indicatif = { workspace = true }
rayon = { workspace = true }
rand = { workspace = true }
argmin = { workspace = true }
argmin-math = { workspace = true }
anyhow = { workspace = true }
plotters = { workspace = true, optional = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/tuning"
File diff suppressed because it is too large Load Diff