feat: 添加了自动超参数整定
This commit is contained in:
Horldsence
@@ -0,0 +1,32 @@
|
||||
[package]
|
||||
name = "chassis-bench"
|
||||
description = "Bench harness: stimulus, safety envelope, trial sampling, and the bench binaries"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
# 二进制从 `src/bin/` 自动发现：
|
||||
# - `chassis-spin`：四轮定速旋转（slcan，全平台）
|
||||
# - `socketcan-spin`：单电机验证（SocketCAN，仅 Linux）
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
dji-esc-slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
dji-esc-socketcan = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,554 @@
|
||||
//! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 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 电流再关总线）；未收到全部反馈前绝不下发速度。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::process::ExitCode;
|
||||
use std::time::Duration;
|
||||
|
||||
use chassis_bench::{SafetyEnvelope, SafetyMonitor, Scenario};
|
||||
use clap::{Parser, ValueEnum};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_slcan::{CanSpeed, SlcanOptions, open_link_with};
|
||||
use motor_core::{Disable, MotorFeedback, VelocityDrive};
|
||||
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.kp = self.kp;
|
||||
cfg.ki = self.ki;
|
||||
cfg.kd = self.kd;
|
||||
cfg.feedforward = self.feedforward;
|
||||
cfg
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
fn bus_options() -> BusOptions {
|
||||
BusOptions {
|
||||
period: CONTROL_PERIOD,
|
||||
feedback_timeout: Duration::from_millis(100),
|
||||
// 命令租约 50ms；控制循环每 1ms 续租。
|
||||
command_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> ExitCode {
|
||||
init_tracing();
|
||||
let args = Args::parse();
|
||||
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,dji_esc_slcan=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())?;
|
||||
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) -> 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
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||||
//!
|
||||
//! ```sh
|
||||
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||||
//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10
|
||||
//! ```
|
||||
//!
|
||||
//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。
|
||||
//!
|
||||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use std::time::Duration;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc::{BusOptions, EscBus, EscConfig};
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc_socketcan::open_link;
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor_core::Motor;
|
||||
#[cfg(target_os = "linux")]
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
#[tokio::main]
|
||||
async fn main() {
|
||||
let mut args = std::env::args().skip(1);
|
||||
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 bus = EscBus::spawn(link, vec![config], BusOptions::default()).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"))]
|
||||
fn main() {
|
||||
eprintln!("spin 仅支持 Linux（需要 SocketCAN）");
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
//! 台架工具库：激励场景、安全包络与真机试验采样。
|
||||
//!
|
||||
//! 本 crate 是**台架相关一切的唯一归属**：以前 `chassis-spin` 是
|
||||
//! `dji-esc-slcan` 的一个 example、`spin` 是 `dji-esc-socketcan` 的一个
|
||||
//! example——运输后端 crate 却装着应用级工具。现在：
|
||||
//!
|
||||
//! - 运输 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接；
|
||||
//! - 台架程序与可复用的安全 / 采样逻辑住在这里；
|
||||
//! - `chassis-tune` 的真机后端复用 [`sampler`] 与 [`safety`]，不重复实现。
|
||||
//!
|
||||
//! 二进制：
|
||||
//!
|
||||
//! - `chassis-spin`：四轮定速旋转（slcan，全平台）；
|
||||
//! - `socketcan-spin`：单电机链路验证（SocketCAN，仅 Linux）；
|
||||
//! - [`link`]：两种运输后端的统一门面（台架与真机整定共用）。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod link;
|
||||
pub mod safety;
|
||||
pub mod sampler;
|
||||
|
||||
pub use link::{CanSpeed, SlcanOptions, default_slcan_options, open_slcan, open_socketcan};
|
||||
pub use safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
pub use sampler::{TrialOutcome, run_step_trial, run_step_trial_until_ctrl_c};
|
||||
|
||||
/// 与离线 harness 共用的场景 / 轨迹类型（同一套实验词汇）。
|
||||
pub use dji_esc_sim::{Scenario, SetpointProfile, Trace};
|
||||
@@ -0,0 +1,87 @@
|
||||
//! 链路打开：把两种运输后端收敛成一个门面。
|
||||
//!
|
||||
//! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`CanLink`]"，
|
||||
//! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处，上层就不必
|
||||
//! 同时依赖两个运输 crate。
|
||||
|
||||
use std::io::Result;
|
||||
use std::time::Duration;
|
||||
|
||||
use can_link::CanLink;
|
||||
|
||||
/// slcan 波特率档位（对应协议 `Sn` 命令）。
|
||||
pub use dji_esc_slcan::CanSpeed;
|
||||
/// slcan 打开参数。
|
||||
pub use dji_esc_slcan::SlcanOptions;
|
||||
|
||||
/// 打开 slcan 串口设备（CANable 等），桥接为链路端点。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。
|
||||
pub fn open_slcan(device: &str, options: SlcanOptions) -> Result<CanLink> {
|
||||
dji_esc_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> {
|
||||
dji_esc_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));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,322 @@
|
||||
//! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。
|
||||
//!
|
||||
//! 台架工具（`chassis-spin`）与自动整定的真机后端（`chassis-tune --backend bench`）
|
||||
//! 共用同一套判定，避免安全逻辑在两个地方各写一遍、然后慢慢漂移。
|
||||
//!
|
||||
//! 所有阈值都是 `Option`：只开需要的那个。`chassis-spin` 只开过速保护
|
||||
//! （保持历史行为），自动整定则开启全部——因为搜索会主动试错，必须由
|
||||
//! 包络兜住"试错到失控"。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use motor_core::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_core::{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:?}");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,239 @@
|
||||
//! 真机试验：把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。
|
||||
//!
|
||||
//! 与 `dji-esc-sim` 的离线 harness 是**同一套场景词汇**：整定工具先用离线
|
||||
//! harness 搜索，再用本模块在台架上复核，两条路径的指标口径因此一致。
|
||||
//!
|
||||
//! # 安全契约
|
||||
//!
|
||||
//! - 每个采样拍都重发设定值——**命令是租约**，不重发会在 50ms 后失电；
|
||||
//! - 每个采样拍都过一遍 [`SafetyMonitor`]；
|
||||
//! - 无论正常结束、触发包络还是被取消，退出前一律 `disable()`（0 电流）。
|
||||
|
||||
use std::future::Future;
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use motor_core::{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_core::{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, "取消后不得继续采样");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user