diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4688c81..cae40c3 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -28,6 +28,19 @@ jobs: env: RUSTDOCFLAGS: -D warnings run: cargo doc --workspace --no-deps + # 整定工具的可选 feature 不在默认 clippy/test 里，单独编译一次防止腐烂。 + - name: chassis-tune plot feature + run: cargo check -p chassis-tune --features plot --all-targets + # 自动整定的确定性冒烟：同 seed 必须逐位一致，且最优解必须通过验收阈值。 + - name: tune smoke (deterministic, no hardware) + run: | + cargo run --release -p chassis-tune -- sweep \ + --samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \ + --out-dir target/tune-smoke + cargo run --release -p chassis-tune -- sweep \ + --samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \ + --out-dir target/tune-smoke-again + diff target/tune-smoke/trials.csv target/tune-smoke-again/trials.csv vcan-integration: name: vcan0 integration (real kernel CAN stack) @@ -65,6 +78,9 @@ jobs: targets: armv7-unknown-linux-musleabihf - uses: Swatinem/rust-cache@v2 - run: cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf + # 台架工具与整定工具同样要在板端跑（真机后端常直接跑在 T153 上）。 + - run: cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf + - run: cargo build --release -p chassis-tune --target armv7-unknown-linux-musleabihf coverage: name: coverage (llvm-cov) diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md new file mode 100644 index 0000000..118a8f8 --- /dev/null +++ b/ARCHITECTURE.md @@ -0,0 +1,1172 @@ +# RoboMaster CAN 控制系统架构文档 + +**版本**: 1.0 +**目标平台**: Allwinner T153 (Cortex-A7 @ 1.6GHz, Linux) +**控制频率**: 500Hz +**文档目的**: 指导执行 Agent 完成项目拆分、结构整理与模块设计 + +--- + +## 目录 + +1. [系统概述](#1-系统概述) +2. [硬件拓扑](#2-硬件拓扑) +3. [架构原则](#3-架构原则) +4. [Crate 拆分](#4-crate-拆分) +5. [各 Crate 详细设计](#5-各-crate-详细设计) +6. [数据流与任务模型](#6-数据流与任务模型) +7. [实时性设计](#7-实时性设计) +8. [仿真与自动整定](#8-仿真与自动整定) +9. [测试策略](#9-测试策略) +10. [部署与配置](#10-部署与配置) +11. [执行 Agent 工作流](#11-执行-agent-工作流) +12. [附录](#12-附录) + +--- + +## 1. 系统概述 + +### 1.1 目标 + +构建一个运行在 T153 Linux 平台上的 RoboMaster 机器人控制系统，实现： + +- 对 **9 个 CAN 电机**（5 个原生 CAN + 4 个 SLCAN）的 500Hz 闭环控制 +- 对 **delta10 IMU** 的姿态数据采集（通过 CAN 连接） +- 对机架不同部分的**完整功能抽象**（云台、发射机构、底盘） +- 支持**仿真环境下的自动 PID 整定**，并将参数迁移到实机 +- 保证每一层的**可维护性、可验证性、可移植性** + +### 1.2 关键约束 + +| 约束 | 值 | 影响 | +|------|-----|------| +| 控制频率 | 500Hz (2ms) | 任务预算分配、调度策略 | +| 平台 | T153 Cortex-A7 Linux | 非硬实时，需 SCHED_FIFO | +| CAN 接口 | 2 原生 + 1 SLCAN | 三种不同的访问路径 | +| IMU 接口 | CAN | 与电机共享 can0 总线 | +| 仿真目标 | 纯算法验证 | 不需要全系统数字孪生 | + +--- + +## 2. 硬件拓扑 + +### 2.1 物理连接图 + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ T153 (Linux, Cortex-A7) │ +│ │ +│ ┌──────────────┐ ┌──────────────┐ ┌──────────────────┐ │ +│ │ CAN0 (原生) │ │ CAN1 (原生) │ │ USB CDC │ │ +│ │ 1Mbps │ │ 1Mbps │ │ /dev/ttyACM0 │ │ +│ └──────┬───────┘ └──────┬───────┘ └────────┬─────────┘ │ +└─────────┼──────────────────┼────────────────────┼──────────────┘ + │ │ │ + │ │ │ (SLCAN 协议) + │ │ ▼ + │ │ ┌──────────────────┐ + │ │ │ candlelite2 │ + │ │ │ (slcan 固件) │ + │ │ └────────┬─────────┘ + │ │ │ + ▼ ▼ ▼ +┌──────────────────┐ ┌──────────┐ ┌───────────────────────┐ +│ can0 总线 │ │can1 总线 │ │ SLCAN CAN 总线 │ +│ │ │ │ │ │ +│ ├─ GM6020 (yaw) │ │ ├─ M2006 │ │ ├─ M3508 (底盘 FL) │ +│ ├─ GM6020(pitch)│ │ │ (拨弹) │ │ ├─ M3508 (底盘 FR) │ +│ ├─ M3508 (摩擦L)│ │ │ │ │ ├─ M3508 (底盘 RL) │ +│ ├─ M3508 (摩擦R)│ │ │ │ │ └─ M3508 (底盘 RR) │ +│ └─ delta10 IMU │ │ │ │ │ +└──────────────────┘ └──────────┘ └───────────────────────┘ +``` + +### 2.2 设备清单 + +| 设备 | 数量 | 总线 | 协议 | 控制帧 ID | 反馈帧 ID | +|------|------|------|------|-----------|-----------| +| GM6020 (yaw) | 1 | can0 | 标准帧 | 0x1FF 或 0x2FF | 0x205-0x208 | +| GM6020 (pitch) | 1 | can0 | 标准帧 | 0x1FF 或 0x2FF | 0x205-0x208 | +| M3508 (摩擦轮) | 2 | can0 | 标准帧 | 0x200 或 0x1FF | 0x201-0x204 | +| M2006 (拨弹轮) | 1 | can1 | 标准帧 | 0x200 或 0x1FF | 0x201-0x204 | +| M3508 (底盘) | 4 | SLCAN | 标准帧 | 0x200 | 0x201-0x204 | +| delta10 IMU | 1 | can0 | FDILINK/标准 | TBD | TBD | + +**注意**：GM6020 的 ID 分配需在初始化时固定，避免与 M3508 冲突。建议 GM6020 使用 0x205-0x206（对应 ID 1-2），M3508 摩擦轮使用 0x201-0x202（对应 ID 1-2），预留 ID 3-4。 + +### 2.3 总线负载估算 + +| 总线 | 设备数 | 帧/秒（500Hz） | 1Mbps 负载 | +|------|--------|----------------|-----------| +| can0 | 4 电机 + delta10 | ~5000 | ~12% | +| can1 | 1 电机 | ~1000 | ~2% | +| SLCAN | 4 电机 | ~4000 | USB CDC 带宽充裕 | + +--- + +## 3. 架构原则 + +### 3.1 分层原则 + +系统分为 **7 层**，每层只依赖其下层，不允许跨层依赖： + +``` +┌─────────────────────────────────────────┐ +│ L7: rm-app 应用层（任务编排） │ +├─────────────────────────────────────────┤ +│ L6: rm-sim 仿真层（纯算法验证） │ +├─────────────────────────────────────────┤ +│ L5: rm-control 控制层（PID/自整定） │ +├─────────────────────────────────────────┤ +│ L4: rm-devices 设备抽象层（Motor） │ +├─────────────────────────────────────────┤ +│ L3: rm-drivers 驱动层（SocketCAN/SLCAN）│ +├─────────────────────────────────────────┤ +│ L2: rm-protocol 协议层（帧编解码） │ +├─────────────────────────────────────────┤ +│ L1: rm-hal 硬件抽象层（trait） │ +└─────────────────────────────────────────┘ +``` + +**依赖方向**：L7 → L6 → L5 → L4 → L3 → L2 → L1。不允许反向依赖，不允许跳层依赖（如 L7 直接使用 L2）。 + +### 3.2 Rust 特性使用原则 + +| 特性 | 使用场景 | 示例 | +|------|---------|------| +| `const` 泛型 | 编译期 ID/通道校验 | `Gm6020Id` | +| `newtype` | 类型安全的值封装 | `Current(i16)`、`Angle(f32)` | +| `async fn in trait` | 异步接口抽象 | `CanBus::send` | +| `PhantomData` | 标记通道归属 | `SocketCanBus` | +| `enum` + `match` | 电机类型的 ad-hoc 多态 | `MotorKind` | +| `#[derive]` | 序列化/调试 | `zerocopy::FromBytes` | +| `no_std` | 纯算法层 | `rm-control`、`rm-protocol` | + +### 3.3 单一职责原则 + +- **一个 crate 一个职责**：`rm-protocol` 只做编解码，不含 IO +- **一个模块一个概念**：`rm-devices/motor/gm6020.rs` 只处理 GM6020 +- **一个函数一个动作**：`send_current` 只负责发送，不负责校验以外的逻辑 + +### 3.4 可移植性原则 + +- **硬件无关性**：`rm-control` 和 `rm-protocol` 不含任何平台相关代码 +- **trait 边界**：所有硬件访问通过 `rm-hal` 的 trait +- **仿真对等**：`SimPlant` 和 `RealPlant` 实现同一 trait，可无缝切换 + +--- + +## 4. Crate 拆分 + +### 4.1 Workspace 结构 + +``` +rm-can-robot/ +├── Cargo.toml # workspace 根 +├── rust-toolchain.toml # 工具链固定 +├── .cargo/ +│ └── config.toml # 交叉编译配置 +├── crates/ +│ ├── rm-hal/ # L1: 硬件抽象 +│ ├── rm-protocol/ # L2: 协议编解码 +│ ├── rm-drivers/ # L3: 驱动实现 +│ ├── rm-devices/ # L4: 设备抽象 +│ ├── rm-control/ # L5: 控制算法 +│ ├── rm-sim/ # L6: 仿真 +│ └── rm-app/ # L7: 应用 +├── config/ # 运行时配置 (TOML) +│ ├── robot.toml # 机器人拓扑 +│ ├── motors.toml # 电机参数 +│ └── pid.toml # PID 参数 +├── scripts/ # 部署与调试脚本 +│ ├── setup_can.sh # CAN 接口初始化 +│ ├── udev_rules.sh # 串口权限 +│ └── deploy.sh # 交叉编译 + 部署 +├── tests/ # 集成测试 +│ ├── vcan_loopback.rs # 虚拟 CAN 回环 +│ └── sim_pid_tune.rs # 仿真整定测试 +└── docs/ # 架构文档 + ├── ARCHITECTURE.md # 本文档 + ├── PROTOCOL.md # CAN 协议说明 + └── TUNING.md # 整定指南 +``` + +### 4.2 Workspace Cargo.toml + +```toml +[workspace] +resolver = "2" +members = [ + "crates/rm-hal", + "crates/rm-protocol", + "crates/rm-drivers", + "crates/rm-devices", + "crates/rm-control", + "crates/rm-sim", + "crates/rm-app", +] + +[workspace.package] +version = "0.1.0" +edition = "2021" +rust-version = "1.75" +license = "MIT" + +[workspace.dependencies] +# 异步 +tokio = { version = "1", features = ["rt-multi-thread", "time", "sync", "macros"] } + +# CAN +socketcan = { version = "3.6", features = ["tokio"] } +slcan_fd = { version = "0.1", features = ["tokio"] } +tokio-serial = "5" + +# 控制 +discrete_pid = "0.2" +nalgebra = { version = "0.33", default-features = false, features = ["libm"] } + +# 实时 +libc = "0.2" + +# 序列化 +zerocopy = "0.8" +serde = { version = "1", features = ["derive"] } +toml = "0.8" + +# 日志 +tracing = "0.1" +tracing-subscriber = "0.3" + +# 错误处理 +thiserror = "1" +anyhow = "1" + +# 测试 +proptest = "1" +``` + +--- + +## 5. 各 Crate 详细设计 + +### 5.1 `rm-hal` — 硬件抽象层 + +**职责**：定义与硬件无关的 trait，作为所有硬件访问的抽象边界。 + +**依赖**：无（`no_std` 兼容） + +**文件结构**： + +``` +rm-hal/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── can.rs # CanBus trait + CanFrame + ├── imu.rs # Imu trait + ├── time.rs # 时间抽象 + └── error.rs # 错误类型 +``` + +**核心 trait**： + +```rust +// can.rs +#[derive(Clone, Debug)] +pub struct CanFrame { + pub id: u32, + pub data: [u8; 8], + pub dlc: u8, + pub is_extended: bool, +} + +pub trait CanBus { + type Error; + async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error>; + async fn recv(&mut self) -> Result; + fn try_recv(&mut self) -> Result, Self::Error>; +} + +// imu.rs +pub struct ImuData { + pub accel: [f32; 3], + pub gyro: [f32; 3], + pub mag: [f32; 3], + pub roll: f32, + pub pitch: f32, + pub yaw: f32, + pub timestamp_ns: u64, +} + +pub trait Imu { + type Error; + async fn read(&mut self) -> Result; +} +``` + +**验收标准**： +- [ ] 所有 trait 不依赖任何具体硬件 +- [ ] `no_std` 下编译通过（`cargo build --no-default-features`） +- [ ] 有完整的文档注释 + +--- + +### 5.2 `rm-protocol` — 协议编解码层 + +**职责**：CAN 帧的编解码，纯逻辑，无 IO。 + +**依赖**：`rm-hal`（仅用于 `CanFrame` 类型）、`zerocopy` + +**文件结构**： + +``` +rm-protocol/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── frame.rs # CanFrame 扩展 + ├── motor/ + │ ├── mod.rs + │ ├── gm6020.rs # GM6020 编解码 + │ ├── m3508.rs # M3508 编解码 + │ ├── m2006.rs # M2006 编解码 + │ └── common.rs # 电机反馈结构 + ├── delta10.rs # delta10 编解码 + └── id.rs # CAN ID 编译期校验 +``` + +**核心设计**： + +```rust +// id.rs — 编译期 ID 校验 +pub struct MotorId; + +impl MotorId { + pub const NEW: Self = { + assert!(ID >= 1 && ID <= 8, "Motor ID must be 1-8"); + Self + }; +} + +// motor/gm6020.rs +#[derive(Debug, Clone, Copy)] +pub struct Gm6020Feedback { + pub angle: f32, // 0-360 度 + pub speed: f32, // rpm + pub current: i16, // 原始电流值 + pub temperature: u8, +} + +#[derive(Debug, Clone, Copy)] +pub struct Gm6020Command { + pub current: i16, // -30000 ~ 30000 +} + +impl Gm6020Command { + pub fn to_frame(&self, id: MotorId) -> CanFrame { + // 编码到 0x1FF 或 0x2FF 广播帧 + } +} + +pub fn decode_feedback(frame: &CanFrame) -> Option<(u8, Gm6020Feedback)> { + // 从 0x205-0x208 解码 +} +``` + +**验收标准**： +- [ ] 所有电机的编解码有单元测试 +- [ ] 使用 `proptest` 做模糊测试 +- [ ] `no_std` 兼容 +- [ ] 无任何 IO 操作 + +--- + +### 5.3 `rm-drivers` — 驱动层 + +**职责**：实现 `rm-hal` 的 trait，对接具体硬件。 + +**依赖**：`rm-hal`、`rm-protocol`、`socketcan`、`slcan_fd`、`tokio-serial` + +**文件结构**： + +``` +rm-drivers/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── socketcan.rs # SocketCAN 实现 (can0, can1) + ├── slcan.rs # SLCAN 实现 (candlelite2) + ├── delta10.rs # delta10 IMU 驱动 + └── mock.rs # 测试用 mock（feature-gated） +``` + +**核心实现**： + +```rust +// socketcan.rs +pub struct SocketCanBus { + socket: socketcan::tokio::CanSocket, +} + +impl CanBus for SocketCanBus { + type Error = socketcan::Error; + + async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> { + let sock_frame = to_socketcan_frame(frame); + self.socket.write_frame(&sock_frame).await + } + + async fn recv(&mut self) -> Result { + let f = self.socket.read_frame().await?; + Ok(from_socketcan_frame(f)) + } +} + +// slcan.rs +pub struct SlcanBus { + socket: slcan_fd::tokio::CanSocket, +} + +impl CanBus for SlcanBus { + // 类似实现，但底层使用 SLCAN 协议 +} + +// delta10.rs +pub struct Delta10Driver { + bus: B, + can_id: u16, + last_data: Option, +} + +impl Imu for Delta10Driver { + async fn read(&mut self) -> Result { + // 等待 delta10 帧，解码 FDILINK/标准协议 + } +} +``` + +**验收标准**： +- [ ] 在 T153 上能成功打开 `can0`、`can1`、SLCAN 串口 +- [ ] 能用 `candump` 验证帧收发 +- [ ] 有 `mock` feature 用于测试 + +--- + +### 5.4 `rm-devices` — 设备抽象层 + +**职责**：将协议层的数据封装为有状态的电机设备，提供统一的 `Motor` trait。 + +**依赖**：`rm-hal`、`rm-protocol` + +**文件结构**： + +``` +rm-devices/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── traits.rs # Motor trait + ├── gm6020.rs + ├── m3508.rs + ├── m2006.rs + └── registry.rs # 电机注册表（管理所有电机） +``` + +**核心设计**： + +```rust +// traits.rs +pub trait Motor { + type Feedback; + type Command; + + fn id(&self) -> u8; + fn set_command(&mut self, cmd: Self::Command) -> Result<(), MotorError>; + fn feedback(&self) -> Option<&Self::Feedback>; + fn update_feedback(&mut self, frame: &CanFrame) -> Result<(), MotorError>; + fn is_online(&self, now: Instant) -> bool; + fn latency_hint(&self) -> Duration; // SLCAN 返回较大值 +} + +// registry.rs — 统一管理所有电机 +pub struct MotorRegistry { + gm6020: [Gm6020; 2], + m3508_arm: [M3508; 2], + m3508_chassis: [M3508; 4], + m2006: M2006, +} + +impl MotorRegistry { + pub fn update_from_frame(&mut self, bus: BusId, frame: &CanFrame) { + // 根据总线 ID 和帧 ID 分发到对应电机 + } +} +``` + +**验收标准**： +- [ ] 每个电机类型有独立的单元测试 +- [ ] `MotorRegistry` 能正确分发帧 +- [ ] 在线检测逻辑正确（超时阈值可配置） + +--- + +### 5.5 `rm-control` — 控制算法层 + +**职责**：PID 控制、串级控制、前馈补偿、自动整定。纯算法，无 IO。 + +**依赖**：`discrete_pid`、`nalgebra`（可选） + +**文件结构**： + +``` +rm-control/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── pid.rs # PID 封装（基于 discrete_pid） + ├── cascade.rs # 串级控制 + ├── feedforward.rs # 前馈补偿 + ├── plant.rs # ControlPlant trait + └── tuner/ + ├── mod.rs + ├── relay.rs # Relay feedback 整定 + └── zn.rs # Ziegler-Nichols 公式 +``` + +**核心设计**： + +```rust +// plant.rs — 抽象被控对象，使仿真和实机共用 PID +pub trait ControlPlant { + fn step(&mut self, input: f32, dt: f32) -> f32; // 返回测量输出 + fn reset(&mut self); +} + +// pid.rs +pub struct PidController { + inner: discrete_pid::Pid, + output_limit: (f32, f32), + integral_limit: (f32, f32), +} + +impl PidController { + pub fn compute(&mut self, setpoint: f32, measured: f32, dt: f32) -> f32 { + let raw = self.inner.compute(setpoint, measured, dt); + raw.clamp(self.output_limit.0, self.output_limit.1) + } +} + +// cascade.rs — 位置-速度串级 +pub struct CascadeController { + outer: PidController, // 位置环 + inner: PidController, // 速度环 +} + +impl CascadeController { + pub fn compute(&mut self, pos_sp: f32, pos_fb: f32, vel_fb: f32, dt: f32) -> f32 { + let vel_sp = self.outer.compute(pos_sp, pos_fb, dt); + self.inner.compute(vel_sp, vel_fb, dt) + } +} + +// tuner/relay.rs +pub struct RelayTuner { + amplitude: f32, + hysteresis: f32, + state: RelayState, + measurements: Vec, +} + +pub struct OscillationPoint { + pub peak: f32, + pub trough: f32, + pub period: f32, +} + +impl RelayTuner { + pub fn step(&mut self, measured: f32, dt: f32) -> f32 { + // 返回 relay 输出，记录振荡点 + } + + pub fn result(&self) -> Option { + // Ziegler-Nichols 计算 Kp, Ki, Kd + } +} +``` + +**验收标准**： +- [ ] PID 在阶跃输入下无超调（可配置） +- [ ] 抗积分饱和逻辑正确 +- [ ] Relay tuner 在标准二阶系统上能收敛 +- [ ] `no_std` 兼容 + +--- + +### 5.6 `rm-sim` — 仿真层 + +**职责**：提供电机模型、总线模型、测试场景，用于算法验证和自动整定。 + +**依赖**：`rm-control`、`rm-protocol` + +**文件结构**： + +``` +rm-sim/ +├── Cargo.toml +└── src/ + ├── lib.rs + ├── motor_model/ + │ ├── mod.rs + │ ├── dc_motor.rs # 通用直流电机模型 + │ ├── gm6020.rs # GM6020 参数化 + │ ├── m3508.rs # M3508 参数化 + │ └── m2006.rs # M2006 参数化 + ├── bus_model.rs # CAN 总线模型（延迟/丢帧） + ├── plant.rs # SimPlant 实现 + ├── scenario.rs # 测试场景 + └── tuner_runner.rs # 自动整定流程 +``` + +**核心设计**： + +```rust +// motor_model/dc_motor.rs +pub struct DcMotorParams { + pub resistance: f32, // 欧姆 + pub inductance: f32, // 亨利 + pub torque_constant: f32, // N·m/A + pub back_emf_constant: f32, // V·s/rad + pub gear_ratio: f32, + pub rotor_inertia: f32, // kg·m² + pub current_loop_bw: f32, // 电流环带宽 Hz +} + +pub struct DcMotorModel { + params: DcMotorParams, + state: MotorState, +} + +impl DcMotorModel { + pub fn step(&mut self, current_cmd: f32, dt: f32) -> MotorOutput { + // 电流环一阶滞后 → 转矩 → 加速度 → 速度 → 位置 + } +} + +// plant.rs — 仿真 plant 实现 ControlPlant +pub struct SimPlant { + model: M, + noise: Option, // 编码器噪声 +} + +impl ControlPlant for SimPlant { + fn step(&mut self, input: f32, dt: f32) -> f32 { + let out = self.model.step(input, dt); + self.noise.map(|n| n.apply(out.position)).unwrap_or(out.position) + } + fn reset(&mut self) { self.model.reset(); } +} + +// tuner_runner.rs +pub async fn auto_tune( + plant: &mut P, + config: TuneConfig, +) -> TuneResult { + let mut tuner = RelayTuner::new(config.amplitude, config.hysteresis); + let mut t = 0.0; + while t < config.max_time { + let output = tuner.step(plant.step(tuner.last_output(), config.dt), config.dt); + // ... + t += config.dt; + } + tuner.result().expect("Tuning failed") +} +``` + +**验收标准**： +- [ ] 电机模型在阶跃输入下的响应符合物理直觉 +- [ ] 仿真整定出的 PID 在实机上只需 ±20% 微调 +- [ ] 支持批量扫描不同参数 + +--- + +### 5.7 `rm-app` — 应用层 + +**职责**：任务编排、配置加载、实时调度、主循环。 + +**依赖**：所有下层 crate + +**文件结构**： + +``` +rm-app/ +├── Cargo.toml +└── src/ + ├── main.rs + ├── config.rs # TOML 配置加载 + ├── realtime.rs # SCHED_FIFO + mlockall + ├── tasks/ + │ ├── mod.rs + │ ├── gimbal.rs # 云台任务 (can0) + │ ├── shooter.rs # 发射机构任务 (can0 + can1) + │ └── chassis.rs # 底盘任务 (SLCAN) + ├── messages.rs # 任务间通信消息 + └── telemetry.rs # 诊断输出 +``` + +**任务模型**： + +```rust +// main.rs +#[tokio::main] +async fn main() -> anyhow::Result<()> { + // 1. 实时调度 + realtime::enable(80)?; + + // 2. 加载配置 + let config = config::load("config/robot.toml")?; + + // 3. 打开 CAN 总线 + let can0 = SocketCanBus::open("can0")?; + let can1 = SocketCanBus::open("can1")?; + let slcan = SlcanBus::open("/dev/ttyACM0")?; + + // 4. 启动任务 + let gimbal = tokio::spawn(tasks::gimbal::run(can0, config.gimbal)); + let shooter = tokio::spawn(tasks::shooter::run(can1, config.shooter)); + let chassis = tokio::spawn(tasks::chassis::run(slcan, config.chassis)); + + // 5. 等待 + tokio::try_join!(gimbal, shooter, chassis)?; + Ok(()) +} + +// tasks/gimbal.rs +pub async fn run(mut bus: SocketCanBus, config: GimbalConfig) -> anyhow::Result<()> { + let mut motors = GimbalMotors::new(); + let mut pid_yaw = PidController::new(config.yaw_pid); + let mut pid_pitch = PidController::new(config.pitch_pid); + let mut imu = Delta10Driver::new(&mut bus, config.imu_can_id); + let mut ticker = tokio::time::interval(Duration::from_millis(2)); + + loop { + ticker.tick().await; + + // 1. 非阻塞读取 delta10 帧 + if let Some(data) = imu.try_read()? { + // 更新姿态 + } + + // 2. 读取电机反馈 + while let Some(frame) = bus.try_recv()? { + motors.update_from_frame(&frame)?; + } + + // 3. 计算控制 + let yaw_out = pid_yaw.compute(setpoint.yaw, imu.yaw, 0.002); + let pitch_out = pid_pitch.compute(setpoint.pitch, imu.pitch, 0.002); + + // 4. 发送控制帧 + motors.send_commands(&mut bus, yaw_out, pitch_out).await?; + } +} +``` + +**验收标准**： +- [ ] 三个任务独立运行，互不阻塞 +- [ ] 每个任务在自己的 2ms 周期内完成 +- [ ] 配置可通过 TOML 热加载（或至少无需重新编译） + +--- + +## 6. 数据流与任务模型 + +### 6.1 任务划分 + +| 任务 | 频率 | 总线 | 设备 | 输出 | +|------|------|------|------|------| +| `gimbal_task` | 500Hz | can0 | 2× GM6020 + delta10 | 云台电流 | +| `shooter_task` | 500Hz | can0 + can1 | 2× M3508 + 1× M2006 | 摩擦轮 + 拨弹电流 | +| `chassis_task` | 500Hz | SLCAN | 4× M3508 | 底盘电流 | + +### 6.2 任务间通信 + +``` +┌──────────────┐ +│ 遥控器输入 │ (未来扩展) +└──────┬───────┘ + │ watch + ▼ +┌──────────────────────────────────────────┐ +│ tokio channels │ +│ ┌────────────┐ ┌────────────┐ │ +│ │ gimbal_cmd │ │chassis_cmd │ │ +│ └─────┬──────┘ └─────┬──────┘ │ +└────────┼────────────────┼────────────────┘ + │ │ + ┌────▼────┐ ┌────▼────┐ + │ gimbal │ │ chassis │ + │ task │ │ task │ + └─────────┘ └─────────┘ +``` + +**通信原语选择**： +- **遥控器输入 → 任务**：`tokio::sync::watch`（单生产者多消费者，只关心最新值） +- **任务 → 诊断输出**：`tokio::sync::mpsc`（有背压） +- **任务间同步**：`tokio::sync::Barrier`（如需要） + +### 6.3 每个周期的执行顺序 + +以 `gimbal_task` 为例： + +``` +t=0ms: + 1. ticker.tick() 返回 + 2. 非阻塞读取 delta10 帧 (try_recv) + 3. 非阻塞读取所有 pending CAN 帧 (while try_recv) + 4. 更新电机反馈 + 5. 计算 PID 输出 + 6. 发送控制帧 (await) + 7. 记录诊断 + +t=2ms: + 重复 +``` + +**关键**：步骤 2-5 必须是**非阻塞**的，只有步骤 6 可以 `await`。这保证任务不会因为等待 CAN 帧而错过下一个 tick。 + +--- + +## 7. 实时性设计 + +### 7.1 调度策略 + +```rust +// realtime.rs +use libc::{sched_param, sched_setscheduler, SCHED_FIFO, mlockall, MCL_CURRENT, MCL_FUTURE}; + +pub fn enable(priority: i32) -> anyhow::Result<()> { + unsafe { + // 1. 锁定内存，避免页错误 + if mlockall(MCL_CURRENT | MCL_FUTURE) != 0 { + return Err(std::io::Error::last_os_error().into()); + } + + // 2. 设置实时调度 + let param = sched_param { sched_priority: priority }; + if sched_setscheduler(0, SCHED_FIFO, ¶m) != 0 { + return Err(std::io::Error::last_os_error().into()); + } + } + Ok(()) +} +``` + +**优先级分配**： +- `gimbal_task`: 80（最高，因为云台抖动直接影响射击） +- `shooter_task`: 75 +- `chassis_task`: 70 + +### 7.2 抖动测量 + +在每个任务的 tick 中记录实际唤醒时间： + +```rust +let expected = start + Duration::from_millis(2 * tick_count); +let actual = Instant::now(); +let jitter = actual.duration_since(expected); +telemetry::record_jitter(jitter); +``` + +**目标**：抖动 < 200μs（T153 标准 Linux 的典型值）。 + +### 7.3 降级策略 + +如果抖动超过阈值（如 500μs），采取： + +1. **降低非关键任务频率**：chassis_task 降到 250Hz +2. **合并任务**：shooter 和 gimbal 合并 +3. **异构核卸载**：将 CAN 收发卸载到 E907（远期） + +--- + +## 8. 仿真与自动整定 + +### 8.1 仿真流程 + +``` +┌─────────────────────────────────────────────────┐ +│ 1. 定义电机参数 (config/motors.toml) │ +│ ↓ │ +│ 2. 构建 SimPlant │ +│ ↓ │ +│ 3. 运行 auto_tune │ +│ ↓ │ +│ 4. 输出 PID 参数 → config/pid.toml │ +│ ↓ │ +│ 5. 在 SimPlant 上验证阶跃响应 │ +│ ↓ │ +│ 6. 部署到实机，微调 ±20% │ +└─────────────────────────────────────────────────┘ +``` + +### 8.2 电机参数配置 + +```toml +# config/motors.toml +[gm6020] +resistance = 0.35 +inductance = 0.0002 +torque_constant = 0.024 +back_emf_constant = 0.024 +gear_ratio = 1.0 +rotor_inertia = 1.5e-5 +current_loop_bw = 1000.0 + +[m3508] +resistance = 0.15 +inductance = 0.0001 +torque_constant = 0.015 +back_emf_constant = 0.015 +gear_ratio = 19.2 +rotor_inertia = 2.0e-5 +current_loop_bw = 800.0 + +[m2006] +# ... +``` + +### 8.3 自动整定示例 + +```rust +#[tokio::test] +async fn tune_m3508_speed_loop() { + let params = load_motor_params("config/motors.toml", "m3508"); + let mut plant = SimPlant::new(DcMotorModel::new(params)); + + let config = TuneConfig { + amplitude: 5000.0, // 电流指令幅值 + hysteresis: 100.0, + dt: 0.002, + max_time: 5.0, + }; + + let result = auto_tune(&mut plant, config).await; + + // 验证整定结果 + assert!(result.kp > 0.0); + assert!(result.ki > 0.0); + assert!(result.ku > 0.0); + assert!(result.tu > 0.0); + + // 在仿真中验证阶跃响应 + let mut pid = PidController::new(result.into()); + let mut plant = SimPlant::new(DcMotorModel::new(params)); + let mut max_overshoot = 0.0f32; + + for _ in 0..500 { // 1 秒 + let out = pid.compute(1000.0, plant.step(0.0, 0.002), 0.002); + let _ = plant.step(out, 0.002); + // 记录超调 + } + + assert!(max_overshoot < 0.1, "超调过大: {}", max_overshoot); +} +``` + +--- + +## 9. 测试策略 + +### 9.1 分层测试 + +| 层 | 测试类型 | 工具 | +|----|---------|------| +| `rm-protocol` | 单元测试 + 模糊测试 | `#[test]`, `proptest` | +| `rm-control` | 单元测试 + 仿真测试 | `#[test]`, `rm-sim` | +| `rm-devices` | 集成测试（mock CAN） | `mock` feature | +| `rm-drivers` | 硬件在环测试 | `vcan` 回环 | +| `rm-app` | 端到端测试 | `vcan` + 模拟电机 | + +### 9.2 vcan 回环测试 + +```bash +# 创建虚拟 CAN 接口 +sudo modprobe vcan +sudo ip link add dev vcan0 type vcan +sudo ip link set up vcan0 +``` + +```rust +#[tokio::test] +async fn test_socketcan_loopback() { + let mut bus = SocketCanBus::open("vcan0").unwrap(); + let frame = CanFrame { id: 0x1FF, data: [1,2,3,4,5,6,7,8], dlc: 8, is_extended: false }; + bus.send(&frame).await.unwrap(); + let received = bus.recv().await.unwrap(); + assert_eq!(received.id, 0x1FF); +} +``` + +### 9.3 仿真回归测试 + +每次修改 `rm-control` 后，运行完整的仿真测试套件： + +```bash +cargo test -p rm-sim --release +``` + +**回归基准**：阶跃响应、抗扰响应、整定收敛时间。任何指标退化 >5% 视为失败。 + +--- + +## 10. 部署与配置 + +### 10.1 CAN 接口初始化脚本 + +```bash +#!/bin/bash +# scripts/setup_can.sh + +# 原生 CAN (T153) +ip link set can0 type can bitrate 1000000 restart-ms 100 +ip link set can0 up +ip link set can1 type can bitrate 1000000 restart-ms 100 +ip link set can1 up + +# SLCAN (candlelite2) — 由应用层直接打开串口，无需 ip link +# 但需要设置串口权限 +chmod 666 /dev/ttyACM0 + +echo "CAN interfaces ready" +ip -details link show can0 +ip -details link show can1 +``` + +### 10.2 交叉编译 + +```toml +# .cargo/config.toml +[target.armv7-unknown-linux-gnueabihf] +linker = "arm-linux-gnueabihf-gcc" +rustflags = [ + "-C", "target-cpu=cortex-a7", + "-C", "target-feature=+neon", +] +``` + +```bash +# 构建 +cargo build --release --target armv7-unknown-linux-gnueabihf + +# 部署 +scp target/armv7-unknown-linux-gnueabihf/release/rm-app root@t153:/opt/rm/ +scp -r config/ root@t153:/opt/rm/ +``` + +### 10.3 运行时配置 + +```toml +# config/robot.toml +[realtime] +gimbal_priority = 80 +shooter_priority = 75 +chassis_priority = 70 +mlock = true + +[can] +can0_bitrate = 1000000 +can1_bitrate = 1000000 +slcan_device = "/dev/ttyACM0" +slcan_bitrate = 1000000 + +[gimbal] +yaw_pid = { kp = 10.0, ki = 0.5, kd = 0.1 } +pitch_pid = { kp = 12.0, ki = 0.6, kd = 0.15 } +imu_can_id = 0x100 + +[shooter] +friction_pid = { kp = 5.0, ki = 0.2, kd = 0.05 } +trigger_pid = { kp = 3.0, ki = 0.1, kd = 0.02 } + +[chassis] +speed_pid = { kp = 8.0, ki = 0.3, kd = 0.05 } +``` + +--- + +## 11. 执行 Agent 工作流 + +### 11.1 阶段划分 + +| 阶段 | 任务 | 验收 | +|------|------|------| +| **P0: 骨架** | 创建 workspace + 所有 crate 的空骨架 | `cargo build` 通过 | +| **P1: 协议层** | 实现 `rm-hal` + `rm-protocol` | 单元测试 + 模糊测试通过 | +| **P2: 驱动层** | 实现 `rm-drivers`（SocketCAN + SLCAN） | vcan 回环测试通过 | +| **P3: 设备层** | 实现 `rm-devices` | mock 测试通过 | +| **P4: 控制层** | 实现 `rm-control` | PID 单元测试通过 | +| **P5: 仿真层** | 实现 `rm-sim` | 整定测试通过 | +| **P6: 应用层** | 实现 `rm-app` | 在 T153 上运行 | +| **P7: 实机联调** | 在真实机器人上验证 | 所有电机可控 | + +### 11.2 每个阶段的交付物 + +- **代码**：所有实现文件 +- **测试**：单元测试 + 集成测试 +- **文档**：模块级文档注释 + 必要的 README +- **配置**：对应的 TOML 配置文件 + +### 11.3 质量门禁 + +每个阶段完成后必须通过： + +1. `cargo build --workspace` 无警告 +2. `cargo test --workspace` 全绿 +3. `cargo clippy --workspace -- -D warnings` 无警告 +4. `cargo fmt --check` 通过 +5. 新增代码有文档注释 +6. 关键路径有测试覆盖 + +### 11.4 决策点 + +以下决策需要人工确认后才能继续： + +- **D1**: delta10 的 CAN ID 和协议模式（需查阅数据手册） +- **D2**: GM6020 和 M3508 的具体 ID 分配（需确认硬件接线） +- **D3**: candlelite2 的串口设备名（需在 T153 上 `ls /dev/ttyACM*`） +- **D4**: 是否启用 E907 异构核（基于 P7 的抖动测试结果） + +--- + +## 12. 附录 + +### 12.1 术语表 + +| 术语 | 含义 | +|------|------| +| SLCAN | Serial CAN，一种通过串口传输 CAN 帧的 ASCII 协议 | +| SocketCAN | Linux 内核的 CAN 子系统 | +| SLCAN 固件 | candlelite2 上运行的非 SocketCAN 固件 | +| vcan | Linux 虚拟 CAN 接口，用于测试 | +| FDILINK | delta10 的一种 CAN 协议模式 | +| ZN | Ziegler-Nichols，一种 PID 整定方法 | + +### 12.2 参考链接 + +- [socketcan-rs](https://github.com/socketcan-rs/socketcan-rs) +- [slcan_fd](https://crates.io/crates/slcan_fd) +- [discrete_pid](https://crates.io/crates/discrete_pid) +- [candleLight](https://github.com/candle-usb/candleLight_fw) +- [DETA10 手册](https://fdisystems.github.io/DETA10_Docs/) + +### 12.3 变更日志 + +| 版本 | 日期 | 变更 | +|------|------|------| +| 1.0 | 2026-09-23 | 初始版本，基于 T153 + SLCAN + delta10 确认 | + +--- + +**文档结束** + +执行 Agent 应按照第 11 节的阶段划分逐步推进，每个阶段完成后提交验收报告，遇到决策点（D1-D4）时暂停并请求人工确认。 diff --git a/Cargo.lock b/Cargo.lock index 24a9a6f..56cbbea 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -61,6 +61,12 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "anyhow" +version = "1.0.104" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "330a5ed07fa54e4702c9d6c4174f74427fc0ef6e214bbd677ae50a5099946470" + [[package]] name = "approx" version = "0.5.1" @@ -70,6 +76,37 @@ dependencies = [ "num-traits", ] +[[package]] +name = "argmin" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7ab7ca97779074715a402e5e8045fae27e7191acaec9b4c5653276316e9e404" +dependencies = [ + "anyhow", + "argmin-math", + "num-traits", + "paste", + "rand", + "rand_xoshiro", + "thiserror", + "web-time", +] + +[[package]] +name = "argmin-math" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba6958a87117ff5e1d0d7716856a4303752518012ec4a67a68446b6631a1a54d" +dependencies = [ + "anyhow", + "cfg-if", + "num-complex", + "num-integer", + "num-traits", + "rand", + "thiserror", +] + [[package]] name = "autocfg" version = "1.5.1" @@ -91,12 +128,24 @@ version = "0.8.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5e764a1d40d510daf35e07be9eb06e75770908c27d411ee6c92109c9840eaaf7" +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + [[package]] name = "bitflags" version = "2.13.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3ded4057c258ba199e2d26386d3af3780957ecaee6c4ef4041c6b4b8b97c0b06" +[[package]] +name = "bumpalo" +version = "3.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72f5acc6cb2ba439de613abc23857ec3d78374d8ed5ac84e9d11336e87da8649" + [[package]] name = "byteorder" version = "1.5.0" @@ -110,6 +159,16 @@ dependencies = [ "tokio", ] +[[package]] +name = "cc" +version = "1.4.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "54413ede23c2daf518f35156dfde027feb2374004d63bd497f983c8db9c0e313" +dependencies = [ + "find-msvc-tools", + "shlex", +] + [[package]] name = "cfg-if" version = "1.0.5" @@ -122,21 +181,79 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f079e83a288787bcd14a6aea84cee5c87a67c5a3e660c30f557a3d24761b3527" +[[package]] +name = "chassis-bench" +version = "0.1.0" +dependencies = [ + "can-link", + "clap", + "dji-esc", + "dji-esc-sim", + "dji-esc-slcan", + "dji-esc-socketcan", + "motor-core", + "thiserror", + "tokio", + "tracing", + "tracing-subscriber", +] + +[[package]] +name = "chassis-config" +version = "0.1.0" +dependencies = [ + "dji-esc", + "mecanum", + "motor-core", + "serde", + "thiserror", + "toml", +] + +[[package]] +name = "chassis-tune" +version = "0.1.0" +dependencies = [ + "anyhow", + "approx", + "argmin", + "argmin-math", + "can-link", + "chassis-bench", + "chassis-config", + "clap", + "csv", + "dji-esc", + "dji-esc-sim", + "indicatif", + "motor-core", + "plotters", + "rand", + "rayon", + "serde", + "serde_json", + "thiserror", + "tokio", + "toml", + "tracing", + "tracing-subscriber", +] + [[package]] name = "chassisd" version = "0.1.0" dependencies = [ "approx", + "chassis-config", "clap", "dji-esc", + "dji-esc-sim", "dji-esc-socketcan", "mecanum", "motor-core", "proptest", - "serde", "thiserror", "tokio", - "toml", "tracing", "tracing-subscriber", ] @@ -187,6 +304,116 @@ version = "1.0.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1d07550c9036bf2ae0c684c4297d503f838287c83c53686d05370d0e139ae570" +[[package]] +name = "console" +version = "0.16.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e96a4956774c13c126a8b5af4daa79384f4d826534c95a02d76afb39e2ab64e3" +dependencies = [ + "encode_unicode", + "libc", + "unicode-width", + "windows-sys", +] + +[[package]] +name = "core-foundation" +version = "0.9.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91e195e091a93c46f7102ec7818a2aa394e1e1771c3ab4825963fa03e45afb8f" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" + +[[package]] +name = "core-graphics" +version = "0.23.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c07782be35f9e1140080c6b96f0d44b739e2278479f64e02fdab4e32dfd8b081" +dependencies = [ + "bitflags 1.3.2", + "core-foundation", + "core-graphics-types", + "foreign-types", + "libc", +] + +[[package]] +name = "core-graphics-types" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "45390e6114f68f718cc7a830514a96f903cccd70d02a8f6d9f643ac4ba45afaf" +dependencies = [ + "bitflags 1.3.2", + "core-foundation", + "libc", +] + +[[package]] +name = "core-text" +version = "20.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c9d2790b5c08465d49f8dc05c8bcae9fea467855947db39b0f8145c091aaced5" +dependencies = [ + "core-foundation", + "core-graphics", + "foreign-types", + "libc", +] + +[[package]] +name = "crossbeam-deque" +version = "0.8.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "622f3fc73690be383c7214310406f28a90e6edeadc3cea882f9d71e495b9711a" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc74980687109a3b14c72fd458107bf0baa1da1a1a805e178d15501ba9b86d9d" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a31eee39dddec8330830986fcd7625edb5a24ec90ea038215273bbc3adb08ac6" + +[[package]] +name = "csv" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52cd9d68cf7efc6ddfaaee42e7288d3a99d613d4b50f76ce9827ae0c6e14f938" +dependencies = [ + "csv-core", + "itoa", + "ryu", + "serde_core", +] + +[[package]] +name = "csv-core" +version = "0.1.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "704a3c26996a80471189265814dbc2c257598b96b8a7feae2d31ace646bb9782" +dependencies = [ + "memchr", +] + [[package]] name = "darling" version = "0.20.11" @@ -253,6 +480,27 @@ dependencies = [ "syn 2.0.119", ] +[[package]] +name = "dirs" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3e8aa94d75141228480295a7d0e7feb620b1a5ad9f12bc40be62411e38cce4e" +dependencies = [ + "dirs-sys", +] + +[[package]] +name = "dirs-sys" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e01a3366d27ee9890022452ee61b2b63a67e6f13f58900b651ff5665f0bb1fab" +dependencies = [ + "libc", + "option-ext", + "redox_users", + "windows-sys", +] + [[package]] name = "dji-esc" version = "0.1.0" @@ -264,6 +512,30 @@ dependencies = [ "tracing", ] +[[package]] +name = "dji-esc-sim" +version = "0.1.0" +dependencies = [ + "can-link", + "dji-esc", + "motor-core", + "rand", + "thiserror", + "tokio", + "tracing", +] + +[[package]] +name = "dji-esc-slcan" +version = "0.1.0" +dependencies = [ + "can-link", + "serial", + "slcan", + "tokio", + "tracing", +] + [[package]] name = "dji-esc-socketcan" version = "0.1.0" @@ -276,6 +548,27 @@ dependencies = [ "tracing", ] +[[package]] +name = "dlib" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ab8ecd87370524b461f8557c119c405552c396ed91fc0a8eec68679eab26f94a" +dependencies = [ + "libloading", +] + +[[package]] +name = "dwrote" +version = "0.11.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e1b35532432acc8b19ceed096e35dfa088d3ea037fe4f3c085f1f97f33b4d02" +dependencies = [ + "lazy_static", + "libc", + "winapi", + "wio", +] + [[package]] name = "either" version = "1.18.0" @@ -291,6 +584,12 @@ dependencies = [ "nb", ] +[[package]] +name = "encode_unicode" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34aa73646ffb006b8f5147f3dc182bd4bcb190227ce861fc4a4844bf8e3cb2c0" + [[package]] name = "equivalent" version = "1.0.2" @@ -313,12 +612,87 @@ version = "2.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "da7c62ceae207dd37ea5b845da6a0696c799f85e97da1ab5b7910be3c1c80223" +[[package]] +name = "find-msvc-tools" +version = "0.1.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef25905e51abafe4dcea6c15fec58c57b601cdbd0ee53d22ea1d3016c587d39b" + +[[package]] +name = "float-ord" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ce81f49ae8a0482e4c55ea62ebbd7e5a686af544c00b9d090bba3ff9be97b3d" + [[package]] name = "fnv" version = "1.0.7" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" +[[package]] +name = "font-kit" +version = "0.14.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2c7e611d49285d4c4b2e1727b72cf05353558885cc5252f93707b845dfcaf3d3" +dependencies = [ + "bitflags 2.13.2", + "byteorder", + "core-foundation", + "core-graphics", + "core-text", + "dirs", + "dwrote", + "float-ord", + "freetype-sys", + "lazy_static", + "libc", + "log", + "pathfinder_geometry", + "pathfinder_simd", + "walkdir", + "winapi", + "yeslogic-fontconfig-sys", +] + +[[package]] +name = "foreign-types" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d737d9aa519fb7b749cbc3b962edcf310a8dd1f4b67c91c4f83975dbdd17d965" +dependencies = [ + "foreign-types-macros", + "foreign-types-shared", +] + +[[package]] +name = "foreign-types-macros" +version = "0.2.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ea5190182e6915eb873ddbc16e23b711b6eb1f9c00a0d0a3a91b5f6228475225" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "foreign-types-shared" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa9a19cbb55df58761df49b23516a86d432839add4af60fc256da840f66ed35b" + +[[package]] +name = "freetype-sys" +version = "0.20.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e7edc5b9669349acfda99533e9e0bcf26a51862ab43b08ee7745c55d28eb134" +dependencies = [ + "cc", + "libc", + "pkg-config", +] + [[package]] name = "futures" version = "0.3.34" @@ -475,12 +849,51 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "indicatif" +version = "0.18.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9433806cd6b4ec1aba79c021c7e4c58fb4c3b9977c085062e611ac929998fb0c" +dependencies = [ + "console", + "portable-atomic", + "unicode-width", + "unit-prefix", + "web-time", +] + +[[package]] +name = "ioctl-rs" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f7970510895cee30b3e9128319f2cefd4bde883a39f38baa279567ba3a7eb97d" +dependencies = [ + "libc", +] + [[package]] name = "is_terminal_polyfill" version = "1.70.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "js-sys" +version = "0.3.105" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce57d20d1ea864ce2ac172ab472d409214f4fd359f0b2a2775abdf522e2af99e" +dependencies = [ + "cfg-if", + "futures-util", + "wasm-bindgen", +] + [[package]] name = "lazy_static" version = "1.5.0" @@ -493,6 +906,25 @@ version = "0.2.189" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3eaf3ede3fee6db1a4c2ee091bf8a8b4dccdc6d17f656fb07896ee72867612f2" +[[package]] +name = "libloading" +version = "0.8.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7c4b02199fee7c5d21a5ae7d8cfa79a6ef5bb2fc834d6e9058e89c825efdc55" +dependencies = [ + "cfg-if", + "windows-link", +] + +[[package]] +name = "libredox" +version = "0.1.25" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61ff90caf6077a803a240f62fdbe88645a890bbca49ef8174c3cb0404362171d" +dependencies = [ + "libc", +] + [[package]] name = "linux-raw-sys" version = "0.12.1" @@ -573,7 +1005,7 @@ version = "0.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "22f9786d56d972959e1408b6a93be6af13b9c1392036c5c1fafa08a1b0c6ee87" dependencies = [ - "bitflags", + "bitflags 2.13.2", "byteorder", "derive_builder", "getset", @@ -602,7 +1034,7 @@ version = "0.31.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "cf20d2fde8ff38632c426f1165ed7436270b44f199fc55284c38276f9db47c3d" dependencies = [ - "bitflags", + "bitflags 2.13.2", "cfg-if", "cfg_aliases", "libc", @@ -618,6 +1050,24 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.47" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ce2d95d4b3734dc35aa2f45e1aa22cd416814592a4f9d9205e11affd5b8e10b" +dependencies = [ + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.19" @@ -639,6 +1089,12 @@ version = "1.70.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" +[[package]] +name = "option-ext" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" + [[package]] name = "parking_lot" version = "0.12.5" @@ -662,12 +1118,81 @@ dependencies = [ "windows-link", ] +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pathfinder_geometry" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b7e7b4ea703700ce73ebf128e1450eb69c3a8329199ffbfb9b2a0418e5ad3" +dependencies = [ + "log", + "pathfinder_simd", +] + +[[package]] +name = "pathfinder_simd" +version = "0.5.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4500030c302e4af1d423f36f3b958d1aecb6c04184356ed5a833bf6b60435777" +dependencies = [ + "rustc_version", +] + [[package]] name = "pin-project-lite" version = "0.2.17" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" +[[package]] +name = "pkg-config" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f6b464fbc74e149a392436b17d523f769e057cb6877f6a5c4618bc6f11800548" + +[[package]] +name = "plotters" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5aeb6f403d7a4911efb1e33402027fc44f29b5bf6def3effcc22d7bb75f2b747" +dependencies = [ + "font-kit", + "lazy_static", + "num-traits", + "pathfinder_geometry", + "plotters-backend", + "plotters-svg", + "ttf-parser", + "wasm-bindgen", + "web-sys", +] + +[[package]] +name = "plotters-backend" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df42e13c12958a16b3f7f4386b9ab1f3e7933914ecea48da7139435263a4172a" + +[[package]] +name = "plotters-svg" +version = "0.3.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "51bae2ac328883f7acdfea3d66a7c35751187f870bc81f94563733a154d7a670" +dependencies = [ + "plotters-backend", +] + +[[package]] +name = "portable-atomic" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05c8b63e8d9609db387f0324918f81d68fe27748f084ef092fb35954d0539a85" + [[package]] name = "ppv-lite86" version = "0.2.21" @@ -694,7 +1219,7 @@ checksum = "4b45fcc2344c680f5025fe57779faef368840d0bd1f42f216291f0dc4ace4744" dependencies = [ "bit-set", "bit-vec", - "bitflags", + "bitflags 2.13.2", "num-traits", "rand", "rand_chacha", @@ -770,13 +1295,52 @@ dependencies = [ "rand_core", ] +[[package]] +name = "rand_xoshiro" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f703f4665700daf5512dcca5f43afa6af89f09db47fb56be587f80636bda2d41" +dependencies = [ + "rand_core", +] + +[[package]] +name = "rayon" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb39b166781f92d482534ef4b4b1b2568f42613b53e5b6c160e24cfbfa30926d" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + [[package]] name = "redox_syscall" version = "0.5.18" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed2bf2547551a7053d6fdfafda3f938979645c44812fbfcda098faae3f1a362d" dependencies = [ - "bitflags", + "bitflags 2.13.2", +] + +[[package]] +name = "redox_users" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60dc65c0ff1a7ae1294b0c67b9f14baf70b644404010370171787bfac1038fc0" +dependencies = [ + "libredox", + "thiserror", ] [[package]] @@ -796,19 +1360,34 @@ version = "0.8.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d6f6ff9a378485b298a5286656da665ba74413d36db0979633275d2e708145d4" +[[package]] +name = "rustc_version" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" +dependencies = [ + "semver", +] + [[package]] name = "rustix" version = "1.1.5" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "891efababe418670775f199f0d233d84843c227a0949a883ce15b37c78d6629d" dependencies = [ - "bitflags", + "bitflags 2.13.2", "errno", "libc", "linux-raw-sys", "windows-sys", ] +[[package]] +name = "rustversion" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf54715a573b99ac80df0bc206da022bcd442c974952c7b9720069370852e21f" + [[package]] name = "rusty-fork" version = "0.3.1" @@ -821,12 +1400,33 @@ dependencies = [ "wait-timeout", ] +[[package]] +name = "ryu" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9774ba4a74de5f7b1c1451ed6cd5285a32eddb5cccb8cc655a4e50009e06477f" + +[[package]] +name = "same-file" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" +dependencies = [ + "winapi-util", +] + [[package]] name = "scopeguard" version = "1.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" +[[package]] +name = "semver" +version = "1.0.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a7852d02fc848982e0c167ef163aaff9cd91dc640ba85e263cb1ce46fae51cd" + [[package]] name = "serde" version = "1.0.229" @@ -857,6 +1457,19 @@ dependencies = [ "syn 3.0.6", ] +[[package]] +name = "serde_json" +version = "1.0.151" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c841b55ecdae098c80dcae9cf767f6f8a0c2cdb3416bbef72181df4d0fe73f14" +dependencies = [ + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + [[package]] name = "serde_spanned" version = "1.1.1" @@ -866,6 +1479,48 @@ dependencies = [ "serde_core", ] +[[package]] +name = "serial" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1237a96570fc377c13baa1b88c7589ab66edced652e43ffb17088f003db3e86" +dependencies = [ + "serial-core", + "serial-unix", + "serial-windows", +] + +[[package]] +name = "serial-core" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f46209b345401737ae2125fe5b19a77acce90cd53e1658cda928e4fe9a64581" +dependencies = [ + "libc", +] + +[[package]] +name = "serial-unix" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f03fbca4c9d866e24a459cbca71283f545a37f8e3e002ad8c70593871453cab7" +dependencies = [ + "ioctl-rs", + "libc", + "serial-core", + "termios", +] + +[[package]] +name = "serial-windows" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "15c6d3b776267a75d31bbdfd5d36c0ca051251caafc285827052bc53bcdc8162" +dependencies = [ + "libc", + "serial-core", +] + [[package]] name = "sharded-slab" version = "0.1.7" @@ -875,6 +1530,12 @@ dependencies = [ "lazy_static", ] +[[package]] +name = "shlex" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fadd59c855ef2080decdef8ff161eb6661b86933c9d82e5ba29dc602a55aba" + [[package]] name = "signal-hook-registry" version = "1.4.8" @@ -891,6 +1552,15 @@ version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" +[[package]] +name = "slcan" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8fb6ccb6c3c4bddda8d5c235cae7465f078130ac0eeed08f7689546a6af7731" +dependencies = [ + "serial-core", +] + [[package]] name = "smallvec" version = "1.16.1" @@ -913,7 +1583,7 @@ version = "4.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "472e000403167b1e2f65cfcf8ff235fe94b911f9bd9bdbc1f48047b8a1cf49aa" dependencies = [ - "bitflags", + "bitflags 2.13.2", "embedded-can", "futures", "hex", @@ -969,6 +1639,15 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "termios" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d5d9cf598a6d7ce700a4e6a9199da127e6819a61e64b68609683cc9a01b5683a" +dependencies = [ + "libc", +] + [[package]] name = "thiserror" version = "2.0.20" @@ -1124,6 +1803,12 @@ dependencies = [ "tracing-log", ] +[[package]] +name = "ttf-parser" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4" + [[package]] name = "unarray" version = "0.1.4" @@ -1136,6 +1821,18 @@ version = "1.0.26" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "d245f478577f809a851594d02313b640fb437e0bb33866753cff937863096954" +[[package]] +name = "unicode-width" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b4ac048d71ede7ee76d585517add45da530660ef4390e49b098733c6e897f254" + +[[package]] +name = "unit-prefix" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81e544489bf3d8ef66c953931f56617f423cd4b5494be343d9b9d3dda037b9a3" + [[package]] name = "utf8parse" version = "0.2.2" @@ -1157,6 +1854,16 @@ dependencies = [ "libc", ] +[[package]] +name = "walkdir" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b" +dependencies = [ + "same-file", + "winapi-util", +] + [[package]] name = "wasi" version = "0.11.1+wasi-snapshot-preview1" @@ -1172,6 +1879,102 @@ dependencies = [ "wit-bindgen", ] +[[package]] +name = "wasm-bindgen" +version = "0.2.128" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aecb87a33d3b0c5e3b7aa46336eaf486cffafbd281b195e4c8b80d50df2351bf" +dependencies = [ + "cfg-if", + "once_cell", + "rustversion", + "wasm-bindgen-macro", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.128" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a690d511e3c1a8b3a55e33511e3c2c00c78415cd23650f32b808627f5696b9ed" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.128" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "411e4887f0071ef2d2164a9d5fdf2d20efbef78fccd3a78b0c10a1dc5295e48a" +dependencies = [ + "bumpalo", + "proc-macro2", + "quote", + "syn 3.0.6", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.128" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81941cd78d0c92026c33e5e01312845a4cb1e9af3407f9134b100dd03144103e" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "web-sys" +version = "0.3.105" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fbddc4a036f00ec4f18c83445bd3115cb306a91da554919a099d9222fe4a7f8" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "web-time" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a6580f308b1fad9207618087a65c04e7a10bc77e02c8e84e9b00dd4b12fa0bb" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + [[package]] name = "windows-link" version = "0.2.1" @@ -1199,12 +2002,32 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "23b97319f7b8343df12cc98938e5c3eb436064524c8d2b4e30a1d3a36eecdf81" +[[package]] +name = "wio" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5d129932f4644ac2396cb456385cbf9e63b5b30c6e8dc4820bdca4eb082037a5" +dependencies = [ + "winapi", +] + [[package]] name = "wit-bindgen" version = "0.57.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e" +[[package]] +name = "yeslogic-fontconfig-sys" +version = "6.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d8b8abf912b9a29ff112e1671c97c33636903d13a69712037190e6805af4f76" +dependencies = [ + "dlib", + "once_cell", + "pkg-config", +] + [[package]] name = "zerocopy" version = "0.8.57" @@ -1224,3 +2047,9 @@ dependencies = [ "quote", "syn 2.0.119", ] + +[[package]] +name = "zmij" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29666d0abbfad1e3dc4dcf6144730dd3a3ab225bbbdac83319345b1b44ccfc1b" diff --git a/Cargo.toml b/Cargo.toml index 8310b7b..f16f72e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,8 +4,13 @@ members = [ "crates/motor-core", "crates/can-link", "crates/dji-esc", + "crates/dji-esc-sim", "crates/dji-esc-socketcan", + "crates/dji-esc-slcan", "crates/mecanum", + "crates/chassis-config", + "crates/chassis-bench", + "crates/chassis-tune", "crates/chassisd", ] @@ -35,12 +40,33 @@ clap = { version = "4", features = ["derive"] } # 测试 proptest = "1" approx = "0.5" +# 自动整定工具（chassis-tune） +# +# `rand` 对齐 argmin 依赖的 0.9，避免依赖树里出现两个 rand 大版本； +# `argmin-math` 必须显式开启 `vec`（argmin 自身只启 `primitives`）， +# 否则 `Vec` 参数向量没有 ArgminAdd/Sub/Mul 实现。 +argmin = { version = "0.11", default-features = false, features = ["rand"] } +argmin-math = { version = "0.5", features = ["vec"] } +rand = "0.9" +rayon = "1" +csv = "1.4" +serde_json = "1" +indicatif = "0.18" +anyhow = "1" +# 仅在 chassis-tune 的 `plot` feature 下引入：只要 SVG 后端与折线系列， +# 但文字标签需要 `ttf`（字体加载），因此不能用纯 svg_backend 最小集。 +plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] } # workspace 内部 crate motor-core = { path = "crates/motor-core" } can-link = { path = "crates/can-link" } dji-esc = { path = "crates/dji-esc" } +dji-esc-sim = { path = "crates/dji-esc-sim" } dji-esc-socketcan = { path = "crates/dji-esc-socketcan" } +dji-esc-slcan = { path = "crates/dji-esc-slcan" } mecanum = { path = "crates/mecanum" } +chassis-config = { path = "crates/chassis-config" } +chassis-bench = { path = "crates/chassis-bench" } +chassis-tune = { path = "crates/chassis-tune" } [workspace.lints.rust] unsafe_code = "forbid" diff --git a/README.md b/README.md index c509d8f..de543e1 100644 --- a/README.md +++ b/README.md @@ -1,19 +1,33 @@ # sentry-chassis -机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），SocketCAN 接入，tokio 异步架构。 +机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），CAN 接入（SocketCAN 或串口 slcan），tokio 异步架构。 ## 架构一览 | crate | 职责 | 平台 | | --- | --- | --- | -| [`motor-core`](crates/motor-core) | 电机抽象：`Motor` trait、`MotorState`、PID / 速度环控制器 | 纯逻辑，全平台 | -| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动、仿真器 | 纯逻辑，全平台 | +| [`motor-core`](crates/motor-core) | 电机抽象：能力 trait、`MotorState`、PID / 速度环 / 位置环 | 纯逻辑，全平台 | +| [`can-link`](crates/can-link) | CAN 传输抽象：`CanFrame` + `CanLink` 通道对（无协议知识） | 纯逻辑，全平台 | +| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动 | 纯逻辑，全平台 | +| [`dji-esc-sim`](crates/dji-esc-sim) | 被控对象仿真：摩擦 / 延迟 / 负载模型、场景与**离线评估 harness**、异步假电调 | 纯逻辑，全平台 | | [`dji-esc-socketcan`](crates/dji-esc-socketcan) | SocketCAN 后端：真实 CAN 接口 ↔ 内存通道桥接 | Linux | +| [`dji-esc-slcan`](crates/dji-esc-slcan) | slcan 串口后端：CANable 等 USB 适配器 ↔ 内存通道桥接 | 全平台 | | [`mecanum`](crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 | +| [`chassis-config`](crates/chassis-config) | `chassis.toml` schema：解析、校验、写回（守护进程与整定工具共用） | 全平台 | +| [`chassis-bench`](crates/chassis-bench) | 台架：激励场景、安全包络、真机试验采样 + `chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） | +| [`chassis-tune`](crates/chassis-tune) | **速度环 PID 自动整定**：参数空间搜索 + 指标 / 成本 + 报告（仿真 / 真机）+ 版本化**超参数预设** | 全平台 | | [`chassisd`](crates/chassisd) | 底盘守护进程：配置、1kHz 控制循环、安全逻辑、`--mock` 模式 | 全平台（实车需 Linux） | -分层原则：`motor-core` 与 `dji-esc` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道（`CanLink`）， -因此协议、运动学与控制逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层。 +分层原则：`motor-core`、`dji-esc`、`dji-esc-sim` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道 +（`CanLink`），因此协议、运动学、控制与整定逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层 +（`dji-esc-socketcan` / `dji-esc-slcan`，以及组合它们的 `chassis-bench` / `chassisd`）。 + +> 运输后端 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接，**不含应用**； +> 台架程序住在 `chassis-bench`，自动整定住在 `chassis-tune`。 + +> **命令是租约**：`EscMotor::set_velocity` / `set_effort_raw` / `set_position` 每次调用即续租， +> 超过 `BusOptions::command_timeout`（默认 50ms）未刷新，驱动强制 0 电流。 +> 控制循环必须**逐拍重发**同一个设定值（`chassisd` 的 1kHz 循环天然满足）。 ## 开发 @@ -22,6 +36,7 @@ ```sh cargo test --workspace # 单测 + 闭环仿真测试（无需硬件） cargo run -p chassisd -- --mock # 无硬件全链路演示 +just tune-smoke # 自动整定冒烟（确定性，无需硬件，CI 同款） just lint # fmt + clippy（与 CI 同标准） ``` @@ -31,31 +46,126 @@ just lint # fmt + clippy（与 CI 同标准） just vcan-test # 或见 justfile 内的 modprobe / ip link 命令 ``` -### 部署到目标板（armv7，实测：创龙 T153 / Buildroot） +## 台架：四轮 500 rpm 旋转测试（slcan） + +实车台架验证工具 `chassis-spin`：四轮同时以固定**转子**转速旋转（默认 500 rpm， +轮端 = 转子 ÷ 减速比 ≈ 26 rpm），带缓升、过速保护、上电门控与安全停机。 + +```sh +# 本机直接跑（CANable2 在 /dev/ttyACM0） +cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 + +# 只验证链路与反馈，不发非零电流（接线排查首选） +cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run + +# 目标板（T153，armv7）：交叉编译 → 推送 → 运行 +just bench-spin # 默认 root@192.168.2.2 + /dev/ttyACM0 + 500 rpm +just bench-check # 同上，但只做 dry-run +``` + +安全与使用要点： + +- **轮子必须架空或整车固定**；ctrl-c 随时安全停机（先 0 电流再关总线）。 +- 上电门控：四个电调都上报新鲜反馈前绝不下发速度；只接了部分电调时用 `--allow-partial` 降级。 +- 过速保护：任一电调超过 `--max-rpm`（默认 目标 × 2 + 200）持续 60ms 即立即停机。 +- 空载台架的合适整定与带载完全不同：默认 `kp=2 ki=10 ff=0 --current-limit 1500`， + 空载**不要**加前馈（会直接把轮子推到最高速度）。 + +## 自动整定（chassis-tune） + +手动试凑 `kp/ki/kd/ff` 既慢又难复现。`chassis-tune` 把这件事变成**参数空间搜索**： +种子化分层粗扫（`rayon` 并行）→ 以 top-k 为起点做多起点 `argmin` 局部精修（Nelder-Mead / PSO）， +每次评估都用同一套指标（上升时间、超调、调节时间、稳态误差、IAE、电流）和同一套硬约束 +（过速 / 长时间饱和 / 不收敛直接判罚，不参与权重折衷）。 + +```sh +# 0) 看有哪些超参数预设（只读；预设文件见 config/tune.toml） +cargo run -p chassis-tune -- presets + +# 1) 仿真搜索（默认，不碰硬件；同 seed 结果逐位一致） +cargo run --release -p chassis-tune -- sweep --samples 512 --out-dir target/tune + +# 1b) 用预设跑：超参数（区间 / 场景 / 权重 / 阈值 / 预算 / 安全包络）进版本管理 +# 优先级：内置默认 < 预设文件 < 显式 CLI（逐字段覆盖） +cargo run --release -p chassis-tune -- sweep \ + --preset config/tune.toml --preset-name bench_loaded + +# 2) 只复算一组给定增益（CI / 回归用，按验收阈值给退出码：0 通过 / 1 未通过 / 2 参数错） +cargo run -p chassis-tune -- validate --gains 2,10,0,0 + +# 3) 台架复验（**轮子会转**：必须架空或整车固定、急停可及） +cargo run --release -p chassis-tune -- sweep \ + --preset config/tune.toml --preset-name bench_noload \ + --device /dev/ttyACM0 --esc-id 1 --i-know-the-wheel-spins +``` + +随仓库提供的预设（`config/tune.toml`）： + +| 预设 | 后端 | 用途 | +| --- | --- | --- | +| `sim_bench_model`（默认） | 仿真 | 台架量级模型全量搜索：评审 / CI / 代码回归基准 | +| `sim_ideal` | 仿真 | 理想模型秒级回归（结果**不可直接上车**） | +| `bench_noload` | 真机 | 空载台架，小样本局部搜索，前馈固定 0 | +| `bench_loaded` | 真机 | 带载（整车落地），前馈区间上调 | +| `field_updown` | 真机 | 实战验收：升 / 降 / 回零双向，阈值收紧 | + +预设**不能**携带 `current_limit` / `direction` / `gear_ratio`（写了直接报错）—— +安全与机械约束只来自 `chassis.toml` 或显式 CLI。 + +产物（`--out-dir`）：`trials.csv`（全部候选与判定）、`best.toml`（可粘贴的增益片段）、 +`best-trace.csv`、`report.md`、`summary.json`；台架模式另有 `hil-validation.csv`（同一组增益的重复性复验）。 +加 `--features plot` 还会输出 `best.svg` 三联图。 + +整定工具与守护进程**共用同一份配置 schema**（`chassis-config`），因此可以直接拿实车配置当输入： + +```sh +# 用 chassis.toml 里的限流 / 电调 ID / 方向 / 减速比（这些**不参与搜索**）， +# 打印与现有增益的成本对比，并把结果写回一份新配置 +cargo run --release -p chassis-tune -- sweep \ + --config config/chassis.toml.example --motor front_left \ + --write-config target/chassis.tuned.toml +``` + +要点： + +- **仿真整定只保证在被控对象模型内最优**。模型里的摩擦 / 延迟是量级估计，且不含测量噪声， + 因此仿真最优解可能偏激进；交付前必须用同一 `--scenario` 跑一次 `--backend bench` 复验。 +- 最优解贴到参数空间边界时报告会明确提示——那是"约束在决定结果"，应放宽 `--kp` / `--ki` 再搜。 +- 台架搜索串行执行（只有一条总线），每次试验后强制 0 电流并静默 `--between-trials-ms`； + 过速 / 过温 / 掉线 / 超时 / ctrl-c 任一命中即停机并收工。 +- `current_limit`、`direction`、`gear_ratio` 属于安全与机械约束，**不参与搜索**。 +- 换一组工况先加**预设**（`config/tune.toml`）而不是复制一长串命令行： + `chassis-tune presets` 查看、`--preset-name` 选用、CLI 只覆盖本次要变的项。 + +## 部署到目标板（armv7，实测：创龙 T153 / Ubuntu 22.04） 二进制为 musl 静态链接，不依赖板端 libc，无需交叉工具链： ```sh -just deploy # 编译 + 推送 + mock 试运行（just deploy root@169.254.63.4） +just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2） # 或手动： cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf -scp target/armv7-unknown-linux-musleabihf/release/chassisd root@169.254.63.4:/tmp/ +scp target/armv7-unknown-linux-musleabihf/release/chassisd root@192.168.2.2:/tmp/ ``` -板端拉起 CAN（C620 标准 1Mbps；`restart-ms` 让 bus-off 后自动恢复）： +板端 CAN 有两条互斥路径： -```sh -ip link set can0 up type can bitrate 1000000 restart-ms 100 -``` +- **SocketCAN**（板载原生 CAN 控制器，走 `can0`/`can1`）： + `ip link set can0 up type can bitrate 1000000 restart-ms 100`，配 `--interface can0`。 +- **slcan**（USB 适配器，如 CANable2 在 CDC ACM 下枚举为 `/dev/ttyACM0`）： + 用 [`dji-esc-slcan`](crates/dji-esc-slcan)（`chassisd` / [`chassis-bench`](crates/chassis-bench) 均可），无需 `ip link`。 实车运行：`/tmp/chassisd --config /tmp/chassis.toml --interface can0`。板上运行样例见 [`deploy/chassisd.service`](deploy/chassisd.service)，配置样例见 [`config/chassis.toml.example`](config/chassis.toml.example)。 -> 注意：电调未上电 / CAN 线未接时，控制器会因无 ACK 进入 bus-off（可用 +> 注意：电调未上电 / CAN 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off（可用 > `ip -d link show can0` 查看），chassisd 侧表现为四电机反馈离线、强制零电流—— > 这正是安全设计的预期行为。 ## 文档 -- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、测试策略 +- [**底盘开发文档**](docs/chassis.md)——硬件接线、坐标与单位约定、控制与安全设计、配置参考、 + **标定与上车流程**、自动整定与超参数预设、台架工具、故障排查（开发 / 调试 / 上车先看这份） +- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、被控对象模型、参数空间搜索、测试策略 +- [整体架构规划](ARCHITECTURE.md)——全系统（含云台 / 发射 / IMU / 实时性）的 crate 划分与阶段计划 diff --git a/config/chassis.toml.example b/config/chassis.toml.example index 7b5b40a..f61ebe8 100644 --- a/config/chassis.toml.example +++ b/config/chassis.toml.example @@ -11,6 +11,9 @@ control_period_ms = 1 command_timeout_ms = 200 # 电机反馈离线判定（毫秒），离线后强制 0 电流。 feedback_timeout_ms = 100 +# 驱动层命令租约（毫秒）：控制循环超过该时长未刷新电机命令 → 强制 0 电流 +# （防控制任务死亡后旧指令持续输出）。必须 >= control_period_ms。 +motor_command_timeout_ms = 50 [mecanum] # 轮轴线到底盘中心的纵向 / 横向半距（米）与轮半径（米）。 diff --git a/config/tune.toml b/config/tune.toml new file mode 100644 index 0000000..549e1e5 --- /dev/null +++ b/config/tune.toml @@ -0,0 +1,334 @@ +# chassis-tune 超参数预设 +# +# 一次整定的结果不只取决于被控对象与种子，还取决于"搜什么区间、用哪个场景、 +# 什么算好、什么算达标、搜多细"。这份文件把这些**实验条件**固化下来： +# 评审一次、长期复用、任何机器上都能复现同一组默认行为。 +# +# 用法： +# chassis-tune presets # 列出本文件全部预设 +# chassis-tune sweep --preset config/tune.toml # 用 default_preset +# chassis-tune sweep --preset config/tune.toml --preset-name bench_loaded +# chassis-tune sweep --preset-name bench_loaded # 缺省路径即本文件 +# chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预设的单项 +# +# 优先级：内置默认 < 本文件的预设 < 显式命令行参数（逐字段覆盖）。 +# 本文件里没写的项一律沿用内置默认，因此预设只需写"与默认不同"的部分—— +# 但为了让评审者一眼看全工况，下面每个预设都写全了关键项。 +# +# **安全边界**：current_limit / direction / gear_ratio / 电调 ID / 串口设备名 +# 不属于超参数，本文件**不接受**这些键（写了会直接报错）。它们只来自 +# chassis.toml 或显式 CLI——否则预设文件就成了绕过限流的口子。 +# +# schema：见 crates/chassis-tune/src/preset.rs（version = 1，未知键报错）。 + +version = 1 +default_preset = "sim_bench_model" + +# ---------------------------------------------------------------- 离线：台架量级模型 +# 默认预设。评审、CI 冒烟、改完代码回归都用它：不碰硬件，结果可复现。 +[preset.sim_bench_model] +description = "离线 · 台架量级模型（静/库仑摩擦 + 2 拍延迟）：全量搜索，评审与回归的基准" + +[preset.sim_bench_model.space] +# kp 覆盖手调基线 2 与仿真最优 8 两侧；对数刻度保证小增益也被采样到。 +kp = { min = 0.5, max = 30.0, scale = "log" } +# ki 允许精确取 0（纯比例在某些工况已经够用）。 +ki = { min = 0.1, max = 100.0, scale = "log0" } +# kd 半概率取 0：速度环加微分对量化噪声敏感，先让搜索自己决定要不要。 +kd = { min = 0.01, max = 3.0, scale = "log0" } +# 前馈上限 2.5 覆盖带载所需的 ≈1.77（raw/rpm）。 +feedforward = { min = 0.01, max = 2.5, scale = "log0" } + +[preset.sim_bench_model.scenario] +profile = "ramp-and-hold" +target_rpm = 1500.0 +ramp_ms = 200.0 +hold_ms = 1800.0 +dt_us = 1000.0 + +[preset.sim_bench_model.cost] +# 跟得上、不过冲最重要；电流只作次要惩罚（省电流不是目的，避免烧电调才是）。 +settle = 1.0 +overshoot = 1.0 +iae = 0.5 +effort = 0.1 + +[preset.sim_bench_model.acceptance] +max_overshoot_pct = 20.0 +max_ss_error_pct = 5.0 + +[preset.sim_bench_model.search] +seed = 20260101 +samples = 512 +top_k = 8 +solver = "nelder-mead" +max_iters = 200 +particles = 24 +max_evals = 4000 +threads = 0 + +[preset.sim_bench_model.sim] +plant = "gm3508-bench" +# 域随机化：单点模型调出来的增益对增益/时间常数/延迟高度敏感， +# 因此默认在 8 个成员的集成上做鲁棒优化，而不是对着一条理想曲线最优化。 +ensemble = 8 +robust_weight = 0.5 +gain_pct = 0.2 +tau_pct = 0.3 +delay_ticks_max = 3 +friction_pct = 0.5 + +[preset.sim_bench_model.bench] +# 离线预设不用台架，但安全包络仍写全，方便临时切 --backend bench 预演。 +max_rpm = 3000.0 +max_temp_c = 70 +trial_timeout_ms = 6000 +between_trials_ms = 500 +startup_timeout_ms = 5000 +continue_on_abort = false +validate_runs = 3 + +# ---------------------------------------------------------------- 离线：理想模型 +# 无摩擦、无延迟的一阶模型：跑得快，适合改完控制逻辑后的秒级回归， +# **不要**拿它的结果直接上车（模型里没有摩擦与延迟，最优解会偏激进）。 +[preset.sim_ideal] +description = "离线 · 理想一阶模型（无摩擦无延迟）：秒级回归用，结果不可直接上车" + +[preset.sim_ideal.space] +kp = { min = 0.5, max = 30.0, scale = "log" } +ki = { min = 0.1, max = 100.0, scale = "log0" } +kd = { min = 0.01, max = 3.0, scale = "log0" } +feedforward = { min = 0.01, max = 2.5, scale = "log0" } + +[preset.sim_ideal.scenario] +profile = "ramp-and-hold" +target_rpm = 1500.0 +ramp_ms = 200.0 +hold_ms = 1800.0 +dt_us = 1000.0 + +[preset.sim_ideal.cost] +settle = 1.0 +overshoot = 1.0 +iae = 0.5 +effort = 0.1 + +[preset.sim_ideal.acceptance] +max_overshoot_pct = 20.0 +max_ss_error_pct = 5.0 + +[preset.sim_ideal.search] +seed = 20260101 +samples = 128 +top_k = 4 +solver = "nelder-mead" +max_iters = 100 +particles = 16 +max_evals = 1000 +threads = 0 + +[preset.sim_ideal.sim] +plant = "gm3508" +# 理想模型本身就是确定的，单成员即可。 +ensemble = 1 +robust_weight = 0.0 +gain_pct = 0.0 +tau_pct = 0.0 +delay_ticks_max = 0 +friction_pct = 0.0 + +[preset.sim_ideal.bench] +max_rpm = 3000.0 +max_temp_c = 70 +trial_timeout_ms = 6000 +between_trials_ms = 500 +startup_timeout_ms = 5000 +continue_on_abort = false +validate_runs = 3 + +# ---------------------------------------------------------------- 台架：空载 +# 轮子架空（无地面载荷）。摩擦小、静差靠积分压制，前馈**必须为 0**： +# 空载加前馈会把轮子直接推到最高转速。目标转速也压低，便于观察与急停。 +[preset.bench_noload] +description = "真机台架 · 空载（轮子架空）：小样本局部搜索，禁用前馈，目标 800 rpm" +# 真机后端：仍然需要 --i-know-the-wheel-spins 才会真的驱动电机。 +backend = "bench" + +[preset.bench_noload.space] +# 空载好解在手调基线（kp≈2, ki≈10）附近，区间收紧以提高样本密度。 +kp = { min = 0.2, max = 12.0, scale = "log" } +ki = { min = 0.1, max = 50.0, scale = "log0" } +kd = { min = 0.01, max = 1.0, scale = "log0" } +# 固定为 0：单点区间 + 线性刻度 = 搜索时该轴恒定。 +feedforward = { min = 0.0, max = 0.0, scale = "linear" } + +[preset.bench_noload.scenario] +profile = "ramp-and-hold" +target_rpm = 800.0 +ramp_ms = 300.0 +hold_ms = 1700.0 +dt_us = 1000.0 + +[preset.bench_noload.cost] +settle = 1.0 +overshoot = 1.0 +iae = 0.5 +effort = 0.1 + +[preset.bench_noload.acceptance] +# 空载量化噪声相对目标更大，放宽到 25%，避免"为了 0.5% 的稳态误差过冲"。 +max_overshoot_pct = 25.0 +max_ss_error_pct = 8.0 + +[preset.bench_noload.search] +seed = 20260101 +# 只有一条总线，每次试验 2s，24 次 ≈ 1 分钟：够做局部精修，不够铺满空间。 +samples = 24 +top_k = 3 +solver = "nelder-mead" +max_iters = 40 +particles = 12 +max_evals = 40 +threads = 1 + +[preset.bench_noload.sim] +# 真机后端不用这些，但保留"最接近台架的模型"，方便先离线预演再上车。 +plant = "gm3508-bench" +ensemble = 8 +robust_weight = 0.5 +gain_pct = 0.2 +tau_pct = 0.3 +delay_ticks_max = 3 +friction_pct = 0.5 + +[preset.bench_noload.bench] +# 目标是 800，过速保护留足裕量但远低于电调上限。 +max_rpm = 2000.0 +max_temp_c = 60 +trial_timeout_ms = 6000 +between_trials_ms = 500 +startup_timeout_ms = 5000 +continue_on_abort = false +# 同一组增益跑 3 次看重复性；离散度大说明参数贴着噪声，不该上车。 +validate_runs = 3 + +# ---------------------------------------------------------------- 台架：带载 +# 整车落地 / 配重（有地面摩擦与惯量）。静差明显，前馈开始有意义（覆盖 ≈1.77）。 +[preset.bench_loaded] +description = "真机台架 · 带载（整车落地/配重）：前馈区间上调，目标 1200 rpm，超调收紧" +backend = "bench" + +[preset.bench_loaded.space] +kp = { min = 0.5, max = 30.0, scale = "log" } +ki = { min = 0.5, max = 100.0, scale = "log0" } +kd = { min = 0.01, max = 3.0, scale = "log0" } +# 带载需要前馈抵消摩擦：下限不为 0，让搜索必须考虑它。 +feedforward = { min = 0.5, max = 2.5, scale = "log" } + +[preset.bench_loaded.scenario] +profile = "ramp-and-hold" +target_rpm = 1200.0 +ramp_ms = 300.0 +hold_ms = 2200.0 +dt_us = 1000.0 + +[preset.bench_loaded.cost] +settle = 1.0 +overshoot = 1.0 +iae = 0.5 +effort = 0.1 + +[preset.bench_loaded.acceptance] +# 带载下超调意味着实际位移过冲，收紧到 15%。 +max_overshoot_pct = 15.0 +max_ss_error_pct = 5.0 + +[preset.bench_loaded.search] +seed = 20260101 +samples = 32 +top_k = 4 +solver = "nelder-mead" +max_iters = 60 +particles = 16 +max_evals = 80 +threads = 1 + +[preset.bench_loaded.sim] +plant = "gm3508-bench" +ensemble = 8 +robust_weight = 0.5 +gain_pct = 0.2 +tau_pct = 0.3 +delay_ticks_max = 3 +friction_pct = 0.5 + +[preset.bench_loaded.bench] +max_rpm = 2500.0 +max_temp_c = 70 +trial_timeout_ms = 8000 +between_trials_ms = 800 +startup_timeout_ms = 5000 +continue_on_abort = false +validate_runs = 3 + +# ---------------------------------------------------------------- 实战验收：双向 +# 上车前的最后一道关：升速 → 保持 → 降速 → 保持（回零），验的是双向跟随与 +# 回零能力，比单向斜坡更接近实战。样本很少（围绕现有增益做局部复算）， +# 因为此时**不该**再大改增益，只想确认现有一组在两个方向上都不出问题。 +[preset.field_updown] +description = "真机实战验收 · 升降速双向（回零）：小样本复验，超调与稳态误差收紧" +backend = "bench" + +[preset.field_updown.space] +kp = { min = 0.5, max = 30.0, scale = "log" } +ki = { min = 0.5, max = 100.0, scale = "log0" } +kd = { min = 0.01, max = 3.0, scale = "log0" } +feedforward = { min = 0.5, max = 2.5, scale = "log" } + +[preset.field_updown.scenario] +profile = "up-down" +target_rpm = 1200.0 +ramp_ms = 400.0 +hold_ms = 800.0 +dt_us = 1000.0 + +[preset.field_updown.cost] +# 双向场景里稳态误差（尤其回零）比调节时间更重要。 +settle = 1.0 +overshoot = 1.0 +iae = 1.0 +effort = 0.1 + +[preset.field_updown.acceptance] +max_overshoot_pct = 12.0 +max_ss_error_pct = 3.0 + +[preset.field_updown.search] +seed = 20260101 +samples = 16 +top_k = 2 +solver = "nelder-mead" +max_iters = 30 +particles = 12 +max_evals = 40 +threads = 1 + +[preset.field_updown.sim] +plant = "gm3508-bench" +ensemble = 8 +robust_weight = 0.5 +gain_pct = 0.2 +tau_pct = 0.3 +delay_ticks_max = 3 +friction_pct = 0.5 + +[preset.field_updown.bench] +max_rpm = 2500.0 +max_temp_c = 70 +# 双向场景总时长翻倍，超时也翻倍。 +trial_timeout_ms = 10000 +between_trials_ms = 1000 +startup_timeout_ms = 5000 +continue_on_abort = false +# 验收阶段多跑几次，重复性不过关就不签字。 +validate_runs = 5 diff --git a/crates/chassis-bench/Cargo.toml b/crates/chassis-bench/Cargo.toml new file mode 100644 index 0000000..05a2efd --- /dev/null +++ b/crates/chassis-bench/Cargo.toml @@ -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"] } diff --git a/crates/chassis-bench/src/bin/chassis-spin.rs b/crates/chassis-bench/src/bin/chassis-spin.rs new file mode 100644 index 0000000..cfbe48d --- /dev/null +++ b/crates/chassis-bench/src/bin/chassis-spin.rs @@ -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, + + /// 各轮安装方向 ±1（默认左右镜像 → 四轮物理同向）。 + #[arg(long, value_delimiter = ',', allow_hyphen_values = true, default_value = "1,-1,1,-1")] + directions: Vec, + + /// 目标**转子**转速（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, +} + +/// 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 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 { + 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> { + 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> { + let motors = bus.motors(); + wait_for_feedback(motors, Duration::from_millis(args.startup_timeout_ms)).await; + + let targets: Vec = motors.iter().filter(|m| is_fresh(m)).cloned().collect(); + let missing: Vec = motors.iter().filter(|m| !is_fresh(m)).map(MotorFeedback::id).collect(); + + if !missing.is_empty() { + eprintln!( + "✗ 以下电调没有新鲜反馈：{missing:?}（收到 {:?}）", + motors.iter().map(MotorFeedback::feedback_age).collect::>() + ); + 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::>() + ); + + 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![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 = + motors.iter().map(|m| format!("{:>7.1}", m.state().rotor_speed_rpm)).collect(); + let current: Vec = + motors.iter().map(|m| format!("{:>6}", m.state().effort_raw)).collect(); + let temp: Vec = + 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]) -> 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::() / samples.len() as f32 +} diff --git a/crates/dji-esc-socketcan/examples/spin.rs b/crates/chassis-bench/src/bin/socketcan-spin.rs similarity index 65% rename from crates/dji-esc-socketcan/examples/spin.rs rename to crates/chassis-bench/src/bin/socketcan-spin.rs index d7861f0..965e1fc 100644 --- a/crates/dji-esc-socketcan/examples/spin.rs +++ b/crates/chassis-bench/src/bin/socketcan-spin.rs @@ -1,14 +1,14 @@ //! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。 //! //! ```sh -//! spin [interface] [esc_id] [rotor_rpm] [duration_secs] +//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs] //! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒 -//! cargo run --release -p dji-esc-socketcan --example spin -- can0 1 1000 10 +//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10 //! ``` //! -//! 关键点在 [`spin_one`]：它只依赖 `motor_core::Motor` 抽象，完全不知道 +//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道 //! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码 -//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（本 crate）在此汇合。 +//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。 //! //! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。 @@ -22,7 +22,7 @@ use dji_esc::{BusOptions, EscBus, EscConfig}; #[cfg(target_os = "linux")] use dji_esc_socketcan::open_link; #[cfg(target_os = "linux")] -use motor_core::{Disable, EffortDrive, Motor, MotorFeedback, VelocityDrive}; +use motor_core::Motor; #[cfg(target_os = "linux")] use tokio::time::{MissedTickBehavior, interval}; @@ -63,13 +63,22 @@ async fn main() { /// 验证目标：本函数只面向 [`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 ticker = interval(Duration::from_millis(100)); - ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + 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 { @@ -78,24 +87,26 @@ async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) { println!("\n收到 ctrl-c"); break; } - _ = ticker.tick() => {} + _ = 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; } - 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 接线"); - } } motor.disable(); diff --git a/crates/chassis-bench/src/lib.rs b/crates/chassis-bench/src/lib.rs new file mode 100644 index 0000000..45fdff4 --- /dev/null +++ b/crates/chassis-bench/src/lib.rs @@ -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}; diff --git a/crates/chassis-bench/src/link.rs b/crates/chassis-bench/src/link.rs new file mode 100644 index 0000000..a511f9b --- /dev/null +++ b/crates/chassis-bench/src/link.rs @@ -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 { + 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 { + dji_esc_socketcan::open_link(interface) +} + +/// 非 Linux 平台没有 SocketCAN。 +/// +/// # Errors +/// +/// 恒返回 `Unsupported`。 +#[cfg(not(target_os = "linux"))] +pub fn open_socketcan(interface: &str) -> Result { + 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)); + } +} diff --git a/crates/chassis-bench/src/safety.rs b/crates/chassis-bench/src/safety.rs new file mode 100644 index 0000000..798e761 --- /dev/null +++ b/crates/chassis-bench/src/safety.rs @@ -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, + /// 阈值（毫秒）。 + 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, + /// 温度上限（℃）。 + pub max_temperature_c: Option, + /// 反馈新鲜度阈值：超过它即视为掉线。 + pub feedback_timeout: Option, + /// 过速需要持续多久才判定为失控（抑制单帧噪声）。 + pub overspeed_grace: Duration, + /// 单次试验的总时长上限。 + pub trial_timeout: Option, +} + +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, +} + +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(&mut self, motors: &[M], now: Instant) -> Option { + 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) -> 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:?}"); + } +} diff --git a/crates/chassis-bench/src/sampler.rs b/crates/chassis-bench/src/sampler.rs new file mode 100644 index 0000000..a3f510d --- /dev/null +++ b/crates/chassis-bench/src/sampler.rs @@ -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, +} + +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( + motor: &M, + scenario: &Scenario, + envelope: &SafetyEnvelope, + effort_limit_raw: i16, + lease_period: Duration, + cancel: F, +) -> TrialOutcome +where + M: VelocityDrive + MotorFeedback + Disable, + F: Future, +{ + 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( + 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, "取消后不得继续采样"); + } +} diff --git a/crates/chassis-config/Cargo.toml b/crates/chassis-config/Cargo.toml new file mode 100644 index 0000000..db4607e --- /dev/null +++ b/crates/chassis-config/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "chassis-config" +description = "chassis.toml schema: parsing, semantic validation, and mapping to driver configuration" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +dji-esc = { workspace = true } +mecanum = { workspace = true } +motor-core = { workspace = true } +serde = { workspace = true } +toml = { workspace = true } +thiserror = { workspace = true } diff --git a/crates/chassisd/src/config.rs b/crates/chassis-config/src/lib.rs similarity index 63% rename from crates/chassisd/src/config.rs rename to crates/chassis-config/src/lib.rs index 23d013d..e761620 100644 --- a/crates/chassisd/src/config.rs +++ b/crates/chassis-config/src/lib.rs @@ -1,10 +1,19 @@ -//! `chassis.toml` 配置模型与加载。 +//! `chassis.toml` 配置模型：解析、语义校验与到驱动配置的映射。 +//! +//! 本 crate 是**配置 schema 的唯一来源**：`chassisd`（守护进程）与 +//! `chassis-tune`（自动整定）读同一份类型定义，因此整定工具写回的 +//! `[[motor]]` 片段必然能被守护进程读回，不需要两份 schema 手工同步。 +//! +//! 全部类型都实现 `Serialize`，所以 [`Config::to_toml_string`] 可以把 +//! （可能已被整定工具改写过的）配置原样落盘。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] use std::time::Duration; use dji_esc::{BusOptions, EscConfig, EscModel}; use mecanum::Mecanum; -use serde::Deserialize; +use motor_core::VelocityGains; +use serde::{Deserialize, Serialize}; use thiserror::Error; /// 配置错误。 @@ -19,7 +28,7 @@ pub enum ConfigError { } /// 顶层配置。 -#[derive(Debug, Clone, PartialEq, Deserialize)] +#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] pub struct Config { /// CAN 配置。 @@ -37,7 +46,7 @@ pub struct Config { /// CAN 接口配置（双 CAN 总线 seam）。 /// /// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。 -#[derive(Debug, Clone, PartialEq, Eq, Deserialize)] +#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] pub struct CanConfig { /// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。 @@ -51,7 +60,7 @@ pub struct CanConfig { } /// 功能总线配置（双 CAN 之二）。 -#[derive(Debug, Clone, PartialEq, Eq, Deserialize)] +#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] pub struct FunctionBusConfig { /// SocketCAN 接口名（如 `can1`）。 @@ -82,7 +91,7 @@ impl ProtocolFamily { } /// 底盘控制参数（全部可省略，采用默认值）。 -#[derive(Debug, Clone, PartialEq, Deserialize)] +#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)] #[serde(deny_unknown_fields, default)] pub struct ChassisConfig { /// 控制循环周期（毫秒）。 @@ -91,6 +100,11 @@ pub struct ChassisConfig { pub command_timeout_ms: u64, /// 电机反馈离线阈值（毫秒）。 pub feedback_timeout_ms: u64, + /// 驱动层**命令租约**（毫秒）：控制循环超过该时长未刷新电机命令 → + /// 驱动强制 0 电流（防控制任务死亡后旧指令持续输出）。 + /// + /// 必须 ≥ `control_period_ms`；默认 1kHz 下约 50 拍。 + pub motor_command_timeout_ms: u64, /// 最大前进 / 横移速度（m/s）。 pub max_vx: f32, /// 最大横移速度（m/s）。 @@ -105,6 +119,7 @@ impl Default for ChassisConfig { control_period_ms: 1, command_timeout_ms: 200, feedback_timeout_ms: 100, + motor_command_timeout_ms: 50, max_vx: 3.0, max_vy: 3.0, max_omega: 6.0, @@ -113,7 +128,7 @@ impl Default for ChassisConfig { } /// 麦轮几何（米）。 -#[derive(Debug, Clone, Copy, PartialEq, Deserialize)] +#[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] pub struct MecanumConfig { /// 前后轮轴线到中心的纵向半距。 @@ -125,7 +140,7 @@ pub struct MecanumConfig { } /// 单电机配置。 -#[derive(Debug, Clone, PartialEq, Deserialize)] +#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] pub struct MotorConfig { /// 便于日志辨认的名字（`front_left` 等）。 @@ -170,7 +185,7 @@ fn default_current_limit() -> i16 { } /// 配置文件中的电调型号。 -#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize)] +#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)] #[serde(deny_unknown_fields, rename_all = "UPPERCASE")] pub enum EscModelSerde { /// C620（GM3508）。 @@ -188,6 +203,22 @@ impl From for EscModel { } } +impl MotorConfig { + /// 速度环增益组（可整定参数）。 + #[must_use] + pub fn velocity_gains(&self) -> VelocityGains { + VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward) + } + + /// 覆写速度环增益组，其余字段（限流、方向、减速比）保持不变。 + pub fn set_velocity_gains(&mut self, gains: VelocityGains) { + self.kp = gains.kp; + self.ki = gains.ki; + self.kd = gains.kd; + self.feedforward = gains.feedforward; + } +} + impl Config { /// 从 TOML 文本解析并校验。 /// @@ -211,6 +242,69 @@ impl Config { Self::from_toml(&text) } + /// 序列化回 TOML 文本。 + /// + /// 与 [`Config::from_toml`] 往返等价（`from_toml(to_toml_string(c)) == c`）， + /// 因此整定工具改写增益后可以直接落盘成守护进程能读的配置。 + /// + /// # Errors + /// + /// 序列化失败（类型定义与 TOML 数据模型冲突时才会发生，属实现缺陷）。 + pub fn to_toml_string(&self) -> Result { + toml::to_string(self).map_err(|e| ConfigError::Parse(e.to_string())) + } + + /// 把配置写到文件。 + /// + /// # Errors + /// + /// 序列化失败或写入失败。 + pub fn save(&self, path: &std::path::Path) -> Result<(), ConfigError> { + let text = self.to_toml_string()?; + std::fs::write(path, text) + .map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display()))) + } + + /// 按名字查找电机下标。 + #[must_use] + pub fn motor_index(&self, name: &str) -> Option { + self.motors.iter().position(|m| m.name == name) + } + + /// 覆写第 `index` 台电机的速度环增益。 + /// + /// # Errors + /// + /// 下标越界时返回 [`ConfigError::Invalid`]。 + pub fn set_motor_gains( + &mut self, + index: usize, + gains: VelocityGains, + ) -> Result<(), ConfigError> { + let count = self.motors.len(); + let motor = self.motors.get_mut(index).ok_or_else(|| { + ConfigError::Invalid(format!("motor index {index} out of 0..{count}")) + })?; + motor.set_velocity_gains(gains); + Ok(()) + } + + /// 按轮名覆写速度环增益（整定工具按名字写回时用）。 + /// + /// # Errors + /// + /// 名字不存在时返回 [`ConfigError::Invalid`]。 + pub fn set_motor_gains_by_name( + &mut self, + name: &str, + gains: VelocityGains, + ) -> Result<(), ConfigError> { + let index = self + .motor_index(name) + .ok_or_else(|| ConfigError::Invalid(format!("no motor named '{name}'")))?; + self.set_motor_gains(index, gains) + } + /// 语义校验。 /// /// # Errors @@ -228,11 +322,21 @@ impl Config { return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}"))); } let c = &self.chassis; - if c.control_period_ms == 0 || c.command_timeout_ms == 0 || c.feedback_timeout_ms == 0 { + if c.control_period_ms == 0 + || c.command_timeout_ms == 0 + || c.feedback_timeout_ms == 0 + || c.motor_command_timeout_ms == 0 + { return Err(ConfigError::Invalid( - "control/command/feedback timeouts must be > 0 ms".into(), + "control/command/feedback/motor-command timeouts must be > 0 ms".into(), )); } + if c.motor_command_timeout_ms < c.control_period_ms { + return Err(ConfigError::Invalid(format!( + "motor_command_timeout_ms ({}) must be >= control_period_ms ({})", + c.motor_command_timeout_ms, c.control_period_ms + ))); + } if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) { return Err(ConfigError::Invalid("speed limits must be > 0".into())); } @@ -254,13 +358,13 @@ impl Config { )); } // 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。 - if let Some(function) = &self.can.function { - if function.interface == self.can.interface { - return Err(ConfigError::Invalid(format!( - "function bus interface must differ from chassis interface (both '{}')", - self.can.interface - ))); - } + if let Some(function) = &self.can.function + && function.interface == self.can.interface + { + return Err(ConfigError::Invalid(format!( + "function bus interface must differ from chassis interface (both '{}')", + self.can.interface + ))); } // 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。 Ok(()) @@ -321,6 +425,7 @@ impl Config { BusOptions { period: Duration::from_millis(self.chassis.control_period_ms), feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms), + command_timeout: Duration::from_millis(self.chassis.motor_command_timeout_ms), } } @@ -501,4 +606,78 @@ gear_ratio = 19.2 let function = cfg.can.function.as_ref().unwrap(); assert_eq!(function.interface, "can1"); } + + #[test] + fn toml_round_trip_is_lossless() { + let original = Config::from_toml(EXAMPLE).unwrap(); + let text = original.to_toml_string().unwrap(); + let reparsed = Config::from_toml(&text).unwrap(); + assert_eq!(reparsed, original); + // 落盘文本必须仍然是合法配置（守护进程直接读它）。 + assert!(text.contains("[[motor]]"), "{text}"); + assert!(text.contains("interface = \"can0\""), "{text}"); + } + + #[test] + fn round_trip_preserves_function_bus_and_timeouts() { + let mut original = Config::mock_default(); + original.can.function = Some(FunctionBusConfig { interface: "can1".into() }); + original.chassis.motor_command_timeout_ms = 37; + let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap(); + assert_eq!(reparsed, original); + assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37); + assert_eq!(reparsed.can.function.unwrap().interface, "can1"); + } + + #[test] + fn motor_gains_can_be_read_and_written_by_name() { + let mut cfg = Config::from_toml(EXAMPLE).unwrap(); + assert_eq!(cfg.motor_index("rear_left"), Some(2)); + assert_eq!(cfg.motor_index("nope"), None); + assert_eq!(cfg.motors[2].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0)); + + let tuned = VelocityGains::new(2.0, 10.0, 0.1, 0.0); + cfg.set_motor_gains_by_name("rear_left", tuned).unwrap(); + assert_eq!(cfg.motors[2].velocity_gains(), tuned); + // 其余电机不受影响。 + assert_eq!(cfg.motors[0].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0)); + // 机械 / 安全字段保持不变。 + assert_eq!(cfg.motors[2].current_limit, 12_000); + assert!((cfg.motors[2].gear_ratio - 19.203).abs() < 1e-6); + assert!(cfg.validate().is_ok()); + } + + #[test] + fn motor_gains_write_back_survives_round_trip() { + let mut cfg = Config::mock_default(); + let tuned = VelocityGains::new(3.5, 22.0, 0.25, 0.75); + cfg.set_motor_gains(0, tuned).unwrap(); + let reparsed = Config::from_toml(&cfg.to_toml_string().unwrap()).unwrap(); + assert_eq!(reparsed.motors[0].velocity_gains(), tuned); + } + + #[test] + fn motor_gains_out_of_range_is_rejected() { + let mut cfg = Config::mock_default(); + assert!(matches!( + cfg.set_motor_gains(9, VelocityGains::ZERO), + Err(ConfigError::Invalid(_)) + )); + assert!(matches!( + cfg.set_motor_gains_by_name("ghost", VelocityGains::ZERO), + Err(ConfigError::Invalid(_)) + )); + } + + #[test] + fn save_and_load_file_round_trip() { + let dir = std::env::temp_dir().join("chassis-config-roundtrip"); + std::fs::create_dir_all(&dir).unwrap(); + let path = dir.join("chassis.toml"); + let original = Config::mock_default(); + original.save(&path).unwrap(); + let loaded = Config::load(&path).unwrap(); + assert_eq!(loaded, original); + std::fs::remove_file(&path).unwrap(); + } } diff --git a/crates/chassis-tune/Cargo.toml b/crates/chassis-tune/Cargo.toml new file mode 100644 index 0000000..f55d33e --- /dev/null +++ b/crates/chassis-tune/Cargo.toml @@ -0,0 +1,42 @@ +[package] +name = "chassis-tune" +description = "Automated PID gain search for the chassis velocity loop: offline sweep/refine plus bench (hardware-in-the-loop) verification" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[features] +default = [] +# 可选：用 plotters 输出 best.svg（默认关闭，避免 CI 依赖系统字体与更长编译）。 +plot = ["dep:plotters"] + +[dependencies] +can-link = { workspace = true } +chassis-bench = { workspace = true } +chassis-config = { workspace = true } +tokio = { workspace = true } +dji-esc = { workspace = true } +dji-esc-sim = { workspace = true } +motor-core = { workspace = true } +argmin = { workspace = true } +argmin-math = { workspace = true } +rand = { workspace = true } +rayon = { workspace = true } +indicatif = { workspace = true } +csv = { workspace = true } +serde = { workspace = true } +serde_json = { workspace = true } +toml = { workspace = true } +clap = { workspace = true } +anyhow = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } +tracing-subscriber = { workspace = true } +plotters = { workspace = true, optional = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/chassis-tune/src/backend.rs b/crates/chassis-tune/src/backend.rs new file mode 100644 index 0000000..aee0673 --- /dev/null +++ b/crates/chassis-tune/src/backend.rs @@ -0,0 +1,173 @@ +//! 评估后端：把"一组增益有多好"这件事抽象出来。 +//! +//! - [`SimEvaluator`]：离线 harness + 被控对象集成（域随机化），CPU 密集、可并行； +//! - [`BenchEvaluator`](crate::bench::BenchEvaluator)：真机台架（`chassis-bench`），单轮、串行、有安全包络。 +//! +//! 搜索器只认识 [`Evaluator`] trait，因此"先仿真粗调、再上台架复验"是同一套 +//! 代码换一个后端，而不是两份实现。 + +use dji_esc_sim::{LoopLimits, PlantEnsemble, Scenario, simulate}; +use motor_core::VelocityGains; + +use crate::cost::{Objective, Score, Verdict}; + +pub use crate::cost::Weights; + +/// 一次评估的结果。 +pub type Evaluated = Score; + +/// 被控对象评估后端。 +/// +/// 必须 `Send + Sync`：粗扫阶段由 `rayon` 并行调用。 +pub trait Evaluator: Send + Sync { + /// 评估一组增益。 + fn evaluate(&self, gains: &VelocityGains) -> Evaluated; + /// 一行描述（写进报告，便于复现）。 + fn describe(&self) -> String; +} + +/// 离线评估：在一组被控对象参数上重放同一条场景。 +#[derive(Debug, Clone)] +pub struct SimEvaluator { + /// 被控对象集成（域随机化）。 + pub ensemble: PlantEnsemble, + /// 评价场景。 + pub scenario: Scenario, + /// 限幅。 + pub limits: LoopLimits, + /// 硬约束与权重。 + pub objective: Objective, + /// 集成聚合权重：0 = 只看均值，1 = 只看最坏成员。 + pub robust_weight: f64, +} + +impl SimEvaluator { + /// 组装。 + #[must_use] + pub fn new( + ensemble: PlantEnsemble, + scenario: Scenario, + limits: LoopLimits, + objective: Objective, + robust_weight: f64, + ) -> Self { + Self { ensemble, scenario, limits, objective, robust_weight: robust_weight.clamp(0.0, 1.0) } + } + + /// 集成规模。 + #[must_use] + pub fn ensemble_size(&self) -> usize { + self.ensemble.len() + } +} + +impl Evaluator for SimEvaluator { + fn evaluate(&self, gains: &VelocityGains) -> Evaluated { + let mut costs = Vec::with_capacity(self.ensemble.len()); + let mut nominal: Option = None; + let mut worst_verdict = Verdict::Ok; + + for (index, plant) in self.ensemble.iter().enumerate() { + let score = match simulate(*gains, plant, &self.scenario, self.limits) { + Ok(trace) => self.objective.score(&trace), + Err(error) => Score::penalized(Objective::from_error(error)), + }; + if index == 0 { + nominal = Some(score); + } + if !score.verdict.is_ok() && worst_verdict.is_ok() { + worst_verdict = score.verdict; + } + costs.push(score.cost); + } + + let count = costs.len() as f64; + let mean = costs.iter().sum::() / count; + let worst = costs.iter().copied().fold(f64::NEG_INFINITY, f64::max); + let cost = (1.0 - self.robust_weight) * mean + self.robust_weight * worst; + + // 判定取"名义成员优先，其次最差成员"：名义模型都没收敛时不必再看抖动。 + // 指标始终报名义成员的——报告要展示的是一条可复现的曲线。 + let nominal = nominal.unwrap_or_else(|| Score::penalized(Verdict::Rejected)); + let verdict = if nominal.verdict.is_ok() { worst_verdict } else { nominal.verdict }; + Score { cost, verdict, metrics: nominal.metrics } + } + + fn describe(&self) -> String { + format!( + "sim(ensemble={}, robust_weight={:.2}, scenario={:?}, target={} rpm, limit={} raw, {})", + self.ensemble.len(), + self.robust_weight, + self.scenario.profile, + self.scenario.target_rpm, + self.limits.current_limit_raw, + self.objective.weights.describe() + ) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::cost::Weights; + use dji_esc_sim::{PlantParams, PlantTolerances}; + + fn evaluator(ensemble_size: usize, robust_weight: f64) -> SimEvaluator { + let scenario = Scenario::step(1500.0, 2.0); + let objective = Objective::new(Weights::default(), scenario, LoopLimits::gm3508()); + let ensemble = PlantEnsemble::randomized( + PlantParams::gm3508_bench(), + PlantTolerances::default(), + ensemble_size, + 11, + ); + SimEvaluator::new(ensemble, scenario, LoopLimits::gm3508(), objective, robust_weight) + } + + #[test] + fn good_gains_beat_bad_gains_offline() { + let evaluator = evaluator(4, 0.5); + let good = evaluator.evaluate(&VelocityGains::new(4.0, 12.0, 0.0, 0.0)); + let bad = evaluator.evaluate(&VelocityGains::new(0.01, 0.0, 0.0, 0.0)); + assert!(good.cost < bad.cost, "good = {}, bad = {}", good.cost, bad.cost); + assert_eq!(good.verdict, Verdict::Ok, "{good:?}"); + assert_eq!(bad.verdict, Verdict::NoSettle, "{bad:?}"); + } + + #[test] + fn non_finite_gains_are_rejected_without_panicking() { + let evaluator = evaluator(2, 0.0); + let score = evaluator.evaluate(&VelocityGains::new(f32::NAN, 0.0, 0.0, 0.0)); + assert_eq!(score.verdict, Verdict::Rejected); + assert!((score.cost - crate::cost::PENALTY).abs() < 1.0); + assert!(score.metrics.is_none()); + } + + #[test] + fn evaluation_is_deterministic() { + let evaluator = evaluator(4, 0.5); + let gains = VelocityGains::new(3.0, 9.0, 0.1, 0.5); + let a = evaluator.evaluate(&gains); + let b = evaluator.evaluate(&gains); + assert_eq!(a.cost.to_bits(), b.cost.to_bits()); + assert_eq!(a.verdict, b.verdict); + } + + #[test] + fn robust_weight_shifts_toward_the_worst_member() { + let mean_only = evaluator(6, 0.0); + let worst_only = evaluator(6, 1.0); + let gains = VelocityGains::new(1.0, 6.0, 0.0, 0.0); + let mean = mean_only.evaluate(&gains); + let worst = worst_only.evaluate(&gains); + assert!(worst.cost >= mean.cost, "worst = {}, mean = {}", worst.cost, mean.cost); + } + + #[test] + fn describe_mentions_the_configuration() { + let text = evaluator(3, 0.25).describe(); + assert!(text.contains("ensemble=3"), "{text}"); + assert!(text.contains("robust_weight=0.25"), "{text}"); + assert!(text.contains("1500 rpm"), "{text}"); + } +} diff --git a/crates/chassis-tune/src/bench.rs b/crates/chassis-tune/src/bench.rs new file mode 100644 index 0000000..326b895 --- /dev/null +++ b/crates/chassis-tune/src/bench.rs @@ -0,0 +1,383 @@ +//! 真机（HIL）评估后端：在台架上逐次试验一组增益。 +//! +//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]， +//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动， +//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。 +//! +//! # 安全设计 +//! +//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）； +//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c， +//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。 +//! +//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池 +//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。 + +use std::sync::Mutex; +use std::time::Duration; + +use anyhow::Context as _; +use chassis_bench::{Abort, SafetyEnvelope, run_step_trial}; +use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor}; +use dji_esc_sim::{LoopLimits, Scenario, Trace}; +use motor_core::{Disable as _, MotorFeedback, VelocityGains}; +use thiserror::Error; + +use crate::backend::Evaluator; +use crate::cost::{Objective, Score, Verdict}; + +/// 真机后端错误。 +#[derive(Debug, Error)] +pub enum BenchError { + /// 无法打开总线。 + #[error("cannot open CAN link: {0}")] + Link(String), + /// 电调总线启动失败。 + #[error("cannot start ESC bus: {0}")] + Bus(String), + /// 启动超时：目标电调没有反馈。 + #[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")] + NoFeedback { + /// 目标电调 ID。 + esc_id: u8, + /// 等待时长（毫秒）。 + waited_ms: u64, + }, + /// 搜索期间因安全事件中止且未要求继续。 + #[error("aborted by safety envelope: {reason}")] + Aborted { + /// 触发原因。 + reason: String, + }, +} + +/// 真机评估配置。 +#[derive(Debug, Clone)] +pub struct BenchConfig { + /// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。 + pub esc: EscConfig, + /// 总线运行参数。 + pub bus: BusOptions, + /// 评估场景。 + pub scenario: Scenario, + /// 安全包络。 + pub envelope: SafetyEnvelope, + /// 硬约束与权重。 + pub objective: Objective, + /// 试验之间的静默期（等轮子停稳）。 + pub between_trials: Duration, + /// 命令租约续期节拍。 + pub lease_period: Duration, + /// 启动时等待反馈的超时。 + pub startup_timeout: Duration, + /// 安全检查触发后是否继续搜索（默认 false = 立即停）。 + pub continue_on_abort: bool, +} + +impl BenchConfig { + /// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。 + #[must_use] + pub fn new( + esc: EscConfig, + scenario: Scenario, + envelope: SafetyEnvelope, + objective: Objective, + ) -> Self { + Self { + esc, + bus: BusOptions::default(), + scenario, + envelope, + objective, + between_trials: Duration::from_millis(500), + lease_period: Duration::from_millis(5), + startup_timeout: Duration::from_secs(5), + continue_on_abort: false, + } + } +} + +/// 真机评估器（持有总线与运行时）。 +pub struct BenchEvaluator { + session: Mutex, + scenario: Scenario, + envelope: SafetyEnvelope, + objective: Objective, + effort_limit_raw: i16, + between_trials: Duration, + lease_period: Duration, + continue_on_abort: bool, + /// 目前见过的最优轨迹（报告用）。 + best: Mutex>, + /// 中止记录（报告用）。 + aborts: Mutex>, + describe: String, +} + +struct Session { + runtime: tokio::runtime::Runtime, + bus: Option, + motor: EscMotor, +} + +impl BenchEvaluator { + /// 打开链路、启动总线并等待目标电调反馈。 + /// + /// # Errors + /// + /// 链路 / 总线启动失败，或启动超时（电调无反馈）。 + pub fn open(link: can_link::CanLink, config: BenchConfig) -> Result { + let esc_id = config.esc.esc_id; + let limits = + LoopLimits { current_limit_raw: config.esc.current_limit, model: config.esc.model }; + let runtime = tokio::runtime::Builder::new_multi_thread() + .worker_threads(2) + .enable_all() + .build() + .map_err(|e| BenchError::Bus(e.to_string()))?; + + let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone()) + .map_err(|e| BenchError::Bus(e.to_string()))?; + let motor = bus + .motor(esc_id) + .ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))? + .clone(); + + let startup_timeout = config.startup_timeout; + let ready = runtime.block_on(async { + let deadline = tokio::time::Instant::now() + startup_timeout; + loop { + if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) { + return true; + } + if tokio::time::Instant::now() >= deadline { + return false; + } + tokio::time::sleep(Duration::from_millis(20)).await; + } + }); + let evaluator = Self { + session: Mutex::new(Session { runtime, bus: Some(bus), motor }), + scenario: config.scenario, + envelope: config.envelope, + objective: config.objective, + effort_limit_raw: limits.current_limit_raw, + between_trials: config.between_trials, + lease_period: config.lease_period, + continue_on_abort: config.continue_on_abort, + best: Mutex::new(None), + aborts: Mutex::new(Vec::new()), + describe: format!( + "bench(esc={esc_id}, name={}, limit={} raw, target={} rpm, trial={:.1}s, gap={}ms)", + config.esc.name, + limits.current_limit_raw, + config.scenario.target_rpm, + config.scenario.total_s(), + config.between_trials.as_millis() + ), + }; + if !ready { + // 启动失败也必须走完整停机路径：先 0 电流，再关总线。 + evaluator.shutdown(); + return Err(BenchError::NoFeedback { + esc_id, + waited_ms: startup_timeout.as_millis() as u64, + }); + } + Ok(evaluator) + } + + /// 最优轨迹（若有）。 + #[must_use] + pub fn best_trace(&self) -> Option { + self.best + .lock() + .unwrap_or_else(std::sync::PoisonError::into_inner) + .as_ref() + .map(|(_, trace)| trace.clone()) + } + + /// 搜索期间的安全中止记录。 + #[must_use] + pub fn aborts(&self) -> Vec { + self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).clone() + } + + /// 停机并关闭总线（**必须**在结束前调用）。 + pub fn shutdown(&self) { + let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + let Some(bus) = session.bus.take() else { + return; + }; + for motor in bus.motors() { + motor.disable(); + } + session.runtime.block_on(async { + tokio::time::sleep(Duration::from_millis(50)).await; + bus.shutdown().await; + }); + } + + /// 一次真机试验（含试验间隔静默）。 + fn run_trial(&self, gains: &VelocityGains) -> Score { + let mut session = self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + let Session { runtime, motor, .. } = &mut *session; + motor.set_velocity_gains(*gains); + let scenario = self.scenario; + let envelope = self.envelope.clone(); + let limit = self.effort_limit_raw; + let lease = self.lease_period; + let between = self.between_trials; + + let outcome = runtime.block_on(async { + let trial = + run_step_trial(motor, &scenario, &envelope, limit, lease, std::future::pending()) + .await; + // 试验间隔：让轮子真正停下来，再让下一次试验的起点一致。 + tokio::time::sleep(between).await; + trial + }); + + if let Some(reason) = outcome.abort { + let verdict = verdict_for(&reason); + let message = format!("{}: {reason}", self.describe); + self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).push(message); + return Score::penalized(verdict); + } + + let score = match crate::metrics::StepMetrics::from_trace(&outcome.trace, &self.scenario) { + Some(metrics) => self.objective.score_metrics(metrics), + None => Score::penalized(Verdict::Rejected), + }; + // 记录目前最优的轨迹，供报告画图 / 复盘。 + let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + if best.as_ref().is_none_or(|(cost, _)| score.cost < *cost) { + *best = Some((score.cost, outcome.trace)); + } + score + } +} + +impl Drop for BenchEvaluator { + fn drop(&mut self) { + self.shutdown(); + } +} + +impl Evaluator for BenchEvaluator { + fn evaluate(&self, gains: &VelocityGains) -> Score { + let score = self.run_trial(gains); + if !score.verdict.is_ok() && !self.continue_on_abort { + // 真机上出现安全事件时不"带伤继续搜索"：交给调用方在 sweep 之后 + // 检查 `aborts()` 并决定是否收工。这里只记录，不 panic。 + tracing::warn!(verdict = score.verdict.label(), "bench trial did not pass"); + } + score + } + + fn describe(&self) -> String { + self.describe.clone() + } +} + +/// 安全事件 → 成本判定。 +/// +/// 过速 / 过温说明控制器失稳；掉线 / 超时 / 取消说明本次试验没做完， +/// 两者的结果都不可用，但原因不同——报告要能区分。 +#[must_use] +pub fn verdict_for(abort: &Abort) -> Verdict { + match abort { + Abort::Overspeed { .. } | Abort::OverTemperature { .. } => Verdict::Overspeed, + Abort::FeedbackLost { .. } | Abort::Timeout { .. } | Abort::Cancelled => Verdict::Rejected, + } +} + +/// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。 +/// +/// # Errors +/// +/// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。 +pub fn open_link( + device: &str, + serial_baud: u32, + can_speed: chassis_bench::CanSpeed, + interface: Option<&str>, +) -> Result { + let result = match interface { + Some(interface) => chassis_bench::open_socketcan(interface), + None => chassis_bench::open_slcan( + device, + chassis_bench::default_slcan_options(serial_baud, can_speed), + ), + }; + result + .with_context(|| match interface { + Some(interface) => format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）"), + None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"), + }) + .map_err(|e| BenchError::Link(e.to_string())) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::cost::Weights; + + fn config() -> BenchConfig { + let mut esc = EscConfig::gm3508("bench", 1, 1.0); + esc.current_limit = 1500; + let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15); + let objective = Objective::new( + Weights::default(), + scenario, + LoopLimits { current_limit_raw: esc.current_limit, model: esc.model }, + ); + let envelope = SafetyEnvelope::new() + .with_overspeed(1200.0, Duration::from_millis(60)) + .with_temperature_max(70) + .with_feedback_timeout(Duration::from_millis(200)) + .with_trial_timeout(Duration::from_secs(2)); + BenchConfig::new(esc, scenario, envelope, objective) + } + + #[test] + fn config_defaults_are_safe() { + let config = config(); + assert_eq!(config.bus.period, Duration::from_millis(1)); + assert_eq!(config.bus.command_timeout, Duration::from_millis(50)); + assert!(!config.continue_on_abort, "默认必须在安全事件后收工"); + assert!(config.between_trials >= Duration::from_millis(200)); + assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约"); + } + + #[test] + fn every_abort_maps_to_a_definite_verdict() { + let cases = [ + Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 }, + Abort::OverTemperature { temperature_c: 90, limit_c: 70 }, + Abort::FeedbackLost { esc_id: 1, age_ms: None, timeout_ms: 200 }, + Abort::Timeout { elapsed_ms: 6000 }, + Abort::Cancelled, + ]; + for abort in cases { + let verdict = verdict_for(&abort); + assert!(!verdict.is_ok(), "{abort:?} 不得被判为达标"); + } + assert_eq!( + verdict_for(&Abort::Overspeed { peak_rpm: 5000.0, limit_rpm: 1200.0, grace_ms: 60 }), + Verdict::Overspeed + ); + assert_eq!(verdict_for(&Abort::Cancelled), Verdict::Rejected); + } + + #[test] + fn link_open_failures_are_errors() { + // 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。 + let result = + open_link("/definitely/not/a/device", 2_000_000, chassis_bench::CanSpeed::M1, None); + assert!(result.is_err(), "不存在的串口设备必须报错"); + let missing_interface = + open_link("ignored", 2_000_000, chassis_bench::CanSpeed::M1, Some("can-not-real")); + assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错"); + } +} diff --git a/crates/chassis-tune/src/cost.rs b/crates/chassis-tune/src/cost.rs new file mode 100644 index 0000000..f7f56bb --- /dev/null +++ b/crates/chassis-tune/src/cost.rs @@ -0,0 +1,302 @@ +//! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。 +//! +//! 两个原则： +//! +//! 1. **硬约束不用权重表达**。过速 / 长时间饱和 / 不收敛不是"差一点"， +//! 而是"不能用"——它们直接给罚值并记录判定，绝不允许被其他项的权重 +//! 抵消掉（否则搜索会主动挑一个会飞车的解）。 +//! 2. **各项归一化**。时间按轨迹时长归一、误差按 `|目标| × 时长` 归一、 +//! effort 按限幅归一，因此权重在不同场景 / 不同电机之间仍然可解释。 + +use dji_esc_sim::{LoopLimits, Scenario, SimError, Trace}; + +use crate::metrics::StepMetrics; + +/// 罚值：远大于任何合法成本，但保持有限（避免 NaN 传染给优化器）。 +pub const PENALTY: f64 = 1.0e9; + +/// 成本权重（各项已归一化到 O(1)）。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Weights { + /// 调节时间权重。 + pub settle: f64, + /// 超调权重。 + pub overshoot: f64, + /// 绝对误差积分（IAE）权重。 + pub iae: f64, + /// effort（电流）权重。 + pub effort: f64, +} + +impl Default for Weights { + fn default() -> Self { + // 底盘速度环：跟得上、不过冲最重要；电流只作为次要惩罚 + // （省电流不是目的，避免烧电调才是）。 + Self { settle: 1.0, overshoot: 1.0, iae: 0.5, effort: 0.1 } + } +} + +impl Weights { + /// 人类可读描述（日志 / 报告用）。 + #[must_use] + pub fn describe(&self) -> String { + format!( + "cost={:.2}·settle+{:.2}·overshoot+{:.2}·iae+{:.2}·effort", + self.settle, self.overshoot, self.iae, self.effort + ) + } + + /// 权重是否全部有限且非负。 + #[must_use] + pub fn is_valid(&self) -> bool { + [self.settle, self.overshoot, self.iae, self.effort] + .iter() + .all(|w| w.is_finite() && *w >= 0.0) + } +} + +/// 一次评估的判定。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum Verdict { + /// 达标（成本 = 加权指标）。 + Ok, + /// 增益含 NaN / inf，或场景退化（被拒绝，不进控制器）。 + Rejected, + /// 数值发散。 + Diverged, + /// 过速。 + Overspeed, + /// 饱和占比过高（积分绕死 / 限幅太紧）。 + Saturated, + /// 在场景内没有收敛。 + NoSettle, +} + +impl Verdict { + /// 是否达标。 + #[must_use] + pub fn is_ok(self) -> bool { + matches!(self, Self::Ok) + } + + /// 简短标签（报告 / CSV 用）。 + #[must_use] + pub fn label(self) -> &'static str { + match self { + Self::Ok => "ok", + Self::Rejected => "rejected", + Self::Diverged => "diverged", + Self::Overspeed => "overspeed", + Self::Saturated => "saturated", + Self::NoSettle => "no-settle", + } + } +} + +/// 一次评估的结果。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Score { + /// 成本（越小越好）。 + pub cost: f64, + /// 判定。 + pub verdict: Verdict, + /// 指标（被拒绝时为 `None`）。 + pub metrics: Option, +} + +impl Score { + /// 罚分结果。 + #[must_use] + pub fn penalized(verdict: Verdict) -> Self { + Self { cost: PENALTY, verdict, metrics: None } + } +} + +/// 目标函数：场景 + 限幅 + 权重 + 硬约束。 +#[derive(Debug, Clone, PartialEq)] +pub struct Objective { + /// 权重。 + pub weights: Weights, + /// 评价场景。 + pub scenario: Scenario, + /// 限幅与型号。 + pub limits: LoopLimits, + /// 过速硬上限（rpm）。 + pub max_rpm: f32, + /// 允许的饱和占比上限（0..1）。 + pub max_saturated_fraction: f32, +} + +impl Objective { + /// 默认硬约束：过速上限 = 目标 × 3 + 500；饱和占比 ≤ 40%。 + #[must_use] + pub fn new(weights: Weights, scenario: Scenario, limits: LoopLimits) -> Self { + let max_rpm = scenario.target_rpm.abs() * 3.0 + 500.0; + Self { weights, scenario, limits, max_rpm, max_saturated_fraction: 0.4 } + } + + /// 由仿真错误折算判定。 + #[must_use] + pub fn from_error(error: SimError) -> Verdict { + match error { + SimError::NonFiniteGains | SimError::DegenerateScenario => Verdict::Rejected, + SimError::Diverged { .. } => Verdict::Diverged, + } + } + + /// 对一条轨迹评分。 + #[must_use] + pub fn score(&self, trace: &Trace) -> Score { + let Some(metrics) = StepMetrics::from_trace(trace, &self.scenario) else { + return Score::penalized(Verdict::Rejected); + }; + self.score_metrics(metrics) + } + + /// 对一组指标评分（真机后端直接复用，保证两条路径同一把尺子）。 + #[must_use] + pub fn score_metrics(&self, metrics: StepMetrics) -> Score { + if metrics.max_rpm > self.max_rpm { + return Score { cost: PENALTY, verdict: Verdict::Overspeed, metrics: Some(metrics) }; + } + if metrics.saturated_pct / 100.0 > self.max_saturated_fraction { + return Score { cost: PENALTY, verdict: Verdict::Saturated, metrics: Some(metrics) }; + } + if !metrics.settled { + return Score { cost: PENALTY, verdict: Verdict::NoSettle, metrics: Some(metrics) }; + } + Score { cost: self.weighted_cost(&metrics), verdict: Verdict::Ok, metrics: Some(metrics) } + } + + /// 归一化加权成本。 + #[must_use] + pub fn weighted_cost(&self, metrics: &StepMetrics) -> f64 { + let total_s = f64::from(self.scenario.total_s().max(1.0e-3)); + let target = f64::from(self.scenario.target_rpm.abs()).max(1.0e-3); + let limit = f64::from(self.limits.current_limit_raw.max(1)); + + let settle = f64::from(metrics.settling_time_s) / total_s; + let overshoot = f64::from(metrics.overshoot_pct) / 100.0; + let iae = f64::from(metrics.iae) / (target * total_s); + let effort = f64::from(metrics.rms_effort_raw) / limit; + + self.weights.settle * settle + + self.weights.overshoot * overshoot + + self.weights.iae * iae + + self.weights.effort * effort + } + + /// 稳态误差是否满足验收阈值（报告 / 退出码用）。 + #[must_use] + pub fn passes( + &self, + metrics: &StepMetrics, + max_overshoot_pct: f32, + max_ss_error_pct: f32, + ) -> bool { + metrics.settled + && metrics.overshoot_pct <= max_overshoot_pct + && metrics.ss_error_pct <= max_ss_error_pct + } +} + +#[cfg(test)] +mod tests { + use super::*; + use dji_esc_sim::{PlantParams, simulate}; + use motor_core::VelocityGains; + + const GOOD: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0); + + fn objective(scenario: Scenario) -> Objective { + Objective::new(Weights::default(), scenario, LoopLimits::gm3508()) + } + + #[test] + fn good_gains_produce_a_finite_ok_cost() { + let scenario = Scenario::step(1500.0, 1.0); + let trace = + simulate(GOOD, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap(); + let score = objective(scenario).score(&trace); + assert_eq!(score.verdict, Verdict::Ok); + assert!(score.cost.is_finite() && score.cost < 10.0, "cost = {}", score.cost); + assert!(score.metrics.is_some()); + } + + #[test] + fn non_converging_gains_are_penalized_not_weighted() { + // 纯比例、极低增益：到不了目标。 + let scenario = Scenario::step(4000.0, 1.0); + let weak = VelocityGains::new(0.01, 0.0, 0.0, 0.0); + let trace = + simulate(weak, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap(); + let score = objective(scenario).score(&trace); + assert_eq!(score.verdict, Verdict::NoSettle); + assert!((score.cost - PENALTY).abs() < 1.0); + } + + #[test] + fn saturation_is_a_hard_constraint() { + let scenario = Scenario::step(9000.0, 1.0); + // 增益极大 + 限流很小 → 长时间饱和。 + let limits = LoopLimits { current_limit_raw: 800, model: dji_esc::EscModel::C620 }; + let trace = simulate( + VelocityGains::new(500.0, 0.0, 0.0, 0.0), + &PlantParams::gm3508(), + &scenario, + limits, + ) + .unwrap(); + let score = Objective::new(Weights::default(), scenario, limits).score(&trace); + assert!( + matches!(score.verdict, Verdict::Saturated | Verdict::NoSettle), + "{:?}", + score.verdict + ); + assert!(score.cost >= PENALTY); + } + + #[test] + fn verdict_labels_are_stable() { + assert_eq!(Verdict::Ok.label(), "ok"); + assert_eq!(Verdict::NoSettle.label(), "no-settle"); + assert!(Verdict::Ok.is_ok()); + assert!(!Verdict::Overspeed.is_ok()); + } + + #[test] + fn weights_must_be_finite_and_non_negative() { + assert!(Weights::default().is_valid()); + let bad = Weights { settle: -1.0, ..Weights::default() }; + assert!(!bad.is_valid()); + let nan = Weights { iae: f64::NAN, ..Weights::default() }; + assert!(!nan.is_valid()); + } + + #[test] + fn weighted_cost_decreases_when_the_response_improves() { + let scenario = Scenario::step(1500.0, 1.0); + let objective = objective(scenario); + let good = simulate(GOOD, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap(); + let sluggish = simulate( + VelocityGains::new(0.6, 1.0, 0.0, 0.0), + &PlantParams::gm3508(), + &scenario, + LoopLimits::gm3508(), + ) + .unwrap(); + let good_metrics = StepMetrics::from_trace(&good, &scenario).unwrap(); + let sluggish_metrics = StepMetrics::from_trace(&sluggish, &scenario).unwrap(); + assert!( + objective.weighted_cost(&good_metrics) < objective.weighted_cost(&sluggish_metrics), + "好的响应必须成本更低" + ); + } + + #[test] + fn sim_errors_map_to_verdicts() { + assert_eq!(Objective::from_error(SimError::NonFiniteGains), Verdict::Rejected); + assert_eq!(Objective::from_error(SimError::DegenerateScenario), Verdict::Rejected); + assert_eq!(Objective::from_error(SimError::Diverged { step: 3 }), Verdict::Diverged); + } +} diff --git a/crates/chassis-tune/src/gains.rs b/crates/chassis-tune/src/gains.rs new file mode 100644 index 0000000..7ef3099 --- /dev/null +++ b/crates/chassis-tune/src/gains.rs @@ -0,0 +1,572 @@ +//! 增益参数空间：轴定义、采样与**坐标空间**映射。 +//! +//! # 为什么要有"坐标空间" +//! +//! 增益的合法取值跨越数量级（`ki` 可能在 0.1 与 100 之间），线性空间的 +//! 单纯形步长对数量级不敏感；同时 `kp/ki/kd` 必须为正。把每个轴映射到 +//! **坐标空间**（对数轴取 `ln`，线性轴恒等）后： +//! +//! - 无约束优化器不会走出正增益区域； +//! - 步长在不同数量级上等价（±0.1 的坐标步长 = ×1.1 的比例步长）； +//! - `LogOrZero` 轴在**粗扫**阶段可以精确取 0（`ff = 0` 是台架常态）， +//! 在**精修**阶段退化为对数轴（零是不可导的拐点，本地搜索收益为负）。 + +use motor_core::VelocityGains; +use rand::Rng; +use rand::seq::SliceRandom as _; +use thiserror::Error; + +/// 轴数量：`kp`、`ki`、`kd`、`feedforward`。 +pub const AXIS_COUNT: usize = 4; + +/// 轴的取值范围映射方式。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum Scale { + /// 线性均匀。 + Linear, + /// 对数均匀（要求 `min > 0`）。 + Log, + /// 以概率 `p_zero` 取精确 0，否则对数均匀（用于 `ki` / `kd` / `ff`）。 + LogOrZero { + /// 取 0 的概率。 + p_zero: f64, + }, +} + +impl Scale { + /// 是否是对数族（`Log` / `LogOrZero`）。 + #[must_use] + pub fn is_log(self) -> bool { + matches!(self, Self::Log | Self::LogOrZero { .. }) + } + + /// 从文本解析（CLI 用）。 + /// + /// # Errors + /// + /// 未知名字时返回 [`GainsError::UnknownScale`]。 + pub fn parse(text: &str) -> Result { + match text { + "linear" => Ok(Self::Linear), + "log" => Ok(Self::Log), + "log0" | "log-or-zero" => Ok(Self::LogOrZero { p_zero: 0.25 }), + other => Err(GainsError::UnknownScale(other.to_owned())), + } + } + + /// 文本名。 + #[must_use] + pub fn name(self) -> &'static str { + match self { + Self::Linear => "linear", + Self::Log => "log", + Self::LogOrZero { .. } => "log0", + } + } +} + +/// 参数空间定义错误。 +#[derive(Debug, Clone, PartialEq, Eq, Error)] +pub enum GainsError { + /// 轴区间非法。 + #[error("invalid range for {axis}: {reason}")] + InvalidRange { + /// 轴名。 + axis: String, + /// 原因。 + reason: String, + }, + /// 未知的映射方式名。 + #[error("unknown scale '{0}' (expected linear|log|log0)")] + UnknownScale(String), + /// 区间文本无法解析（期望 `MIN..MAX`）。 + #[error("cannot parse range '{0}' (expected MIN..MAX)")] + BadRange(String), + /// 只给了对数轴 0 下界。 + #[error("axis {0} uses a log scale but min must be > 0")] + NonPositiveLogMin(String), +} + +/// 单条增益轴。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct GainAxis { + /// 轴名（`kp` / `ki` / `kd` / `feedforward`）。 + pub name: &'static str, + /// 下界。 + pub min: f64, + /// 上界。 + pub max: f64, + /// 取值映射。 + pub scale: Scale, +} + +impl GainAxis { + /// 构造。 + #[must_use] + pub const fn new(name: &'static str, min: f64, max: f64, scale: Scale) -> Self { + Self { name, min, max, scale } + } + + /// 校验。 + /// + /// # Errors + /// + /// 非有限 / `min > max` / 对数轴下界 ≤ 0 时返回错误。 + pub fn validate(&self) -> Result<(), GainsError> { + let axis = self.name.to_owned(); + if !(self.min.is_finite() && self.max.is_finite()) { + return Err(GainsError::InvalidRange { axis, reason: "bounds must be finite".into() }); + } + if self.min > self.max { + return Err(GainsError::InvalidRange { + axis, + reason: format!("min {} > max {}", self.min, self.max), + }); + } + if self.scale.is_log() && self.min <= 0.0 { + return Err(GainsError::NonPositiveLogMin(axis)); + } + Ok(()) + } + + /// 在区间内采样（含 `LogOrZero` 的零分支）。 + pub fn sample(&self, rng: &mut impl Rng) -> f64 { + match self.scale { + Scale::Linear => rng.random_range(self.min..=self.max), + Scale::Log => self.log_sample(rng), + Scale::LogOrZero { p_zero } => { + if rng.random_bool(p_zero.clamp(0.0, 1.0)) { + 0.0 + } else { + self.log_sample(rng) + } + } + } + } + + fn log_sample(&self, rng: &mut impl Rng) -> f64 { + let lo = self.min.max(f64::MIN_POSITIVE).ln(); + let hi = self.max.ln(); + if hi <= lo { + return self.min; + } + rng.random_range(lo..=hi).exp() + } + + /// 夹到区间内。 + #[must_use] + pub fn clamp(&self, value: f64) -> f64 { + let clamped = value.clamp(self.min, self.max); + if clamped.is_finite() { clamped } else { self.min } + } + + /// 参数 → 坐标。 + #[must_use] + pub fn encode(&self, value: f64) -> f64 { + if self.scale.is_log() { + self.clamp(value).max(f64::MIN_POSITIVE).ln() + } else { + self.clamp(value) + } + } + + /// 坐标 → 参数（先夹到坐标边界，再取指数）。 + #[must_use] + pub fn decode(&self, coord: f64) -> f64 { + let (lo, hi) = self.coord_bounds(); + let coord = if coord.is_finite() { coord.clamp(lo, hi) } else { lo }; + if self.scale.is_log() { coord.exp() } else { coord } + } + + /// 坐标空间的边界。 + #[must_use] + pub fn coord_bounds(&self) -> (f64, f64) { + if self.scale.is_log() { + (self.min.max(f64::MIN_POSITIVE).ln(), self.max.ln()) + } else { + (self.min, self.max) + } + } + + /// 人类可读描述。 + #[must_use] + pub fn describe(&self) -> String { + format!("{} ∈ [{}..{}] ({})", self.name, self.min, self.max, self.scale.name()) + } +} + +/// 四轴参数空间。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct GainSpace { + /// 比例增益（raw / rpm）。 + pub kp: GainAxis, + /// 积分增益（raw / (rpm·s)）。 + pub ki: GainAxis, + /// 微分增益。 + pub kd: GainAxis, + /// 速度前馈（raw / rpm）。 + pub feedforward: GainAxis, +} + +impl GainSpace { + /// 组装。 + #[must_use] + pub const fn new(kp: GainAxis, ki: GainAxis, kd: GainAxis, feedforward: GainAxis) -> Self { + Self { kp, ki, kd, feedforward } + } + + /// 默认空间：覆盖空载台架（`kp≈2, ki≈10, ff=0`）与带载（`ff≈1.77`）两侧。 + #[must_use] + pub const fn default_gm3508() -> Self { + Self { + kp: GainAxis::new("kp", 0.5, 30.0, Scale::Log), + ki: GainAxis::new("ki", 0.1, 100.0, Scale::LogOrZero { p_zero: 0.2 }), + kd: GainAxis::new("kd", 0.01, 3.0, Scale::LogOrZero { p_zero: 0.5 }), + feedforward: GainAxis::new("feedforward", 0.01, 2.5, Scale::LogOrZero { p_zero: 0.2 }), + } + } + + /// 校验全部轴。 + /// + /// # Errors + /// + /// 任一轴非法时返回错误。 + pub fn validate(&self) -> Result<(), GainsError> { + for axis in self.axes() { + axis.validate()?; + } + Ok(()) + } + + /// 四轴（顺序固定 `kp, ki, kd, feedforward`）。 + #[must_use] + pub const fn axes(&self) -> [GainAxis; AXIS_COUNT] { + [self.kp, self.ki, self.kd, self.feedforward] + } + + /// 随机采样一个候选增益。 + pub fn sample(&self, rng: &mut impl Rng) -> VelocityGains { + let [kp, ki, kd, ff] = self.axes().map(|axis| axis.sample(rng)); + VelocityGains::new(kp as f32, ki as f32, kd as f32, ff as f32) + } + + /// 分层（Latin Hypercube）采样：每个轴把 `[0,1)` 等分成 `count` 层， + /// 每层恰好落一个样本，再逐轴独立打乱。 + /// + /// 相比独立随机采样，分层采样在四维空间里不会留下大片空洞，粗扫阶段 + /// 用更少的评估次数覆盖更大的空间。 + pub fn stratified(&self, count: usize, rng: &mut impl Rng) -> Vec { + let count = count.max(1); + let mut columns: Vec> = Vec::with_capacity(AXIS_COUNT); + for axis in self.axes() { + let (lo, hi) = axis.coord_bounds(); + let mut column = Vec::with_capacity(count); + for layer in 0..count { + let unit = (layer as f64 + rng.random_range(0.0..1.0)) / count as f64; + column.push(lo + unit * (hi - lo)); + } + column.shuffle(rng); + columns.push(column); + } + (0..count) + .map(|index| { + self.from_coords([ + columns[0][index], + columns[1][index], + columns[2][index], + columns[3][index], + ]) + }) + .collect() + } + + /// 把任意（可能越界 / 非有限）增益夹进空间。 + #[must_use] + pub fn clamp_gains(&self, gains: VelocityGains) -> VelocityGains { + self.from_values([gains.kp, gains.ki, gains.kd, gains.feedforward].map(f64::from)) + } + + /// 由四个参数值构造（先夹界，再保证有限）。 + #[must_use] + pub fn from_values(&self, values: [f64; AXIS_COUNT]) -> VelocityGains { + let axes = self.axes(); + let v: [f64; AXIS_COUNT] = std::array::from_fn(|i| axes[i].clamp(values[i])); + VelocityGains::new(v[0] as f32, v[1] as f32, v[2] as f32, v[3] as f32) + } + + /// 参数 → 坐标。 + #[must_use] + pub fn to_coords(&self, gains: &VelocityGains) -> [f64; AXIS_COUNT] { + let axes = self.axes(); + let values = [gains.kp, gains.ki, gains.kd, gains.feedforward].map(f64::from); + std::array::from_fn(|i| axes[i].encode(values[i])) + } + + /// 坐标 → 参数。 + #[must_use] + pub fn from_coords(&self, coords: [f64; AXIS_COUNT]) -> VelocityGains { + let axes = self.axes(); + let v: [f64; AXIS_COUNT] = std::array::from_fn(|i| axes[i].decode(coords[i])); + VelocityGains::new(v[0] as f32, v[1] as f32, v[2] as f32, v[3] as f32) + } + + /// 坐标空间的边界（下界数组, 上界数组）。 + #[must_use] + pub fn coord_bounds(&self) -> ([f64; AXIS_COUNT], [f64; AXIS_COUNT]) { + let axes = self.axes(); + let pairs: [(f64, f64); AXIS_COUNT] = std::array::from_fn(|i| axes[i].coord_bounds()); + let lower = std::array::from_fn(|i| pairs[i].0); + let upper = std::array::from_fn(|i| pairs[i].1); + (lower, upper) + } + + /// 最优解是否贴在某个轴的边界上（提示是否该放宽搜索范围）。 + /// + /// 贴边界通常意味着"约束而不是数据在决定结果"——继续在这个盒子里搜索 + /// 不会更好，报告里要明确说出来，而不是把一个边界值当成最优解交付。 + #[must_use] + pub fn boundary_hits(&self, gains: &VelocityGains) -> Vec { + let values = [gains.kp, gains.ki, gains.kd, gains.feedforward]; + let mut hits = Vec::new(); + for (axis, value) in self.axes().iter().zip(values) { + let value = f64::from(value); + let tolerance = ((axis.max - axis.min).abs() * 0.01).max(1.0e-9); + if (value - axis.max).abs() <= tolerance { + hits.push(format!( + "{} 落在上界 {}（若要继续提升，放宽 --{}）", + axis.name, axis.max, axis.name + )); + } else if (value - axis.min).abs() <= tolerance { + hits.push(format!( + "{} 落在下界 {}（若要继续压缩，放宽 --{}）", + axis.name, axis.min, axis.name + )); + } + } + hits + } + + /// 人类可读描述（报告用）。 + #[must_use] + pub fn describe(&self) -> String { + self.axes().iter().map(GainAxis::describe).collect::>().join("; ") + } +} + +/// 解析 `MIN..MAX`（`..=` 视为等价）。 +/// +/// # Errors +/// +/// 无法解析或区间非法时返回错误。 +pub fn parse_axis(name: &'static str, spec: &str, scale: Scale) -> Result { + let spec = spec.trim(); + let bad = || GainsError::BadRange(spec.to_owned()); + // `MIN..MAX` 与 `MIN..=MAX` 视为等价；单个数值表示固定该轴。 + let (min_text, max_text) = match spec.split_once("..=").or_else(|| spec.split_once("..")) { + Some((lo, hi)) => (lo.trim(), hi.trim()), + None => (spec, spec), + }; + if min_text.is_empty() || max_text.is_empty() { + return Err(bad()); + } + let min: f64 = min_text.parse().map_err(|_| bad())?; + let max: f64 = max_text.parse().map_err(|_| bad())?; + let axis = GainAxis::new(name, min, max, scale); + axis.validate()?; + Ok(axis) +} + +#[cfg(test)] +mod tests { + use super::*; + use rand::SeedableRng as _; + use rand::rngs::StdRng; + + fn rng(seed: u64) -> StdRng { + StdRng::seed_from_u64(seed) + } + + #[test] + fn clamp_keeps_values_inside_the_space() { + let space = GainSpace::default_gm3508(); + let wild = VelocityGains::new(1.0e9, -5.0, 100.0, f32::NAN); + let clamped = space.clamp_gains(wild); + assert!(clamped.kp <= 30.0 && clamped.kp >= 0.5); + assert!(clamped.ki >= 0.1); + assert!(clamped.kd <= 3.0); + assert!(clamped.feedforward.is_finite()); + assert!(clamped.is_finite()); + } + + #[test] + fn sampling_stays_inside_bounds_and_is_deterministic() { + let space = GainSpace::default_gm3508(); + let mut a = rng(7); + let mut b = rng(7); + for _ in 0..500 { + let g1 = space.sample(&mut a); + let g2 = space.sample(&mut b); + assert_eq!(g1, g2, "同 seed 必须一致"); + assert!((0.5..=30.0).contains(&g1.kp)); + assert!((0.0..=100.0).contains(&g1.ki)); + assert!((0.0..=3.0).contains(&g1.kd)); + assert!((0.0..=2.5).contains(&g1.feedforward)); + assert!(g1.is_finite()); + } + } + + #[test] + fn log_axis_samples_across_orders_of_magnitude() { + let axis = GainAxis::new("kp", 0.1, 100.0, Scale::Log); + let mut rng = rng(3); + let mut below_one = 0; + let mut above_ten = 0; + for _ in 0..2000 { + let value = axis.sample(&mut rng); + assert!((0.1..=100.0).contains(&value)); + if value < 1.0 { + below_one += 1; + } + if value > 10.0 { + above_ten += 1; + } + } + // 对数均匀：三个数量级应大致各占 1/3。 + assert!(below_one > 400 && below_one < 900, "below_one = {below_one}"); + assert!(above_ten > 400 && above_ten < 900, "above_ten = {above_ten}"); + } + + #[test] + fn log_or_zero_hits_exact_zero() { + let axis = GainAxis::new("feedforward", 0.01, 2.5, Scale::LogOrZero { p_zero: 0.5 }); + let mut rng = rng(11); + let mut zeros = 0; + for _ in 0..2000 { + if axis.sample(&mut rng) == 0.0 { + zeros += 1; + } + } + assert!((800..1200).contains(&zeros), "zeros = {zeros}"); + } + + #[test] + fn coord_round_trip_is_stable() { + let space = GainSpace::default_gm3508(); + let mut rng = rng(5); + let mut saw_zero = false; + for _ in 0..200 { + let gains = space.sample(&mut rng); + let back = space.from_coords(space.to_coords(&gains)); + // 坐标空间对**非零**取值是双射（只允许 f32 舍入误差）； + // 0 在对数轴里没有原像，往返会落到该轴下界——这是 LogOrZero 的定义。 + for (axis, (sampled, restored)) in space.axes().iter().zip([ + (gains.kp, back.kp), + (gains.ki, back.ki), + (gains.kd, back.kd), + (gains.feedforward, back.feedforward), + ]) { + if sampled == 0.0 { + saw_zero = true; + assert!( + (restored - axis.min as f32).abs() <= 1e-6 * axis.min as f32, + "{} 的零值往返应落到下界 {}，得到 {restored}", + axis.name, + axis.min + ); + } else { + let tolerance = 1e-3 * sampled.max(1.0); + assert!( + (restored - sampled).abs() <= tolerance, + "{} 往返误差过大：{sampled} → {restored}", + axis.name + ); + } + } + } + assert!(saw_zero, "LogOrZero 轴必须真的采到过 0"); + } + + #[test] + fn decode_clamps_out_of_range_coordinates() { + let space = GainSpace::default_gm3508(); + let below = space.from_coords([-1.0e9, -1.0e9, -1.0e9, -1.0e9]); + assert!(below.kp >= 0.5); + let above = space.from_coords([1.0e9; AXIS_COUNT]); + assert!(above.kp <= 30.0); + assert!(above.feedforward.is_finite()); + } + + #[test] + fn stratified_covers_every_layer_of_every_axis() { + let space = GainSpace::default_gm3508(); + let count = 32; + let samples = space.stratified(count, &mut rng(9)); + assert_eq!(samples.len(), count); + // 每个轴在坐标空间被等分：把样本坐标排序后，相邻差应大致等于层宽。 + let (lower, upper) = space.coord_bounds(); + let span = upper[0] - lower[0]; + let mut coords: Vec = samples.iter().map(|g| space.to_coords(g)[0]).collect(); + coords.sort_by(f64::total_cmp); + let layer = span / count as f64; + for window in coords.windows(2) { + let gap = window[1] - window[0]; + assert!(gap < layer * 3.0, "gap {gap} too large for layer {layer}"); + } + } + + #[test] + fn stratified_handles_large_counts_without_truncation() { + let space = GainSpace::default_gm3508(); + let samples = space.stratified(96, &mut rng(4)); + assert_eq!(samples.len(), 96); + } + + #[test] + fn parse_axis_accepts_ranges_and_single_values() { + let axis = parse_axis("kp", "0.5..20", Scale::Log).unwrap(); + assert!((axis.min - 0.5).abs() < 1e-12); + assert!((axis.max - 20.0).abs() < 1e-12); + + let open = parse_axis("ki", "0..=50", Scale::Linear).unwrap(); + assert!((open.max - 50.0).abs() < 1e-12); + + let fixed = parse_axis("kd", "0.25", Scale::Linear).unwrap(); + assert!((fixed.min - 0.25).abs() < 1e-12 && (fixed.max - 0.25).abs() < 1e-12); + } + + #[test] + fn parse_axis_rejects_garbage() { + assert!(matches!(parse_axis("kp", "abc", Scale::Log), Err(GainsError::BadRange(_)))); + assert!(matches!(parse_axis("kp", "", Scale::Log), Err(GainsError::BadRange(_)))); + assert!(matches!( + parse_axis("kp", "-1..5", Scale::Log), + Err(GainsError::NonPositiveLogMin(_)) + )); + } + + #[test] + fn validate_rejects_inverted_and_non_finite_bounds() { + let bad = GainAxis::new("kp", 10.0, 1.0, Scale::Linear); + assert!(bad.validate().is_err()); + let nan = GainAxis::new("kp", f64::NAN, 1.0, Scale::Linear); + assert!(nan.validate().is_err()); + } + + #[test] + fn validate_accepts_the_default_space() { + assert!(GainSpace::default_gm3508().validate().is_ok()); + } + + #[test] + fn boundary_hits_flag_pinned_axes_only() { + let space = GainSpace::default_gm3508(); + let pinned = VelocityGains::new(30.0, 0.1, 1.0, 1.0); + let hits = space.boundary_hits(&pinned); + assert_eq!(hits.len(), 2, "{hits:?}"); + assert!(hits.iter().any(|hit| hit.contains("kp")), "{hits:?}"); + assert!(hits.iter().any(|hit| hit.contains("ki")), "{hits:?}"); + + let inside = VelocityGains::new(5.0, 10.0, 0.5, 1.0); + assert!(space.boundary_hits(&inside).is_empty()); + } +} diff --git a/crates/chassis-tune/src/lib.rs b/crates/chassis-tune/src/lib.rs new file mode 100644 index 0000000..31511cb --- /dev/null +++ b/crates/chassis-tune/src/lib.rs @@ -0,0 +1,60 @@ +//! 底盘速度环 **PID 自动整定**：参数空间搜索 + 指标评估 + 报告。 +//! +//! # 一条命令背后发生了什么 +//! +//! 1. [`gains`] 定义搜索空间（四轴、对数刻度、可精确取 0 的前馈 / 微分）； +//! 2. [`search`] 先做种子化分层粗扫（`rayon` 并行），再以 top-k 为起点做 +//! 多起点 `argmin` 局部精修； +//! 3. 每次评估由 [`backend`] 执行：离线 harness 在**被控对象集成**上重放场景， +//! 或（真机后端）在台架上跑一次受安全包络约束的试验； +//! 4. [`metrics`] / [`cost`] 把轨迹折成加权成本，硬约束（过速 / 长时饱和 / +//! 不收敛）直接给罚值，不参与权重折衷； +//! 5. [`report`] 落盘 CSV / TOML / JSON / Markdown（可选 SVG）。 +//! +//! # 超参数从哪来 +//! +//! "搜什么、怎么打分、什么算达标"由 [`preset`] 定义，优先级为 +//! **内置默认 < 预设文件（`config/tune.toml`）< 显式命令行参数**。 +//! 一次评审过的预设可以长期复用，不再靠命令行回忆当时那组数。 +//! +//! # 可复现 +//! +//! 随机性只来自 `--seed`：同一 seed + 同一参数空间 + 同一后端 → 逐位一致的结果。 +//! 这是 CI 里做冒烟回归的前提。预设只是把参数空间 / 权重 / 场景固化下来， +//! 因此"同一个预设 + 同一个 seed"在任何机器上都应当复现同一结果。 +//! +//! # 安全 +//! +//! 真机后端（`--backend bench`）会**真的驱动电机**。安全由三层保证： +//! 搜索前的强制确认标志、每次试验的 `chassis-bench` 安全包络、以及任何退出 +//! 路径都先归零电流。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod backend; +pub mod bench; +pub mod cost; +pub mod gains; +pub mod metrics; +#[cfg(feature = "plot")] +pub mod plot; +pub mod preset; +pub mod report; +pub mod search; + +pub use backend::{Evaluated, Evaluator, SimEvaluator}; +pub use bench::{BenchConfig, BenchError, BenchEvaluator}; +pub use cost::{Objective, PENALTY, Score, Verdict, Weights}; +pub use gains::{AXIS_COUNT, GainAxis, GainSpace, GainsError, Scale, parse_axis}; +pub use metrics::StepMetrics; +pub use preset::{ + Acceptance, AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, + DEFAULT_PRESET_PATH, PRESET_VERSION, PlantSpec, Preset, PresetError, PresetFile, ProfileSpec, + RangeSpec, ScaleSpec, ScenarioSpec, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, +}; +pub use report::{ReportContext, ReportError, write_reports}; +pub use search::{ + Phase, Ranked, SearchConfig, SearchError, SolverKind, Trial, evaluate_once, sweep, +}; + +/// 被整定的物理量类型（来自 `motor-core`，与驱动层、台架共用同一个定义）。 +pub use motor_core::VelocityGains; diff --git a/crates/chassis-tune/src/main.rs b/crates/chassis-tune/src/main.rs new file mode 100644 index 0000000..2fd6718 --- /dev/null +++ b/crates/chassis-tune/src/main.rs @@ -0,0 +1,1781 @@ +//! `chassis-tune`：底盘速度环 PID 自动整定命令行工具。 +//! +//! ```sh +//! # 0) 看预设文件里有哪些工况预设（只读，不搜索） +//! cargo run -p chassis-tune -- presets +//! +//! # 1) 仿真搜索（默认；不碰硬件，可反复跑） +//! cargo run --release -p chassis-tune -- sweep --samples 512 --out-dir target/tune +//! +//! # 1b) 用预设跑（超参数进版本管理，见 config/tune.toml） +//! cargo run --release -p chassis-tune -- sweep \ +//! --preset config/tune.toml --preset-name bench_loaded +//! +//! # 2) 复算一组给定增益（CI / 回归用，按阈值给退出码） +//! cargo run -p chassis-tune -- validate --gains 2,10,0,0 +//! +//! # 3) 台架复验（**轮子会转**：必须架空或整车固定，急停可及） +//! cargo run --release -p chassis-tune -- sweep --backend bench \ +//! --device /dev/ttyACM0 --esc-id 1 --samples 24 --top-k 3 \ +//! --i-know-the-wheel-spins +//! ``` +//! +//! # 超参数的优先级 +//! +//! `内置默认 < 预设文件（`--preset` + `--preset-name`）< 显式命令行参数`。 +//! 机械 / 安全参数（`--current-limit`、`--direction`、`--gear-ratio`、电调 ID） +//! 不参与这条链：它们只来自 `--config`（`chassis.toml`）或显式 CLI， +//! 预设文件**不能**覆盖它们。 +//! +//! 退出码：最优解满足验收阈值（`--max-overshoot` / `--max-ss-error`）为 0， +//! 否则 1；参数 / IO 错误为 2。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +use std::path::{Path, PathBuf}; +use std::process::ExitCode; +use std::time::Duration; + +use anyhow::{Context as _, Result, bail}; +use chassis_bench::{CanSpeed, SafetyEnvelope}; +use chassis_config::Config; +use chassis_tune::bench::{BenchConfig, BenchEvaluator, open_link}; +use chassis_tune::{ + AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH, + Evaluator, Objective, PlantSpec, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, + ReportContext, ScaleSpec, ScenarioSpec, Score, SearchSpec, SimEvaluator, SimSpec, SolverSpec, + SpaceSpec, Tuning, VelocityGains, parse_axis, sweep, write_reports, +}; +use clap::{Args as ClapArgs, Parser, Subcommand, ValueEnum}; +use dji_esc::{EscConfig, EscModel}; +use dji_esc_sim::{LoopLimits, PlantEnsemble, SetpointProfile}; +use indicatif::ProgressBar; +use serde::Serialize; + +/// 子命令。 +#[derive(Debug, Subcommand)] +enum Command { + /// 分层粗扫 + 多起点精修（默认动作） + Sweep(SweepArgs), + /// 只复算一组给定增益的指标（不搜索） + Validate(ValidateArgs), + /// 列出预设文件里的超参数预设（只读） + Presets(PresetsArgs), +} + +/// 公共参数（仿真与真机共用）。 +/// +/// 超参数（参数空间、场景、权重、阈值、搜索预算、被控对象、安全包络）都可以缺省： +/// 缺省时按 **内置默认 → 预设文件 → 显式 CLI** 解算，见模块文档与 +/// [`chassis_tune::preset`]。这里的 `Option` 正是"用户是否显式给了"的载体。 +#[derive(Debug, Clone, ClapArgs)] +struct CommonArgs { + /// 评估后端：`sim`（离线）或 `bench`（真机台架） + #[arg(long, value_enum)] + backend: Option, + + // ---------------- 超参数预设 ---------------- + /// 超参数预设文件（TOML）。缺省时若给了 `--preset-name` 则用 `config/tune.toml` + #[arg(long, value_name = "FILE")] + preset: Option, + /// 预设名（缺省取文件里的 `default_preset`） + #[arg(long, value_name = "NAME")] + preset_name: Option, + + // ---------------- 参数空间（覆盖预设） ---------------- + /// kp 范围，如 `0.5..30`（内置默认 `0.5..30`） + #[arg(long, value_name = "MIN..MAX")] + kp: Option, + /// ki 范围，如 `0.1..100`（内置默认 `0.1..100`） + #[arg(long, value_name = "MIN..MAX")] + ki: Option, + /// kd 范围（内置默认 `0.01..3`） + #[arg(long, value_name = "MIN..MAX")] + kd: Option, + /// 前馈范围（raw/rpm；内置默认 `0.01..2.5`） + #[arg(long, value_name = "MIN..MAX")] + feedforward: Option, + /// kp 刻度（内置默认 `log`） + #[arg(long, value_enum)] + kp_scale: Option, + /// ki 刻度（`log0` 允许精确取 0；内置默认 `log0`） + #[arg(long, value_enum)] + ki_scale: Option, + /// kd 刻度（内置默认 `log0`） + #[arg(long, value_enum)] + kd_scale: Option, + /// 前馈刻度（内置默认 `log0`） + #[arg(long, value_enum)] + feedforward_scale: Option, + + // ---------------- 场景（覆盖预设） ---------------- + /// 目标转子转速（rpm；内置默认 1500） + #[arg(long)] + target_rpm: Option, + /// 场景类型（内置默认 `ramp-and-hold`） + #[arg(long, value_enum)] + scenario: Option, + /// 斜坡时间（毫秒；内置默认 200） + #[arg(long)] + ramp_ms: Option, + /// 保持时间（毫秒；内置默认 1800） + #[arg(long)] + hold_ms: Option, + /// 阶梯级数（`--scenario staircase`；内置默认 4） + #[arg(long)] + stair_levels: Option, + /// 采样 / 控制节拍（微秒；内置默认 1000） + #[arg(long)] + dt_us: Option, + /// 电流限幅（raw；同时是 PID 输出钳位点）。缺省取 `--config` 里该电机的值，否则 12000 + #[arg(long)] + current_limit: Option, + /// 电调型号。缺省取 `--config` 里该电机的值，否则 C620 + #[arg(long, value_enum)] + model: Option, + /// 现有 `chassis.toml`：用作机械 / 安全参数的来源（电流限幅、电调 ID、方向、减速比） + #[arg(long)] + config: Option, + /// 把整定结果写回一份配置（需同时给 `--config`；只改写 kp/ki/kd/feedforward） + #[arg(long)] + write_config: Option, + + // ---------------- 成本权重（覆盖预设） ---------------- + /// 调节时间权重（内置默认 1.0） + #[arg(long)] + w_settle: Option, + /// 超调权重（内置默认 1.0） + #[arg(long)] + w_overshoot: Option, + /// IAE 权重（内置默认 0.5） + #[arg(long)] + w_iae: Option, + /// 电流（RMS）权重（内置默认 0.1） + #[arg(long)] + w_effort: Option, + + // ---------------- 验收阈值（覆盖预设） ---------------- + /// 允许的最大超调（%；内置默认 20） + #[arg(long)] + max_overshoot: Option, + /// 允许的最大稳态误差（%；内置默认 5） + #[arg(long)] + max_ss_error: Option, + + // ---------------- 搜索（覆盖预设） ---------------- + /// 随机种子（同 seed 结果逐位一致；内置默认 20260101） + #[arg(long)] + seed: Option, + /// 粗扫样本数（内置默认 512） + #[arg(long)] + samples: Option, + /// 进入精修的种子数（内置默认 8） + #[arg(long)] + top_k: Option, + /// 精修求解器（内置默认 `nelder-mead`） + #[arg(long, value_enum)] + solver: Option, + /// 每个起点的最大迭代数（内置默认 200） + #[arg(long)] + max_iters: Option, + /// PSO 粒子数（`--solver pso`；内置默认 24） + #[arg(long)] + particles: Option, + /// 评估预算上限（内置默认 4000） + #[arg(long)] + max_evals: Option, + /// 线程数（0 = rayon 默认；真机后端强制 1；内置默认 0） + #[arg(long)] + threads: Option, + + // ---------------- 真机安全包络（覆盖预设） ---------------- + /// 过速保护上限（rpm；内置默认 3000） + #[arg(long)] + max_rpm: Option, + /// 温度保护上限（℃；内置默认 70） + #[arg(long)] + max_temp_c: Option, + /// 单次试验超时（毫秒；内置默认 6000） + #[arg(long)] + trial_timeout_ms: Option, + /// 试验之间的静默期（毫秒；内置默认 500） + #[arg(long)] + between_trials_ms: Option, + /// 启动时等待反馈的超时（毫秒；内置默认 5000） + #[arg(long)] + startup_timeout_ms: Option, + /// 安全事件后继续搜索（默认立即收工） + #[arg(long)] + continue_on_abort: bool, + /// 最优增益的复验次数（写入 hil-validation.csv；内置默认 3） + #[arg(long)] + validate_runs: Option, + + // ---------------- 输出 ---------------- + /// 产物目录 + #[arg(long, default_value = "target/chassis-tune")] + out_dir: PathBuf, + /// 目标轮名（写进 best.toml 注释与报告） + #[arg(long, default_value = "front_left")] + motor: String, + /// 把 JSON 摘要打印到 stdout + #[arg(long)] + json: bool, + + #[command(flatten)] + sim: SimArgs, + #[command(flatten)] + bench: BenchArgs, +} + +/// 仿真后端参数（覆盖预设）。 +#[derive(Debug, Clone, ClapArgs)] +struct SimArgs { + /// 被控对象模型（内置默认 `gm3508-bench`） + #[arg(long, value_enum)] + plant: Option, + /// 集成成员数（域随机化；1 = 只用名义模型；内置默认 8） + #[arg(long)] + ensemble: Option, + /// 鲁棒权重：0 = 只看均值，1 = 只看最坏成员（内置默认 0.5） + #[arg(long)] + robust_weight: Option, + /// 抖动幅度，如 `gain=0.2,tau=0.3,delay=3,friction=0.5`（内置默认同左） + #[arg(long)] + jitter: Option, +} + +/// 真机接线参数（**不参与搜索，也不受预设影响**）。 +#[derive(Debug, Clone, ClapArgs)] +struct BenchArgs { + /// slcan 串口设备 + #[arg(long, default_value = "/dev/ttyACM0")] + device: String, + /// SocketCAN 接口名（给了它就走 SocketCAN，忽略 --device） + #[arg(long)] + interface: Option, + /// 串口波特率 + #[arg(long, default_value_t = 2_000_000)] + serial_baud: u32, + /// CAN 波特率档位 + #[arg(long, value_enum, default_value_t = CanSpeedArg::M1)] + can_speed: CanSpeedArg, + /// 目标电调 ID（缺省取 `--config`，否则 1） + #[arg(long)] + esc_id: Option, + /// 安装方向 +1 / -1（缺省取 `--config`，否则 1） + #[arg(long, allow_hyphen_values = true)] + direction: Option, + /// 减速比（转子 : 输出轴；缺省取 `--config`，否则 19.203） + #[arg(long)] + gear_ratio: Option, + /// **明确确认**：轮子会真的转起来（架空或整车固定、急停可及） + #[arg(long)] + i_know_the_wheel_spins: bool, +} + +#[derive(Debug, ClapArgs)] +struct SweepArgs { + #[command(flatten)] + common: CommonArgs, +} + +#[derive(Debug, ClapArgs)] +struct ValidateArgs { + #[command(flatten)] + common: CommonArgs, + /// 四个增益，如 `2,10,0,0`（kp,ki,kd,feedforward） + #[arg(long)] + gains: String, +} + +#[derive(Debug, ClapArgs)] +struct PresetsArgs { + /// 预设文件（缺省 `config/tune.toml`） + #[arg(long, default_value = DEFAULT_PRESET_PATH)] + preset: PathBuf, +} + +#[derive(Debug, Parser)] +#[command( + name = "chassis-tune", + version, + about = "底盘速度环 PID 自动整定（参数空间搜索：仿真 / 真机台架）" +)] +struct Cli { + #[command(subcommand)] + command: Command, +} + +// ---------------------------------------------------------------- 枚举参数 + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum BackendArg { + /// 离线被控对象仿真 + Sim, + /// 真机台架（会驱动电机） + Bench, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum ScaleArg { + /// 线性 + Linear, + /// 对数（恒正） + Log, + /// 以一定概率取 0，否则对数 + Log0, +} + +impl From for ScaleSpec { + fn from(value: ScaleArg) -> Self { + match value { + ScaleArg::Linear => Self::Linear, + ScaleArg::Log => Self::Log, + ScaleArg::Log0 => Self::Log0, + } + } +} + +impl From for BackendSpec { + fn from(value: BackendArg) -> Self { + match value { + BackendArg::Sim => Self::Sim, + BackendArg::Bench => Self::Bench, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum ScenarioArg { + /// 阶跃 + Step, + /// 斜坡 + 保持 + RampAndHold, + /// 阶梯 + Staircase, + /// 升 → 保持 → 降 → 保持 + UpDown, +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum SolverArg { + /// Nelder-Mead（默认，无导数） + NelderMead, + /// 粒子群 + Pso, +} + +impl From for SolverSpec { + fn from(value: SolverArg) -> Self { + match value { + SolverArg::NelderMead => Self::NelderMead, + SolverArg::Pso => Self::Pso, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum PlantArg { + /// 理想一阶模型（无摩擦、无延迟） + Gm3508, + /// 台架量级：静 / 库仑摩擦 + 2 拍延迟 + Gm3508Bench, +} + +impl From for PlantSpec { + fn from(value: PlantArg) -> Self { + match value { + PlantArg::Gm3508 => Self::Gm3508, + PlantArg::Gm3508Bench => Self::Gm3508Bench, + } + } +} + +impl From for ProfileSpec { + fn from(value: ScenarioArg) -> Self { + match value { + ScenarioArg::Step => Self::Step, + ScenarioArg::RampAndHold => Self::RampAndHold, + ScenarioArg::Staircase => Self::Staircase, + ScenarioArg::UpDown => Self::UpDown, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum ModelArg { + /// C620（±16384 ↔ ±20 A） + C620, + /// C610（±10000 ↔ ±10 A） + C610, +} + +impl From for EscModel { + fn from(value: ModelArg) -> Self { + match value { + ModelArg::C620 => Self::C620, + ModelArg::C610 => Self::C610, + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] +enum CanSpeedArg { + /// 1 Mbps + #[value(name = "1m")] + M1, + /// 500 kbps + #[value(name = "500k")] + K500, +} + +impl From for CanSpeed { + fn from(value: CanSpeedArg) -> Self { + match value { + CanSpeedArg::M1 => Self::M1, + CanSpeedArg::K500 => Self::K500, + } + } +} + +// ---------------------------------------------------------------- 入口 + +fn main() -> ExitCode { + init_tracing(); + let cli = Cli::parse(); + match run(cli) { + Ok(code) => code, + Err(error) => { + eprintln!("✗ {error:#}"); + ExitCode::from(2) + } + } +} + +fn init_tracing() { + let filter = tracing_subscriber::EnvFilter::try_from_default_env() + .unwrap_or_else(|_| tracing_subscriber::EnvFilter::new("warn")); + let _ = tracing_subscriber::fmt().with_env_filter(filter).try_init(); +} + +fn run(cli: Cli) -> Result { + match cli.command { + Command::Sweep(args) => sweep_cmd(&args.common.resolve()?), + Command::Validate(args) => validate_cmd(&args.common.resolve()?, &args.gains), + Command::Presets(args) => presets_cmd(&args), + } +} + +/// 解算后的运行设置：**已校验**的超参数 + 本次运行的透传项。 +/// +/// 下游只面对这个结构，因此"超参数是从哪来的"（内置默认 / 预设 / CLI） +/// 不再散落在每个使用点，而是集中在 [`CommonArgs::resolve`] 一处。 +#[derive(Debug)] +struct Settings { + // ---- 透传：机械 / 安全与输出（不参与超参数优先级链） ---- + /// `chassis.toml` 路径。 + config: Option, + /// 整定结果写回路径。 + write_config: Option, + /// 产物目录。 + out_dir: PathBuf, + /// 目标轮名。 + motor: String, + /// 打印 JSON 摘要。 + json: bool, + /// 电流限幅覆盖（缺省取 `chassis.toml`，再缺省 12000）。 + current_limit: Option, + /// 电调型号覆盖。 + model: Option, + /// 真机接线参数。 + bench: BenchArgs, + /// 超参数来源描述（打印与报告用）。 + source: String, + // ---- 超参数（已解析 / 已校验） ---- + /// 解算后的超参数。 + tuning: Tuning, + /// 真机安全包络。 + envelope: BenchEnvelope, +} + +impl CommonArgs { + /// 解算超参数：**内置默认 → 预设文件 → 显式 CLI**。 + /// + /// # Errors + /// + /// 预设文件读取 / 解析失败、预设名不存在、区间 / 阈值 / 预算非法、 + /// 或预设路径缺失时返回错误。 + fn resolve(&self) -> Result { + let (file, prefix) = preset_source( + self.preset.as_ref(), + self.preset_name.as_deref(), + Path::new(DEFAULT_PRESET_PATH), + )?; + + let (name, selected) = file.select_named(self.preset_name.as_deref())?; + let mut merged = Preset::builtin().overlay(selected).overlay(&self.range_overlay()?); + // 刻度与抖动允许"只改这一项"，因此不放进 range_overlay，而是在合并后定点覆盖。 + apply_scales(&mut merged, self); + apply_jitter(&mut merged, self)?; + + let source = + if prefix.is_empty() { "内置默认".to_owned() } else { format!("{prefix}{name}") }; + let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{source}' 失败"))?; + let envelope = merged + .bench_envelope() + .with_context(|| format!("解算预设 '{source}' 的安全包络失败"))?; + + Ok(Settings { + config: self.config.clone(), + write_config: self.write_config.clone(), + out_dir: self.out_dir.clone(), + motor: self.motor.clone(), + json: self.json, + current_limit: self.current_limit, + model: self.model, + bench: self.bench.clone(), + source, + tuning, + envelope, + }) + } + + /// 只含"显式给出的区间 / 后端 / 场景 / 权重 / 阈值 / 搜索 / 被控对象"的覆盖层。 + /// + /// 刻度（`--kp-scale` 等）与 `--jitter` 不在这里：它们可以单独给出， + /// 由 [`apply_scales`] / [`apply_jitter`] 在合并后覆盖。 + /// + /// # Errors + /// + /// `--kp` 这类区间文本无法解析时返回错误。 + fn range_overlay(&self) -> Result { + let space = SpaceSpec { + kp: range(self.kp.as_deref(), "kp")?, + ki: range(self.ki.as_deref(), "ki")?, + kd: range(self.kd.as_deref(), "kd")?, + feedforward: range(self.feedforward.as_deref(), "feedforward")?, + }; + let scenario = ScenarioSpec { + profile: self.scenario.map(Into::into), + target_rpm: self.target_rpm, + ramp_ms: self.ramp_ms, + hold_ms: self.hold_ms, + stair_levels: self.stair_levels, + dt_us: self.dt_us, + }; + let cost = CostSpec { + settle: self.w_settle, + overshoot: self.w_overshoot, + iae: self.w_iae, + effort: self.w_effort, + }; + let acceptance = AcceptanceSpec { + max_overshoot_pct: self.max_overshoot, + max_ss_error_pct: self.max_ss_error, + }; + let search = SearchSpec { + seed: self.seed, + samples: self.samples, + top_k: self.top_k, + solver: self.solver.map(Into::into), + max_iters: self.max_iters, + particles: self.particles, + max_evals: self.max_evals, + threads: self.threads, + }; + let sim = SimSpec { + plant: self.sim.plant.map(Into::into), + ensemble: self.sim.ensemble, + robust_weight: self.sim.robust_weight, + ..SimSpec::default() + }; + let bench = BenchSpec { + max_rpm: self.max_rpm, + max_temp_c: self.max_temp_c, + trial_timeout_ms: self.trial_timeout_ms, + between_trials_ms: self.between_trials_ms, + startup_timeout_ms: self.startup_timeout_ms, + // `--continue-on-abort` 是布尔开关：只在给出时为 true，不去覆盖预设的 true。 + continue_on_abort: self.continue_on_abort.then_some(true), + validate_runs: self.validate_runs, + }; + Ok(Preset { + backend: self.backend.map(Into::into), + space, + scenario, + cost, + acceptance, + search, + sim, + bench, + ..Preset::default() + }) + } +} + +/// 选择预设来源，返回（预设库, 来源前缀）。 +/// +/// 顺序：显式 `--preset` > 给了 `--preset-name` 时的 `default_path` > 内置库。 +/// `default_path` 作为参数传入（而不是直接读常量），这样"只给 `--preset-name`" +/// 的回落逻辑可以在不依赖当前工作目录的情况下被测试。 +/// +/// # Errors +/// +/// 预设文件不存在 / 无法解析，或"给了名字却找不到默认文件"时返回错误。 +fn preset_source( + explicit: Option<&PathBuf>, + preset_name: Option<&str>, + default_path: &Path, +) -> Result<(PresetFile, String)> { + let path = match explicit { + Some(path) => path.clone(), + None if preset_name.is_some() => { + if !default_path.exists() { + bail!( + "给了 --preset-name 但没给 --preset，且默认路径 {} 不存在\n \ + 请显式指定：--preset {} --preset-name ", + default_path.display(), + default_path.display() + ); + } + default_path.to_path_buf() + } + None => return Ok((PresetFile::builtin(), String::new())), + }; + let file = + PresetFile::load(&path).with_context(|| format!("加载预设文件 {}", path.display()))?; + Ok((file, format!("{}#", path.display()))) +} + +/// 解析 `--kp 0.5..30` 形式的区间（刻度由预设 / 内置默认提供）。 +/// +/// # Errors +/// +/// 区间文本非法时返回错误。 +fn range(text: Option<&str>, name: &'static str) -> Result> { + text.map(|text| { + // 先用线性刻度只解析区间；对数轴"下界必须为正"的检查留给最终解算， + // 这样只给区间、刻度继承预设时也能得到准确的报错字段。 + let axis = parse_axis(name, text, chassis_tune::Scale::Linear)?; + Ok(RangeSpec::new(axis.min, axis.max)) + }) + .transpose() +} + +/// 用显式 `--kp-scale` 等覆盖合并结果的刻度（允许"只改刻度"）。 +fn apply_scales(preset: &mut Preset, cli: &CommonArgs) { + let set = |axis: &mut Option, scale: Option| { + if let (Some(axis), Some(scale)) = (axis.as_mut(), scale) { + axis.scale = Some(scale.into()); + } + }; + set(&mut preset.space.kp, cli.kp_scale); + set(&mut preset.space.ki, cli.ki_scale); + set(&mut preset.space.kd, cli.kd_scale); + set(&mut preset.space.feedforward, cli.feedforward_scale); +} + +/// 用显式 `--jitter` 覆盖域随机化幅度。 +/// +/// # Errors +/// +/// 抖动文本不是 `key=value` 或含未知键时返回错误。 +fn apply_jitter(preset: &mut Preset, cli: &CommonArgs) -> Result<()> { + let Some(spec) = cli.sim.jitter.as_deref() else { + return Ok(()); + }; + for part in spec.split(',').map(str::trim).filter(|part| !part.is_empty()) { + let (key, value) = + part.split_once('=').with_context(|| format!("抖动参数 '{part}' 应为 key=value"))?; + let value: f64 = + value.trim().parse().with_context(|| format!("抖动参数 '{part}' 的数值无法解析"))?; + match key.trim() { + "gain" => preset.sim.gain_pct = Some(value as f32), + "tau" => preset.sim.tau_pct = Some(value as f32), + "delay" => preset.sim.delay_ticks_max = Some(value.max(0.0) as u32), + "friction" => preset.sim.friction_pct = Some(value as f32), + other => bail!("未知抖动参数 '{other}'（可用 gain/tau/delay/friction）"), + } + } + Ok(()) +} + +/// 后端选择：把两种评估器收进一个枚举，报告与后处理因此不必 `Box`。 +enum Backend { + Sim(Box), + Bench(Box), +} + +impl Evaluator for Backend { + fn evaluate(&self, gains: &VelocityGains) -> Score { + match self { + Self::Sim(evaluator) => evaluator.evaluate(gains), + Self::Bench(evaluator) => evaluator.evaluate(gains), + } + } + + fn describe(&self) -> String { + match self { + Self::Sim(evaluator) => evaluator.describe(), + Self::Bench(evaluator) => evaluator.describe(), + } + } +} + +impl Backend { + fn bench(&self) -> Option<&BenchEvaluator> { + match self { + Self::Bench(evaluator) => Some(evaluator), + Self::Sim(_) => None, + } + } +} + +/// 场景的文本标签（报告 / `presets` 子命令用）。 +fn profile_label(profile: SetpointProfile) -> &'static str { + match profile { + SetpointProfile::Step => "step", + SetpointProfile::RampAndHold => "ramp-and-hold", + SetpointProfile::Staircase { .. } => "staircase", + SetpointProfile::UpDown => "up-down", + } +} + +/// 由解算后的权重与场景构造目标函数。 +fn objective(settings: &Settings, limits: LoopLimits) -> Objective { + Objective::new(settings.tuning.weights, settings.tuning.scenario, limits) +} + +/// 已知的机械 / 安全参数（CLI > `chassis.toml` > 内置默认）。 +#[derive(Debug, Clone)] +struct Hardware { + /// 轮名。 + name: String, + /// 电调 ID。 + esc_id: u8, + /// 安装方向。 + direction: f32, + /// 减速比。 + gear_ratio: f32, + /// 限幅与型号。 + limits: LoopLimits, + /// 基线增益（来自 `--config`；仅用于对比）。 + baseline: Option, +} + +/// 解析机械 / 安全参数：显式 CLI 覆盖 `--config`，`--config` 覆盖内置默认。 +/// +/// 这些参数**不参与搜索**（它们是安全与机械约束），但必须来自同一份配置—— +/// 否则"整定工具用的限流"和"守护进程用的限流"会悄悄不一致。 +fn hardware(settings: &Settings) -> Result { + let config = settings + .config + .as_ref() + .map(|path| Config::load(path).with_context(|| format!("读取配置 {}", path.display()))) + .transpose()?; + + let motor = match (&config, &settings.motor) { + (Some(config), name) => { + let index = config.motor_index(name).with_context(|| { + format!( + "配置里没有名为 '{name}' 的电机（可用：{}）", + config.motors.iter().map(|m| m.name.as_str()).collect::>().join(", ") + ) + })?; + Some(&config.motors[index]) + } + (None, _) => None, + }; + + let limits = LoopLimits { + current_limit_raw: settings + .current_limit + .or_else(|| motor.map(|m| m.current_limit)) + .unwrap_or(12_000), + model: settings + .model + .map(Into::into) + .or_else(|| motor.map(|m| m.model.into())) + .unwrap_or(EscModel::C620), + }; + if limits.current_limit_raw <= 0 { + bail!("电流限幅必须为正，得到 {}", limits.current_limit_raw); + } + + Ok(Hardware { + name: settings.motor.clone(), + esc_id: settings.bench.esc_id.or_else(|| motor.map(|m| m.esc_id)).unwrap_or(1), + direction: settings.bench.direction.or_else(|| motor.map(|m| m.direction)).unwrap_or(1.0), + gear_ratio: settings + .bench + .gear_ratio + .or_else(|| motor.map(|m| m.gear_ratio)) + .unwrap_or(19.203), + baseline: motor.map(chassis_config::MotorConfig::velocity_gains), + limits, + }) +} + +/// 线程池：真机后端强制单线程（只有一条总线），仿真后端默认吃满所有核。 +fn install_thread_pool(threads: usize) -> Result<()> { + let mut builder = rayon::ThreadPoolBuilder::new(); + if threads > 0 { + builder = builder.num_threads(threads); + } + builder.build_global().context("初始化线程池失败")?; + Ok(()) +} + +fn progress_bar(settings: &Settings) -> ProgressBar { + ProgressBar::new(settings.tuning.search.samples as u64 + settings.tuning.search.top_k as u64) +} + +/// 构造后端（仿真或真机）。 +fn build_backend( + settings: &Settings, + hardware: &Hardware, + limits: LoopLimits, + objective: Objective, +) -> Result { + match settings.tuning.backend { + BackendSpec::Sim => { + let ensemble = PlantEnsemble::randomized( + settings.tuning.plant, + settings.tuning.tolerances, + settings.tuning.ensemble, + settings.tuning.search.seed, + ); + Ok(Backend::Sim(Box::new(SimEvaluator::new( + ensemble, + settings.tuning.scenario, + limits, + objective, + settings.tuning.robust_weight, + )))) + } + BackendSpec::Bench => { + Ok(Backend::Bench(Box::new(open_bench(settings, hardware, limits, objective)?))) + } + } +} + +// ---------------------------------------------------------------- presets + +/// 列出预设文件里的全部预设（只读，不搜索、不碰硬件）。 +fn presets_cmd(args: &PresetsArgs) -> Result { + let file = PresetFile::load(&args.preset) + .with_context(|| format!("加载预设文件 {}", args.preset.display()))?; + println!("=== 预设文件 {}（version {}）===", args.preset.display(), file.version); + println!("共 {} 个预设", file.preset.len()); + for (name, preset) in &file.preset { + let merged = Preset::builtin().overlay(preset); + let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{name}' 失败"))?; + let envelope = + merged.bench_envelope().with_context(|| format!("解算预设 '{name}' 的安全包络失败"))?; + let mark = + if file.default_preset.as_deref() == Some(name.as_str()) { "（默认）" } else { "" }; + println!("\n{name}{mark}：{}", preset.description.as_deref().unwrap_or("（无说明）")); + println!( + " 后端 {}｜场景 {}｜目标 {:.0} rpm｜dt {:.0}µs｜样本 {}｜top-k {}｜种子 {}｜求解器 {}", + tuning.backend.name(), + profile_label(tuning.scenario.profile), + tuning.scenario.target_rpm, + tuning.scenario.dt_s * 1e6, + tuning.search.samples, + tuning.search.top_k, + tuning.search.seed, + tuning.search.solver.name(), + ); + println!(" 空间：{}", tuning.space.describe()); + println!( + " 权重：{}｜验收 超调 ≤ {:.1}% / 稳态误差 ≤ {:.1}%", + tuning.weights.describe(), + tuning.acceptance.max_overshoot_pct, + tuning.acceptance.max_ss_error_pct + ); + println!( + " 被控对象 {}｜集成 {}｜鲁棒权重 {:.2}｜抖动 gain±{:.0}% tau±{:.0}% delay≤{}拍 friction±{:.0}%", + tuning.plant_spec.name(), + tuning.ensemble, + tuning.robust_weight, + tuning.tolerances.gain_pct * 100.0, + tuning.tolerances.tau_pct * 100.0, + tuning.tolerances.delay_ticks_max, + tuning.tolerances.friction_pct * 100.0, + ); + println!( + " 安全包络：过速 {:.0} rpm｜温度 ≤ {}℃｜试验超时 {}ms｜启动等待 {}ms｜试验间隔 {}ms｜复验 {} 次｜安全事件后{}", + envelope.max_rpm, + envelope.max_temp_c, + envelope.trial_timeout_ms, + envelope.startup_timeout_ms, + envelope.between_trials_ms, + envelope.validate_runs, + if envelope.continue_on_abort { "继续" } else { "收工" }, + ); + } + println!("\n用法：chassis-tune sweep --preset {} --preset-name ", args.preset.display()); + Ok(ExitCode::SUCCESS) +} + +// ---------------------------------------------------------------- sweep + +fn sweep_cmd(settings: &Settings) -> Result { + let hardware = hardware(settings)?; + let limits = hardware.limits; + let objective = objective(settings, limits); + let config = settings.tuning.search.clone(); + install_thread_pool(match settings.tuning.backend { + BackendSpec::Bench => 1, + BackendSpec::Sim => settings.tuning.threads, + })?; + + println!("=== chassis-tune sweep ==="); + println!("超参数来源：{}", settings.source); + println!("参数空间：{}", settings.tuning.space.describe()); + println!("场景：{}", chassis_tune::search::scenario_summary(&settings.tuning.scenario)); + println!( + "轮子：{}｜电调 {}｜方向 {:+.0}｜减速比 {:.3}｜限流 {} raw", + hardware.name, + hardware.esc_id, + hardware.direction, + hardware.gear_ratio, + limits.current_limit_raw + ); + + let evaluator = build_backend(settings, &hardware, limits, objective)?; + println!("后端：{}", evaluator.describe()); + println!( + "种子：{}｜粗扫 {}｜top-k {}｜求解器 {}", + config.seed, + config.samples, + config.top_k, + config.solver.name() + ); + + if settings.tuning.backend == BackendSpec::Bench { + let trials = config.samples as u64 + + config.top_k as u64 * 2 + + settings.envelope.validate_runs as u64; + let per_trial = settings.tuning.scenario.total_s() + + settings.envelope.between_trials_ms as f32 / 1000.0; + println!( + "⚠ 真机模式：轮子会转！预计 {} 次试验 × {:.1}s ≈ {:.0}s", + trials, + per_trial, + trials as f32 * per_trial + ); + } + + let bar = progress_bar(settings); + let ranked = sweep(&settings.tuning.space, &config, &evaluator, Some(&bar))?; + bar.finish_and_clear(); + + // 最优轨迹：仿真后端重放名义模型；真机后端用搜索期间记录的最好一次。 + let trace = match &evaluator { + Backend::Sim(_) => dji_esc_sim::simulate( + ranked.best.gains, + &settings.tuning.plant, + &settings.tuning.scenario, + limits, + ) + .ok(), + Backend::Bench(evaluator) => evaluator.best_trace(), + }; + + let ctx = ReportContext { + out_dir: settings.out_dir.clone(), + space: settings.tuning.space, + config: config.clone(), + scenario: settings.tuning.scenario, + limits, + backend: evaluator.describe(), + motor_name: settings.motor.clone(), + max_overshoot_pct: settings.tuning.acceptance.max_overshoot_pct, + max_ss_error_pct: settings.tuning.acceptance.max_ss_error_pct, + best_trace: trace, + }; + let written = write_reports(&ctx, &ranked)?; + print_ranked(&ranked); + if settings.json { + println!("\n{}", serde_json::to_string_pretty(&summary_json(&ctx, &ranked))?); + } + println!("\n产物："); + for path in &written { + println!(" {}", path.display()); + } + + compare_with_baseline(&evaluator, &hardware, &ranked); + // 真机：对最优增益做重复性复验（仿真到真机是否可迁移的证据）。 + if let Some(bench_evaluator) = evaluator.bench() { + hil_validate( + bench_evaluator, + ranked.best.gains, + settings.envelope.validate_runs, + &settings.out_dir, + )?; + print_aborts(bench_evaluator); + } + if let Some(path) = &settings.write_config { + write_config(settings, &hardware, ranked.best.gains, path)?; + } + + let passes = ranked.best.score.metrics.is_some_and(|metrics| ctx.best_passes(&metrics)); + Ok(exit_code(passes)) +} + +/// 与 `--config` 里的现有增益对比（有基线才有意义）。 +fn compare_with_baseline(evaluator: &Backend, hardware: &Hardware, ranked: &Ranked) { + let Some(baseline) = hardware.baseline else { + return; + }; + let base = evaluator.evaluate(&baseline); + println!( + "\n基线（来自 --config）：kp={:.3} ki={:.3} kd={:.3} ff={:.3}｜成本 {:.4}｜判定 {}", + baseline.kp, + baseline.ki, + baseline.kd, + baseline.feedforward, + base.cost, + base.verdict.label() + ); + if base.cost > 0.0 { + println!( + " → 整定后成本 {:.4}（基线的 {:.0}%）", + ranked.best.cost(), + ranked.best.cost() / base.cost * 100.0 + ); + } +} + +/// 打印真机搜索期间触发的安全事件（最多 5 条）。 +fn print_aborts(evaluator: &BenchEvaluator) { + let aborts = evaluator.aborts(); + if aborts.is_empty() { + return; + } + println!("\n⚠ 搜索期间触发 {} 次安全事件：", aborts.len()); + for abort in aborts.iter().take(5) { + println!(" - {abort}"); + } +} + +/// 把整定结果写回一份 `chassis.toml`（只改写目标电机的四个增益）。 +fn write_config( + settings: &Settings, + hardware: &Hardware, + gains: VelocityGains, + path: &Path, +) -> Result<()> { + let source = settings + .config + .as_ref() + .context("--write-config 必须与 --config 一起使用（否则不知道该从哪份配置派生）")?; + let mut config = + Config::load(source).with_context(|| format!("读取配置 {}", source.display()))?; + config + .set_motor_gains_by_name(&hardware.name, gains) + .with_context(|| format!("配置里没有名为 '{}' 的电机", hardware.name))?; + let text = config.to_toml_string().context("序列化配置失败")?; + std::fs::write(path, text).with_context(|| format!("写入 {}", path.display()))?; + println!( + "\n已写出 {}（仅 {} 的 kp/ki/kd/feedforward 改写为 {:.3}/{:.3}/{:.3}/{:.3}）", + path.display(), + hardware.name, + gains.kp, + gains.ki, + gains.kd, + gains.feedforward + ); + Ok(()) +} + +/// `hil-validation.csv` 的一行。 +#[derive(Debug, Serialize)] +struct HilRow { + run: usize, + cost: f64, + verdict: &'static str, + settled: bool, + overshoot_pct: f32, + ss_error_pct: f32, + settling_time_s: f32, + peak_effort_raw: f32, +} + +/// 真机重复性复验：同一组增益跑 `runs` 次，写出 `hil-validation.csv`。 +fn hil_validate( + evaluator: &BenchEvaluator, + gains: VelocityGains, + runs: usize, + out_dir: &Path, +) -> Result<()> { + let runs = runs.max(1); + println!("\n=== 真机复验（{runs} 次）==="); + let mut rows = Vec::with_capacity(runs); + for run in 0..runs { + let score = evaluator.evaluate(&gains); + let metrics = score.metrics; + let row = HilRow { + run: run + 1, + cost: score.cost, + verdict: score.verdict.label(), + settled: metrics.is_some_and(|m| m.settled), + overshoot_pct: metrics.map_or(f32::NAN, |m| m.overshoot_pct), + ss_error_pct: metrics.map_or(f32::NAN, |m| m.ss_error_pct), + settling_time_s: metrics.map_or(f32::NAN, |m| m.settling_time_s), + peak_effort_raw: metrics.map_or(f32::NAN, |m| m.peak_effort_raw), + }; + println!( + " 第 {} 次：{}｜超调 {:.1}%｜稳态误差 {:.2}%｜调节 {:.0}ms", + row.run, + row.verdict, + row.overshoot_pct, + row.ss_error_pct, + row.settling_time_s * 1000.0 + ); + rows.push(row); + } + let ok = rows.iter().filter(|row| row.verdict == "ok").count(); + println!("复验结果：{ok} / {runs} 次达标"); + + let path = out_dir.join("hil-validation.csv"); + let mut writer = + csv::Writer::from_path(&path).with_context(|| format!("写出 {}", path.display()))?; + for row in &rows { + writer.serialize(row)?; + } + writer.flush()?; + println!(" → {}", path.display()); + Ok(()) +} + +fn print_ranked(ranked: &Ranked) { + println!("\n=== Top 5 ==="); + println!( + "{:<4} {:<8} {:>9} {:>9} {:>9} {:>9} {:>10} {:<11}", + "#", "阶段", "kp", "ki", "kd", "ff", "成本", "判定" + ); + for (rank, trial) in ranked.top(5).iter().enumerate() { + println!( + "{:<4} {:<8} {:>9.3} {:>9.3} {:>9.3} {:>9.3} {:>10.4} {:<11}", + rank + 1, + trial.phase.label(), + trial.gains.kp, + trial.gains.ki, + trial.gains.kd, + trial.gains.feedforward, + trial.cost(), + trial.score.verdict.label() + ); + } + let best = &ranked.best; + println!( + "\n最优：kp={:.3} ki={:.3} kd={:.3} ff={:.3}（成本 {:.4}，判定 {}）", + best.gains.kp, + best.gains.ki, + best.gains.kd, + best.gains.feedforward, + best.cost(), + best.score.verdict.label() + ); + if let Some(metrics) = best.score.metrics { + println!( + "指标：上升 {:.0}ms｜超调 {:.1}%｜调节 {:.0}ms｜稳态误差 {:.2}%｜峰值电流 {:.0} raw｜饱和 {:.1}%", + metrics.rise_time_s * 1000.0, + metrics.overshoot_pct, + metrics.settling_time_s * 1000.0, + metrics.ss_error_pct, + metrics.peak_effort_raw, + metrics.saturated_pct + ); + } + println!( + "评估 {} 次：达标 {} / {}（{} 次被硬约束拦下）", + ranked.evals, + ranked.ok_count(), + ranked.trials.len(), + ranked.rejected + ); +} + +fn summary_json(ctx: &ReportContext, ranked: &Ranked) -> serde_json::Value { + serde_json::json!({ + "backend": ctx.backend, + "seed": ctx.config.seed, + "samples": ctx.config.samples, + "evals": ranked.evals, + "ok": ranked.ok_count(), + "rejected": ranked.rejected, + "best": { + "kp": ranked.best.gains.kp, + "ki": ranked.best.gains.ki, + "kd": ranked.best.gains.kd, + "feedforward": ranked.best.gains.feedforward, + "cost": ranked.best.cost(), + "verdict": ranked.best.score.verdict.label(), + }, + }) +} + +fn exit_code(passes: bool) -> ExitCode { + if passes { ExitCode::SUCCESS } else { ExitCode::from(1) } +} + +// ---------------------------------------------------------------- validate + +fn validate_cmd(settings: &Settings, gains_text: &str) -> Result { + let hardware = hardware(settings)?; + let gains = parse_gains(gains_text)?; + let limits = hardware.limits; + let objective = objective(settings, limits); + // 单组增益不需要并行：单线程既省事又让真机路径的顺序确定。 + install_thread_pool(1)?; + + let evaluator = build_backend(settings, &hardware, limits, objective.clone())?; + println!("=== chassis-tune validate ==="); + println!("超参数来源：{}", settings.source); + println!("后端：{}", evaluator.describe()); + println!("增益：kp={} ki={} kd={} ff={}", gains.kp, gains.ki, gains.kd, gains.feedforward); + + let score = evaluator.evaluate(&gains); + let acceptance = settings.tuning.acceptance; + let passes = score.metrics.is_some_and(|metrics| { + objective.passes(&metrics, acceptance.max_overshoot_pct, acceptance.max_ss_error_pct) + }); + println!("判定：{}（成本 {:.6}）", score.verdict.label(), score.cost); + match score.metrics { + Some(metrics) => { + println!( + "上升 {:.0}ms｜超调 {:.1}%｜调节 {:.0}ms｜稳态误差 {:.2}%｜峰值电流 {:.0} raw｜饱和 {:.1}%", + metrics.rise_time_s * 1000.0, + metrics.overshoot_pct, + metrics.settling_time_s * 1000.0, + metrics.ss_error_pct, + metrics.peak_effort_raw, + metrics.saturated_pct + ); + println!( + "验收（超调 ≤ {:.1}%，稳态误差 ≤ {:.1}%）：{}", + acceptance.max_overshoot_pct, + acceptance.max_ss_error_pct, + if passes { "✓ 通过" } else { "✗ 未通过" } + ); + } + None => println!("没有可用指标（{}）", chassis_tune::search::verdict_hint(score.verdict)), + } + Ok(exit_code(passes)) +} + +/// `kp,ki,kd,ff` → 增益组。 +fn parse_gains(text: &str) -> Result { + let parts: Vec<&str> = text.split(',').map(str::trim).collect(); + if parts.len() != 4 { + bail!("--gains 需要四个逗号分隔的数值（kp,ki,kd,feedforward），得到 '{text}'"); + } + let value = |index: usize| -> Result { + parts[index] + .parse::() + .with_context(|| format!("--gains 第 {} 项 '{}' 不是数值", index + 1, parts[index])) + }; + let gains = VelocityGains::new(value(0)?, value(1)?, value(2)?, value(3)?); + if !gains.is_finite() { + bail!("--gains 必须是有限值"); + } + Ok(gains) +} + +// ---------------------------------------------------------------- bench + +/// 打开真机后端（含安全横幅与确认门控）。 +fn open_bench( + settings: &Settings, + hardware: &Hardware, + limits: LoopLimits, + objective: Objective, +) -> Result { + let bench = &settings.bench; + let envelope_spec = settings.envelope; + if !bench.i_know_the_wheel_spins { + bail!( + "真机模式必须显式确认：加 `--i-know-the-wheel-spins`。\n \ + 前提：轮子架空或整车固定、急停可及、电流限流已按台架设置（当前 {} raw）。", + limits.current_limit_raw + ); + } + println!("⚠ 台架模式：轮子会以最高 {:.0} rpm（转子）转动。", envelope_spec.max_rpm); + println!(" 确认：轮子已架空或整车固定，急停可及，ctrl-c 随时安全停机。"); + + let link = open_link( + &bench.device, + bench.serial_baud, + bench.can_speed.into(), + bench.interface.as_deref(), + )?; + + let mut esc = EscConfig::gm3508(hardware.name.clone(), hardware.esc_id, hardware.direction); + esc.gear_ratio = hardware.gear_ratio; + esc.current_limit = limits.current_limit_raw; + esc.model = limits.model; + + let envelope = SafetyEnvelope::new() + .with_overspeed(envelope_spec.max_rpm, Duration::from_millis(60)) + .with_temperature_max(envelope_spec.max_temp_c) + .with_feedback_timeout(Duration::from_millis(200)) + .with_trial_timeout(Duration::from_millis(envelope_spec.trial_timeout_ms)); + + let mut config = BenchConfig::new(esc, settings.tuning.scenario, envelope, objective); + config.between_trials = Duration::from_millis(envelope_spec.between_trials_ms); + config.startup_timeout = Duration::from_millis(envelope_spec.startup_timeout_ms); + config.continue_on_abort = envelope_spec.continue_on_abort; + + BenchEvaluator::open(link, config).map_err(|e| anyhow::anyhow!("{e}")) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// 一份最小可用配置（四轮，顺序 FL/FR/RL/RR）。 + const CONFIG: &str = r#" +[can] +interface = "can0" + +[mecanum] +half_length = 0.2 +half_width = 0.25 +wheel_radius = 0.0762 + +[[motor]] +name = "front_left" +esc_id = 3 +direction = -1 +gear_ratio = 19.203 +current_limit = 6000 +kp = 2.0 +ki = 10.0 +kd = 0.0 +feedforward = 0.0 + +[[motor]] +name = "front_right" +esc_id = 4 +direction = 1 +gear_ratio = 19.203 +current_limit = 6000 +kp = 2.0 +ki = 10.0 +kd = 0.0 +feedforward = 0.0 + +[[motor]] +name = "rear_left" +esc_id = 5 +direction = -1 +gear_ratio = 19.203 +current_limit = 6000 +kp = 2.0 +ki = 10.0 +kd = 0.0 +feedforward = 0.0 + +[[motor]] +name = "rear_right" +esc_id = 6 +direction = 1 +gear_ratio = 19.203 +current_limit = 6000 +kp = 2.0 +ki = 10.0 +kd = 0.0 +feedforward = 0.0 +"#; + + fn write_temp(name: &str, text: &str) -> PathBuf { + let path = std::env::temp_dir().join(format!("chassis-tune-cli-{name}.toml")); + std::fs::write(&path, text).unwrap(); + path + } + + /// 仓库里随代码一起评审的预设文件。 + fn repo_preset_path() -> PathBuf { + Path::new(env!("CARGO_MANIFEST_DIR")).join("../../config/tune.toml") + } + + fn args_with(extra: &[&str]) -> CommonArgs { + let mut argv = vec!["chassis-tune", "sweep"]; + argv.extend_from_slice(extra); + match Cli::parse_from(argv).command { + Command::Sweep(sweep) => sweep.common, + _ => panic!("expected sweep"), + } + } + + /// 解算后的设置（内置默认 + 额外 CLI）。 + fn settings_with(extra: &[&str]) -> Settings { + args_with(extra).resolve().unwrap() + } + + #[test] + fn hardware_defaults_without_config() { + let settings = settings_with(&[]); + let hardware = hardware(&settings).unwrap(); + assert_eq!(hardware.esc_id, 1); + assert!((hardware.direction - 1.0).abs() < 1e-6); + assert!((hardware.gear_ratio - 19.203).abs() < 1e-6); + assert_eq!(hardware.limits.current_limit_raw, 12_000); + assert!(hardware.baseline.is_none()); + } + + #[test] + fn hardware_takes_mechanical_parameters_from_config() { + let path = write_temp("mech", CONFIG); + let settings = + settings_with(&["--config", path.to_str().unwrap(), "--motor", "front_left"]); + let hardware = hardware(&settings).unwrap(); + assert_eq!(hardware.esc_id, 3, "电调 ID 必须来自配置"); + assert_eq!(hardware.direction.to_bits(), (-1.0f32).to_bits()); + assert_eq!(hardware.limits.current_limit_raw, 6000); + assert_eq!(hardware.baseline.unwrap(), VelocityGains::new(2.0, 10.0, 0.0, 0.0)); + let _ = std::fs::remove_file(&path); + } + + #[test] + fn explicit_cli_overrides_the_config() { + let path = write_temp("override", CONFIG); + let settings = settings_with(&[ + "--config", + path.to_str().unwrap(), + "--motor", + "front_left", + "--current-limit", + "1500", + ]); + let hardware = hardware(&settings).unwrap(); + assert_eq!(hardware.limits.current_limit_raw, 1500, "显式 CLI 必须覆盖配置"); + assert_eq!(hardware.esc_id, 3, "未显式给出的字段仍取配置"); + let _ = std::fs::remove_file(&path); + } + + #[test] + fn unknown_motor_name_is_rejected() { + let path = write_temp("unknown", CONFIG); + let settings = settings_with(&["--config", path.to_str().unwrap(), "--motor", "ghost"]); + let error = hardware(&settings).unwrap_err().to_string(); + assert!(error.contains("ghost"), "{error}"); + let _ = std::fs::remove_file(&path); + } + + #[test] + fn write_config_only_touches_the_target_motor() { + let source = write_temp("write-src", CONFIG); + let out = std::env::temp_dir().join("chassis-tune-cli-write-out.toml"); + let settings = settings_with(&[ + "--config", + source.to_str().unwrap(), + "--motor", + "front_left", + "--write-config", + out.to_str().unwrap(), + ]); + let hardware = hardware(&settings).unwrap(); + let gains = VelocityGains::new(4.5, 22.0, 0.25, 0.75); + write_config(&settings, &hardware, gains, &out).unwrap(); + + let written = Config::load(&out).unwrap(); + assert_eq!(written.motors[0].velocity_gains(), gains); + assert_eq!( + written.motors[1].velocity_gains(), + VelocityGains::new(2.0, 10.0, 0.0, 0.0), + "其它电机不得被改写" + ); + assert_eq!(written.motors[0].current_limit, 6000, "安全字段不得被改写"); + assert_eq!(written.motors[0].direction.to_bits(), (-1.0f32).to_bits()); + let _ = std::fs::remove_file(&source); + let _ = std::fs::remove_file(&out); + } + + #[test] + fn write_config_requires_config() { + let settings = settings_with(&[]); + let hardware = hardware(&settings).unwrap(); + let out = std::env::temp_dir().join("chassis-tune-cli-never.toml"); + let error = + write_config(&settings, &hardware, VelocityGains::ZERO, &out).unwrap_err().to_string(); + assert!(error.contains("--config"), "{error}"); + assert!(!out.exists(), "报错时不得写出文件"); + } + + // ------------------------------------------------ 超参数优先级 + + #[test] + fn defaults_match_the_documented_builtin_values() { + let settings = settings_with(&[]); + assert_eq!(settings.source, "内置默认"); + let tuning = &settings.tuning; + assert_eq!(tuning.backend, BackendSpec::Sim); + assert!((tuning.space.kp.min - 0.5).abs() < 1e-12); + assert!((tuning.space.kp.max - 30.0).abs() < 1e-12); + assert_eq!(tuning.search.samples, 512); + assert_eq!(tuning.search.seed, 20_260_101); + assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6); + assert!((tuning.acceptance.max_overshoot_pct - 20.0).abs() < 1e-6); + assert_eq!(tuning.plant_spec, PlantSpec::Gm3508Bench); + assert_eq!(settings.envelope.validate_runs, 3); + } + + #[test] + fn preset_supplies_hyperparameters_and_cli_overrides_them() { + let path = write_temp( + "preset", + r#" +version = 1 +default_preset = "quick" + +[preset.quick] +description = "快跑" + +[preset.quick.space] +kp = { min = 1.0, max = 4.0, scale = "log" } + +[preset.quick.search] +samples = 24 +top_k = 3 +seed = 7 + +[preset.other] + +[preset.other.search] +samples = 99 +"#, + ); + + // 预设生效，来源可追溯。 + let settings = settings_with(&["--preset", path.to_str().unwrap()]); + assert!(settings.source.ends_with("#quick"), "{}", settings.source); + assert_eq!(settings.tuning.search.samples, 24); + assert_eq!(settings.tuning.search.top_k, 3); + assert_eq!(settings.tuning.search.seed, 7); + assert!((settings.tuning.space.kp.max - 4.0).abs() < 1e-12); + // 预设没写的项继续沿用内置默认。 + assert_eq!(settings.tuning.search.max_iters, 200); + assert!((settings.tuning.scenario.target_rpm - 1500.0).abs() < 1e-6); + + // 显式 CLI 覆盖预设的同一项，但不影响预设的其它项。 + let settings = settings_with(&["--preset", path.to_str().unwrap(), "--samples", "128"]); + assert_eq!(settings.tuning.search.samples, 128, "CLI 必须覆盖预设"); + assert_eq!(settings.tuning.search.top_k, 3, "预设的其它项必须保留"); + assert_eq!(settings.tuning.search.seed, 7); + + // 按名字选另一个预设。 + let settings = + settings_with(&["--preset", path.to_str().unwrap(), "--preset-name", "other"]); + assert!(settings.source.ends_with("#other"), "{}", settings.source); + assert_eq!(settings.tuning.search.samples, 99); + + let _ = std::fs::remove_file(&path); + } + + #[test] + fn unknown_preset_name_lists_the_available_ones() { + let path = write_temp("unknown-preset", "version = 1\n[preset.alpha]\n[preset.beta]\n"); + let error = args_with(&["--preset", path.to_str().unwrap(), "--preset-name", "ghost"]) + .resolve() + .unwrap_err() + .to_string(); + assert!(error.contains("ghost"), "{error}"); + assert!(error.contains("alpha") && error.contains("beta"), "{error}"); + let _ = std::fs::remove_file(&path); + } + + #[test] + fn preset_name_without_file_falls_back_to_the_default_path() { + // 用临时文件充作"默认路径"，验证"只给 --preset-name"的回落逻辑 + // （不依赖当前工作目录，因此可测）。 + let path = write_temp( + "fallback", + "version = 1\ndefault_preset = \"only\"\n[preset.only]\n[preset.only.search]\nsamples = 11\n", + ); + let (file, prefix) = preset_source(None, Some("only"), &path).unwrap(); + assert!(prefix.ends_with('#'), "{prefix}"); + assert_eq!(file.select(Some("only")).unwrap().search.samples, Some(11)); + + // 默认路径不存在时必须给出可操作的报错。 + let error = preset_source(None, Some("only"), Path::new("/definitely/not/here/tune.toml")) + .unwrap_err() + .to_string(); + assert!(error.contains("--preset"), "{error}"); + + // 没给名字也没给文件 → 内置库。 + let (file, prefix) = preset_source(None, None, &path).unwrap(); + assert!(prefix.is_empty()); + assert!(file.preset.contains_key("default")); + + // 端到端：仓库里的 config/tune.toml 能按名字直接选中（避开 cwd 依赖）。 + let settings = args_with(&[ + "--preset", + repo_preset_path().to_str().unwrap(), + "--preset-name", + "bench_noload", + ]) + .resolve() + .unwrap(); + assert!(settings.source.ends_with("#bench_noload"), "{}", settings.source); + assert_eq!(settings.tuning.backend, BackendSpec::Bench); + + let _ = std::fs::remove_file(&path); + } + + #[test] + fn presets_cannot_smuggle_safety_parameters() { + // current_limit / direction / gear_ratio 不是预设的字段：写进去必须报错， + // 否则预设文件就成了绕过限流的口子。 + for key in ["current_limit = 16384", "direction = -1.0", "gear_ratio = 1.0"] { + let text = format!("version = 1\n[preset.a.bench]\n{key}\n"); + assert!(PresetFile::parse_str(&text).is_err(), "'{key}' 不得被预设接受"); + } + } + + #[test] + fn scale_can_be_overridden_without_touching_the_range() { + // 只给 --kp-scale：区间必须仍然来自内置默认。 + let settings = settings_with(&["--kp-scale", "linear"]); + assert_eq!(settings.tuning.space.kp.scale, chassis_tune::Scale::Linear); + assert!((settings.tuning.space.kp.min - 0.5).abs() < 1e-12); + assert!((settings.tuning.space.kp.max - 30.0).abs() < 1e-12); + + // 只给区间：刻度必须从内置默认继承（Log）。 + let settings = settings_with(&["--kp", "1..10"]); + assert!((settings.tuning.space.kp.min - 1.0).abs() < 1e-12); + assert!((settings.tuning.space.kp.max - 10.0).abs() < 1e-12); + assert_eq!(settings.tuning.space.kp.scale, chassis_tune::Scale::Log); + } + + #[test] + fn continue_on_abort_only_ever_turns_on() { + assert!(!settings_with(&[]).envelope.continue_on_abort); + assert!(settings_with(&["--continue-on-abort"]).envelope.continue_on_abort); + } + + #[test] + fn the_repo_preset_file_resolves_completely() { + let path = repo_preset_path(); + let file = PresetFile::load(&path).unwrap_or_else(|error| { + panic!("{} 必须能被加载：{error}", path.display()); + }); + assert!(!file.preset.is_empty()); + let default = file.default_preset.as_deref().expect("必须指定 default_preset"); + assert!(file.preset.contains_key(default), "default_preset '{default}' 必须存在"); + for (name, preset) in &file.preset { + let merged = Preset::builtin().overlay(preset); + let tuning = merged + .to_tuning() + .unwrap_or_else(|error| panic!("预设 '{name}' 无法解算：{error}")); + merged + .bench_envelope() + .unwrap_or_else(|error| panic!("预设 '{name}' 的安全包络非法：{error}")); + assert!( + preset.description.as_deref().is_some_and(|text| !text.is_empty()), + "预设 '{name}' 必须有说明（评审时要知道它是给什么工况用的）" + ); + // 命名约定必须与后端一致：否则一个叫 bench_* 的预设可能悄悄跑离线， + // 拿它当"台架已验证"的证据就是错的。 + let expected = if name.starts_with("bench_") || name.starts_with("field_") { + BackendSpec::Bench + } else { + BackendSpec::Sim + }; + assert_eq!( + tuning.backend, + expected, + "预设 '{name}' 的后端 ({}) 与命名约定不符", + tuning.backend.name() + ); + } + } + + #[test] + fn presets_cmd_lists_every_preset() { + let args = PresetsArgs { preset: repo_preset_path() }; + assert_eq!(presets_cmd(&args).unwrap(), ExitCode::SUCCESS); + } + + // ------------------------------------------------ 参数校验 + + #[test] + fn gain_space_comes_from_cli_ranges() { + let settings = settings_with(&["--kp", "1..10", "--ki", "0..5", "--ki-scale", "linear"]); + let space = settings.tuning.space; + assert!((space.kp.min - 1.0).abs() < 1e-12); + assert!((space.kp.max - 10.0).abs() < 1e-12); + assert_eq!(space.ki.scale, chassis_tune::Scale::Linear); + } + + #[test] + fn bad_ranges_are_rejected() { + assert!(args_with(&["--kp", "abc"]).resolve().is_err()); + assert!(args_with(&["--kp=-1..5"]).resolve().is_err(), "对数轴的负下界必须被拒绝"); + } + + #[test] + fn scenario_comes_from_cli() { + let settings = settings_with(&[ + "--dt-us", + "2000", + "--ramp-ms", + "100", + "--hold-ms", + "900", + "--target-rpm", + "800", + ]); + let scenario = settings.tuning.scenario; + assert!((scenario.dt_s - 0.002).abs() < 1e-9); + assert!((scenario.ramp_s - 0.1).abs() < 1e-6); + assert!((scenario.total_s() - 1.0).abs() < 1e-6); + assert!((scenario.target_rpm - 800.0).abs() < 1e-6); + } + + #[test] + fn degenerate_scenario_is_rejected() { + assert!(args_with(&["--dt-us", "0"]).resolve().is_err()); + assert!( + args_with(&["--hold-ms", "0", "--ramp-ms", "0", "--scenario", "step"]) + .resolve() + .is_err() + ); + } + + #[test] + fn gains_parse_requires_four_finite_values() { + assert_eq!(parse_gains("2, 10, 0, 0.5").unwrap(), VelocityGains::new(2.0, 10.0, 0.0, 0.5)); + assert!(parse_gains("2,10,0").is_err()); + assert!(parse_gains("2,10,0,inf").is_err()); + } + + #[test] + fn jitter_parses_known_keys_only() { + let settings = settings_with(&["--jitter", "gain=0.1,tau=0.2,delay=2,friction=0.3"]); + let tolerances = settings.tuning.tolerances; + assert!((tolerances.gain_pct - 0.1).abs() < 1e-6); + assert!((tolerances.tau_pct - 0.2).abs() < 1e-6); + assert_eq!(tolerances.delay_ticks_max, 2); + assert!((tolerances.friction_pct - 0.3).abs() < 1e-6); + + assert!(args_with(&["--jitter", "gain"]).resolve().is_err()); + assert!(args_with(&["--jitter", "gain=abc"]).resolve().is_err()); + assert!(args_with(&["--jitter", "unknown=1"]).resolve().is_err()); + } + + #[test] + fn negative_weights_are_rejected() { + // 负数必须用 `=` 形式，否则 clap 会把它当成选项。 + assert!(args_with(&["--w-iae=-1"]).resolve().is_err()); + assert!(args_with(&["--max-overshoot=-1"]).resolve().is_err()); + } + + #[test] + fn cli_requires_a_subcommand() { + assert!(Cli::try_parse_from(["chassis-tune"]).is_err()); + } +} diff --git a/crates/chassis-tune/src/metrics.rs b/crates/chassis-tune/src/metrics.rs new file mode 100644 index 0000000..f77b326 --- /dev/null +++ b/crates/chassis-tune/src/metrics.rs @@ -0,0 +1,265 @@ +//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。 +//! +//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能 +//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。 + +use dji_esc_sim::{Scenario, Trace}; + +/// 稳态判据的相对带宽（±2% of target）。 +pub const SETTLING_BAND: f32 = 0.02; +/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。 +/// +/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡， +/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。 +pub const STEADY_FRACTION: f32 = 0.75; +/// 指标最少需要的采样数（样本太少时一切统计都无意义）。 +pub const MIN_SAMPLES: usize = 8; + +/// 一条步响应轨迹的指标集合。 +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct StepMetrics { + /// 上升时间 10% → 90%（秒）；未到达时等于轨迹总时长。 + pub rise_time_s: f32, + /// 超调量（%，相对目标；负值截为 0）。 + pub overshoot_pct: f32, + /// 调节时间：最后一次离开 ±2% 带的时刻（秒）。 + pub settling_time_s: f32, + /// 是否在稳态窗口内保持 ±2% 带（即真正收敛）。 + pub settled: bool, + /// 稳态误差（%，相对目标）。 + pub ss_error_pct: f32, + /// 绝对误差积分 ∫|e|dt（归一化前的原始值）。 + pub iae: f32, + /// 误差平方积分 ∫e²dt。 + pub ise: f32, + /// 时间加权绝对误差积分 ∫t|e|dt。 + pub itae: f32, + /// 峰值 effort（raw，绝对值）。 + pub peak_effort_raw: f32, + /// effort 均方根（raw）。 + pub rms_effort_raw: f32, + /// effort 符号翻转次数（抖振）。 + pub effort_reversals: usize, + /// 饱和采样占比（%）。 + pub saturated_pct: f32, + /// 全过程最大转速（rpm，绝对值）。 + pub max_rpm: f32, + /// 稳态窗口内的平均转速（rpm）。 + pub steady_rpm: f32, +} + +impl StepMetrics { + /// 从轨迹计算指标；样本不足或目标转速为 0 时返回 `None`。 + #[must_use] + pub fn from_trace(trace: &Trace, scenario: &Scenario) -> Option { + let target = scenario.target_rpm; + if trace.len() < MIN_SAMPLES || trace.len() != trace.t_s.len() { + return None; + } + if !target.is_finite() || target.abs() < f32::EPSILON { + return None; + } + let sign = if target < 0.0 { -1.0 } else { 1.0 }; + let magnitude = target.abs(); + + // 上升时间：10% / 90% 两个阈值首次被跨过的时刻。 + let t10 = trace + .rpm + .iter() + .zip(&trace.t_s) + .find(|(rpm, _)| **rpm * sign >= 0.1 * magnitude) + .map(|(_, &t)| t); + let t90 = trace + .rpm + .iter() + .zip(&trace.t_s) + .find(|(rpm, _)| **rpm * sign >= 0.9 * magnitude) + .map(|(_, &t)| t); + let total_s = trace.t_s.last().copied().unwrap_or(0.0); + let rise_time_s = match (t10, t90) { + (Some(t10), Some(t90)) if t90 >= t10 => t90 - t10, + // 没有跨过 90%：用剩余时长表达"到不了"，让成本函数自然惩罚它。 + _ => total_s, + }; + + // 超调：朝着目标方向的过冲。 + let peak_directed = + trace.rpm.iter().map(|rpm| *rpm * sign).fold(f32::NEG_INFINITY, f32::max); + let overshoot_pct = ((peak_directed - magnitude) / magnitude * 100.0).max(0.0); + + // 稳态窗口：场景给出的起点与"最后 25% 时长"取更晚者。 + let steady_start = (total_s * STEADY_FRACTION).max(scenario.steady_start_s()); + let steady_end = scenario.steady_end_s().max(steady_start); + let window = trace.window(steady_start, steady_end); + let steady_rpm = window.mean_rpm(); + let ss_error_pct = ((steady_rpm - target).abs() / magnitude) * 100.0; + + // 调节时间：最后一次离开 ±band 的时刻；此后一直留在带内才算收敛。 + let band = SETTLING_BAND * magnitude; + let mut settling_time_s = 0.0f32; + for (rpm, &t) in trace.rpm.iter().zip(&trace.t_s) { + if (*rpm - target).abs() > band { + settling_time_s = t; + } + } + // 收敛 = 最后一次离开带宽的时刻已经进入稳态窗口（此后一直留在带内）。 + let settled = settling_time_s <= steady_start && !window.is_empty(); + + // 误差积分（全过程，按 dt 累加）。 + let mut abs_error_area = 0.0f32; + let mut squared_error_area = 0.0f32; + let mut time_weighted_area = 0.0f32; + for index in 1..trace.len() { + let dt = (trace.t_s[index] - trace.t_s[index - 1]).max(0.0); + let error = trace.setpoint_rpm[index] - trace.rpm[index]; + abs_error_area += error.abs() * dt; + squared_error_area += error * error * dt; + time_weighted_area += trace.t_s[index] * error.abs() * dt; + } + + let peak_effort_raw = + trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(0.0f32, f32::max); + let rms_effort_raw = if trace.effort_raw.is_empty() { + 0.0 + } else { + (trace.effort_raw.iter().map(|&raw| f32::from(raw) * f32::from(raw)).sum::() + / trace.effort_raw.len() as f32) + .sqrt() + }; + + Some(Self { + rise_time_s, + overshoot_pct, + settling_time_s, + settled, + ss_error_pct, + iae: abs_error_area, + ise: squared_error_area, + itae: time_weighted_area, + peak_effort_raw, + rms_effort_raw, + effort_reversals: trace.effort_reversals(), + saturated_pct: trace.saturated_fraction() * 100.0, + max_rpm: trace.peak_rpm(), + steady_rpm, + }) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn scenario() -> Scenario { + Scenario::step(1000.0, 1.0) + } + + /// 合成一条轨迹：一阶响应 + 可选超调 + 可选末端偏差。 + fn synthetic(alpha: f32, overshoot: f32, final_offset: f32) -> Trace { + let scenario = scenario(); + let dt = scenario.dt_s; + let steps = scenario.samples(); + let mut trace = Trace::with_capacity(steps); + let mut rpm = 0.0f32; + for step in 0..steps { + let t = step as f32 * dt; + let target = scenario.setpoint_at(t); + rpm += (target - rpm) * alpha; + let value = if t > 0.3 { + rpm + overshoot * 1000.0 * (-(t - 0.3) * 20.0).exp() + final_offset + } else { + rpm + }; + trace.push(t, target, value, 0, false); + } + trace + } + + #[test] + fn first_order_response_has_no_overshoot_and_settles() { + let trace = synthetic(0.02, 0.0, 0.0); + let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap(); + assert!(metrics.overshoot_pct < 0.5, "overshoot = {}", metrics.overshoot_pct); + assert!(metrics.settled, "一阶响应必须收敛"); + assert!(metrics.rise_time_s > 0.0 && metrics.rise_time_s < 1.0); + assert!(metrics.ss_error_pct < 1.0, "ss = {}", metrics.ss_error_pct); + assert!(metrics.iae > 0.0 && metrics.itae > 0.0); + } + + #[test] + fn overshoot_is_measured_against_the_target() { + let trace = synthetic(0.05, 0.2, 0.0); + let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap(); + assert!( + (metrics.overshoot_pct - 20.0).abs() < 3.0, + "overshoot = {}", + metrics.overshoot_pct + ); + } + + #[test] + fn steady_state_offset_shows_up() { + let trace = synthetic(0.02, 0.0, 50.0); + let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap(); + assert!((metrics.ss_error_pct - 5.0).abs() < 1.0, "ss = {}", metrics.ss_error_pct); + // 5% 的末端偏差超出 ±2% 带 → 不算收敛。 + assert!(!metrics.settled); + } + + #[test] + fn non_converging_response_is_flagged() { + // 误差始终不减（alpha 很小），且稳态窗口内仍在带外。 + let trace = synthetic(0.0005, 0.0, 0.0); + let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap(); + assert!(!metrics.settled, "未收敛必须被标记"); + assert!(metrics.ss_error_pct > 2.0); + } + + #[test] + fn effort_statistics_and_saturation_are_reported() { + let mut trace = synthetic(0.02, 0.0, 0.0); + for (index, raw) in trace.effort_raw.iter_mut().enumerate() { + *raw = if index % 2 == 0 { 500 } else { -500 }; + } + trace.saturated.fill(true); + let metrics = StepMetrics::from_trace(&trace, &scenario()).unwrap(); + assert!((metrics.peak_effort_raw - 500.0).abs() < 1e-3); + assert!((metrics.rms_effort_raw - 500.0).abs() < 1e-3); + assert_eq!(metrics.effort_reversals, trace.len() - 1); + assert!((metrics.saturated_pct - 100.0).abs() < 1e-6); + } + + #[test] + fn negative_targets_are_handled_symmetrically() { + let scenario = Scenario::step(-1000.0, 1.0); + let steps = scenario.samples(); + let mut trace = Trace::with_capacity(steps); + let mut rpm = 0.0f32; + for step in 0..steps { + let t = step as f32 * scenario.dt_s; + let target = scenario.setpoint_at(t); + rpm += (target - rpm) * 0.02; + trace.push(t, target, rpm, 0, false); + } + let metrics = StepMetrics::from_trace(&trace, &scenario).unwrap(); + assert!(metrics.settled); + assert!(metrics.ss_error_pct < 1.0); + assert!(metrics.overshoot_pct < 1.0); + } + + #[test] + fn degenerate_inputs_are_rejected() { + let short = Trace { + t_s: vec![0.0, 0.001], + setpoint_rpm: vec![1.0, 1.0], + rpm: vec![0.0, 0.1], + effort_raw: vec![0, 0], + saturated: vec![false, false], + }; + assert!(StepMetrics::from_trace(&short, &scenario()).is_none()); + + let zero_target = Scenario::step(0.0, 1.0); + let trace = synthetic(0.02, 0.0, 0.0); + assert!(StepMetrics::from_trace(&trace, &zero_target).is_none()); + } +} diff --git a/crates/chassis-tune/src/plot.rs b/crates/chassis-tune/src/plot.rs new file mode 100644 index 0000000..e78a2bf --- /dev/null +++ b/crates/chassis-tune/src/plot.rs @@ -0,0 +1,129 @@ +//! 可选（`--features plot`）：把最优步响应画成 SVG。 +//! +//! 为什么是 SVG 而不是 PNG：矢量图在任何缩放级别都能看清起动段的细节， +//! 且不需要位图编码器（依赖更小）。默认关闭该 feature——绘图不是整定的 +//! 必要条件，CSV 已经给了全部数据。 + +use std::path::Path; + +use dji_esc_sim::{Scenario, Trace}; +use plotters::backend::SVGBackend; +use plotters::chart::ChartBuilder; +use plotters::drawing::IntoDrawingArea as _; +use plotters::series::LineSeries; +use plotters::style::{BLACK, BLUE, Color as _, RED, WHITE}; + +use crate::report::ReportError; + +/// 写出步响应三联图：转速跟随、误差、电流。 +/// +/// # Errors +/// +/// 绘图或写文件失败时返回 [`ReportError::Plot`]。 +pub fn write_step_svg(path: &Path, trace: &Trace, scenario: &Scenario) -> Result<(), ReportError> { + let root = SVGBackend::new(path, (1000, 900)).into_drawing_area(); + root.fill(&WHITE).map_err(plot_err)?; + let (upper, lower) = root.split_vertically(600); + let (middle, bottom) = lower.split_vertically(150); + + let total = trace.t_s.last().copied().unwrap_or(1.0).max(1.0e-3); + let peak = trace.rpm.iter().copied().fold(scenario.target_rpm, f32::max).max(1.0); + let effort_peak = + trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(1.0f32, f32::max); + + // 转速跟随。 + let mut chart = ChartBuilder::on(&upper) + .caption("velocity step response (rpm)", ("sans-serif", 24)) + .margin(12) + .x_label_area_size(36) + .y_label_area_size(64) + .build_cartesian_2d(0.0f32..total, 0.0f32..peak * 1.1) + .map_err(plot_err)?; + chart.configure_mesh().x_desc("t (s)").y_desc("rpm").draw().map_err(plot_err)?; + chart + .draw_series(LineSeries::new( + trace.t_s.iter().copied().zip(trace.setpoint_rpm.iter().copied()), + &BLACK, + )) + .map_err(plot_err)? + .label("setpoint") + .legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], BLUE)); + chart + .draw_series(LineSeries::new( + trace.t_s.iter().copied().zip(trace.rpm.iter().copied()), + &BLUE, + )) + .map_err(plot_err)? + .label("measured") + .legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], RED)); + chart.configure_series_labels().background_style(WHITE.mix(0.8)).draw().map_err(plot_err)?; + + // 误差。 + let mut error_chart = ChartBuilder::on(&middle) + .caption("error (rpm)", ("sans-serif", 16)) + .margin(8) + .x_label_area_size(28) + .y_label_area_size(64) + .build_cartesian_2d(0.0f32..total, -peak..peak) + .map_err(plot_err)?; + error_chart.configure_mesh().draw().map_err(plot_err)?; + error_chart + .draw_series(LineSeries::new( + trace + .t_s + .iter() + .zip(trace.setpoint_rpm.iter().zip(trace.rpm.iter())) + .map(|(&t, (&sp, &rpm))| (t, sp - rpm)), + &RED, + )) + .map_err(plot_err)?; + + // 电流。 + let mut effort_chart = ChartBuilder::on(&bottom) + .caption("effort (raw)", ("sans-serif", 16)) + .margin(8) + .x_label_area_size(28) + .y_label_area_size(64) + .build_cartesian_2d(0.0f32..total, -effort_peak..effort_peak) + .map_err(plot_err)?; + effort_chart.configure_mesh().draw().map_err(plot_err)?; + effort_chart + .draw_series(LineSeries::new( + trace.t_s.iter().zip(trace.effort_raw.iter()).map(|(&t, &raw)| (t, f32::from(raw))), + &BLUE, + )) + .map_err(plot_err)?; + + root.present().map_err(plot_err)?; + Ok(()) +} + +fn plot_err(error: E) -> ReportError { + ReportError::Plot(format!("{error:?}")) +} + +#[cfg(test)] +mod tests { + use super::*; + use dji_esc_sim::{LoopLimits, PlantParams, SetpointProfile, simulate}; + use motor_core::VelocityGains; + + #[test] + fn writes_a_non_empty_svg() { + let scenario = + Scenario::ramp_and_hold(1200.0, 0.2, 0.8).with_profile(SetpointProfile::RampAndHold); + let trace = simulate( + VelocityGains::new(4.0, 12.0, 0.0, 0.3), + &PlantParams::gm3508(), + &scenario, + LoopLimits::gm3508(), + ) + .unwrap(); + let path = std::env::temp_dir().join("chassis-tune-plot.svg"); + write_step_svg(&path, &trace, &scenario).unwrap(); + let text = std::fs::read_to_string(&path).unwrap(); + assert!(text.starts_with(" 1000, "svg looks empty"); + let _ = std::fs::remove_file(&path); + } +} diff --git a/crates/chassis-tune/src/preset.rs b/crates/chassis-tune/src/preset.rs new file mode 100644 index 0000000..73f6104 --- /dev/null +++ b/crates/chassis-tune/src/preset.rs @@ -0,0 +1,1305 @@ +//! 整定**超参数**预设：把"搜什么、怎么打分、什么算达标"从 CLI 默认值提升为 +//! 版本化、可评审、可复现的 TOML 产物。 +//! +//! # 为什么需要它 +//! +//! 一次整定的结果不只取决于被控对象与种子，还取决于一组**超参数**： +//! +//! | 组 | 内容 | 影响 | +//! | --- | --- | --- | +//! | `space` | 四轴区间与刻度 | 能不能搜到解（解在区间外 = 白搜） | +//! | `scenario` | 设定值曲线 / 目标转速 / 节拍 | 评的是"哪种工况下的好" | +//! | `cost` | 调节时间 / 超调 / IAE / 电流权重 | 什么叫"好" | +//! | `acceptance` | 超调与稳态误差阈值 | 什么算"通过"（退出码） | +//! | `search` | 种子 / 样本数 / top-k / 求解器 / 预算 | 搜得多细、能否复现 | +//! | `sim` | 被控对象与域随机化幅度 | 鲁棒优化对抗的是哪种不确定性 | +//! | `bench` | 真机安全包络与试验节奏 | 台架会不会飞车、跑多久 | +//! +//! 把这些散落在命令行默认值里，等于"实验条件"没有进版本管理：换台机器、 +//! 隔一周再跑，没人能说清当时那组数是怎么来的。本模块把它们变成 +//! `config/tune.toml` 里的具名预设，一次评审、长期复用。 +//! +//! # 优先级 +//! +//! ```text +//! 内置默认 < 预设文件里的预设 < 显式命令行参数 +//! ``` +//! +//! 逐字段覆盖：预设只写它关心的项，其余沿用内置默认；命令行只写本次要变的项。 +//! 因此"跑一次预设 + 临时改一个权重"是安全的，不会把其它超参数打回硬编码值。 +//! +//! # 不在这里的东西 +//! +//! `current_limit`、`direction`、`gear_ratio`、电调 ID、串口 / 接口名属于 +//! **安全与机械约束**，由 `chassis.toml`（`chassis-config`）给出，不参与搜索， +//! 也不允许被预设覆盖——否则预设文件就成了绕过限流的口子。 +//! +//! # 例 +//! +//! ```toml +//! version = 1 +//! default_preset = "sim_bench_model" +//! +//! [preset.bench_noload] +//! description = "空载台架：摩擦小、静差靠积分压制" +//! backend = "bench" +//! +//! [preset.bench_noload.space] +//! kp = { min = 0.5, max = 30.0, scale = "log" } +//! ki = { min = 0.1, max = 100.0, scale = "log0" } +//! +//! [preset.bench_noload.search] +//! samples = 24 +//! top_k = 3 +//! ``` + +use std::collections::BTreeMap; +use std::path::{Path, PathBuf}; + +use dji_esc_sim::{PlantParams, PlantTolerances, Scenario, SetpointProfile}; +use serde::{Deserialize, Serialize}; +use thiserror::Error; + +use crate::cost::Weights; +use crate::gains::{GainAxis, GainSpace, Scale}; +use crate::search::{SearchConfig, SolverKind}; + +/// 预设 schema 版本。schema 不兼容变更时递增。 +pub const PRESET_VERSION: u32 = 1; + +/// 未显式给 `--preset` 时尝试的默认预设路径。 +pub const DEFAULT_PRESET_PATH: &str = "config/tune.toml"; + +/// `log0` 轴取精确 0 的概率（与 `Scale::parse` 保持一致）。 +const LOG0_P_ZERO: f64 = 0.25; + +// ---------------------------------------------------------------- 枚举 + +/// 评估后端。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum BackendSpec { + /// 离线被控对象仿真。 + Sim, + /// 真机台架（会驱动电机）。 + Bench, +} + +impl BackendSpec { + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Sim => "sim", + Self::Bench => "bench", + } + } +} + +/// 增益轴的取值映射。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum ScaleSpec { + /// 线性均匀。 + Linear, + /// 对数均匀（要求 `min > 0`）。 + Log, + /// 以固定概率 0.25 取精确 0，否则对数均匀（与 `Scale::parse` 的 `log0` 一致）。 + Log0, +} + +impl ScaleSpec { + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Linear => "linear", + Self::Log => "log", + Self::Log0 => "log0", + } + } +} + +impl From for Scale { + fn from(value: ScaleSpec) -> Self { + match value { + ScaleSpec::Linear => Self::Linear, + ScaleSpec::Log => Self::Log, + ScaleSpec::Log0 => Self::LogOrZero { p_zero: LOG0_P_ZERO }, + } + } +} + +/// 设定值曲线。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum ProfileSpec { + /// 阶跃。 + Step, + /// 斜坡 + 保持。 + RampAndHold, + /// 阶梯。 + Staircase, + /// 升 → 保持 → 降 → 保持。 + UpDown, +} + +impl From for SetpointProfile { + fn from(value: ProfileSpec) -> Self { + match value { + ProfileSpec::Step => Self::Step, + ProfileSpec::RampAndHold => Self::RampAndHold, + ProfileSpec::Staircase => Self::Staircase { levels: 1 }, + ProfileSpec::UpDown => Self::UpDown, + } + } +} + +/// 局部精修求解器。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum SolverSpec { + /// Nelder-Mead（无导数）。 + NelderMead, + /// 粒子群。 + Pso, +} + +impl From for SolverKind { + fn from(value: SolverSpec) -> Self { + match value { + SolverSpec::NelderMead => Self::NelderMead, + SolverSpec::Pso => Self::Pso, + } + } +} + +impl SolverSpec { + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::NelderMead => "nelder-mead", + Self::Pso => "pso", + } + } +} + +/// 离线被控对象模型。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum PlantSpec { + /// 理想一阶模型（无摩擦、无延迟）。 + Gm3508, + /// 台架量级：静 / 库仑摩擦 + 2 拍延迟。 + Gm3508Bench, +} + +impl PlantSpec { + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Gm3508 => "gm3508", + Self::Gm3508Bench => "gm3508-bench", + } + } + + /// 展开为被控对象参数。 + #[must_use] + pub fn params(self) -> PlantParams { + match self { + Self::Gm3508 => PlantParams::gm3508(), + Self::Gm3508Bench => PlantParams::gm3508_bench(), + } + } +} + +// ---------------------------------------------------------------- spec 宏 + +/// 生成"逐项可选 + 逐项覆盖"的预设小节。 +/// +/// 每个字段都是 `Option`：`None` 表示"本层不表态"，由更低优先级来源决定。 +/// `overlay` 让 `other` 覆盖 `self` 的非空项。 +macro_rules! spec { + ( + $(#[$outer:meta])* + $name:ident { $( $(#[$field_doc:meta])* $field:ident : $ty:ty ),* $(,)? } + ) => { + $(#[$outer])* + #[derive(Debug, Clone, Default, PartialEq, Serialize, Deserialize)] + #[serde(deny_unknown_fields)] + pub struct $name { + $( + $(#[$field_doc])* + #[serde(default, skip_serializing_if = "Option::is_none")] + pub $field: Option<$ty>, + )* + } + + impl $name { + /// 以 `other` 的非空项覆盖自身（`other` 优先）。 + #[must_use] + pub fn overlay(&self, other: &Self) -> Self { + Self { + $($field: other.$field.clone().or_else(|| self.$field.clone()),)* + } + } + } + }; +} + +/// 单条增益轴：区间（必填）+ 刻度（可继承）。 +#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct RangeSpec { + /// 下界。 + pub min: f64, + /// 上界。 + pub max: f64, + /// 刻度；缺省沿用更低优先级来源（内置默认里四轴都有刻度）。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub scale: Option, +} + +impl RangeSpec { + /// 只给区间（刻度继承更低优先级来源）。 + #[must_use] + pub const fn new(min: f64, max: f64) -> Self { + Self { min, max, scale: None } + } + + /// 覆盖：区间整体替换，刻度缺省时继承。 + #[must_use] + pub fn overlay(&self, other: &Self) -> Self { + Self { min: other.min, max: other.max, scale: other.scale.or(self.scale) } + } +} + +/// 增益参数空间（四轴）。 +#[derive(Debug, Clone, Default, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct SpaceSpec { + /// 比例增益区间。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub kp: Option, + /// 积分增益区间。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub ki: Option, + /// 微分增益区间。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub kd: Option, + /// 速度前馈区间。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub feedforward: Option, +} + +/// 覆盖单条轴：区间整体替换，刻度继承。 +fn overlay_range(base: Option<&RangeSpec>, other: Option<&RangeSpec>) -> Option { + match (base, other) { + (Some(base), Some(other)) => Some(base.overlay(other)), + (None, Some(other)) => Some(*other), + (Some(base), None) => Some(*base), + (None, None) => None, + } +} + +impl SpaceSpec { + /// 以 `other` 的非空项覆盖自身（`other` 优先）。 + #[must_use] + pub fn overlay(&self, other: &Self) -> Self { + Self { + kp: overlay_range(self.kp.as_ref(), other.kp.as_ref()), + ki: overlay_range(self.ki.as_ref(), other.ki.as_ref()), + kd: overlay_range(self.kd.as_ref(), other.kd.as_ref()), + feedforward: overlay_range(self.feedforward.as_ref(), other.feedforward.as_ref()), + } + } +} + +spec! { + /// 场景。 + ScenarioSpec { + /// 设定值曲线。 + profile: ProfileSpec, + /// 目标转子转速（rpm）。 + target_rpm: f32, + /// 斜坡时间（毫秒）。 + ramp_ms: f32, + /// 保持时间（毫秒）。 + hold_ms: f32, + /// 阶梯级数（`profile = "staircase"`）。 + stair_levels: u32, + /// 控制 / 采样节拍（微秒）。 + dt_us: f32, + } +} + +spec! { + /// 成本权重（各项已归一化）。 + CostSpec { + /// 调节时间权重。 + settle: f64, + /// 超调权重。 + overshoot: f64, + /// IAE 权重。 + iae: f64, + /// 电流（effort）权重。 + effort: f64, + } +} + +spec! { + /// 验收阈值（决定退出码与报告结论）。 + AcceptanceSpec { + /// 允许的最大超调（%）。 + max_overshoot_pct: f32, + /// 允许的最大稳态误差（%）。 + max_ss_error_pct: f32, + } +} + +spec! { + /// 搜索预算与可复现性。 + SearchSpec { + /// 随机种子。 + seed: u64, + /// 粗扫样本数。 + samples: usize, + /// 进入精修的种子数。 + top_k: usize, + /// 精修求解器。 + solver: SolverSpec, + /// 每个起点的最大迭代数。 + max_iters: usize, + /// PSO 粒子数。 + particles: usize, + /// 评估预算上限。 + max_evals: u64, + /// 线程数（0 = rayon 默认；真机后端强制 1）。 + threads: usize, + } +} + +spec! { + /// 离线被控对象与域随机化。 + SimSpec { + /// 名义模型。 + plant: PlantSpec, + /// 集成成员数（1 = 只用名义模型）。 + ensemble: usize, + /// 鲁棒权重：0 = 只看均值，1 = 只看最坏成员。 + robust_weight: f64, + /// 增益抖动 ± 比例。 + gain_pct: f32, + /// 时间常数抖动 ± 比例。 + tau_pct: f32, + /// 命令延迟抖动上界（拍）。 + delay_ticks_max: u32, + /// 摩擦抖动 ± 比例。 + friction_pct: f32, + } +} + +spec! { + /// 真机安全包络与试验节奏。 + BenchSpec { + /// 过速保护上限（rpm）。 + max_rpm: f32, + /// 温度保护上限（℃）。 + max_temp_c: i8, + /// 单次试验超时（毫秒）。 + trial_timeout_ms: u64, + /// 试验之间的静默期（毫秒）。 + between_trials_ms: u64, + /// 启动时等待反馈的超时（毫秒）。 + startup_timeout_ms: u64, + /// 安全事件后是否继续搜索。 + continue_on_abort: bool, + /// 最优增益的复验次数。 + validate_runs: usize, + } +} + +// ---------------------------------------------------------------- 预设 + +/// 一组完整的整定超参数（各小节可缺省）。 +#[derive(Debug, Clone, Default, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct Preset { + /// 人类可读说明（报告与 `presets` 子命令展示）。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub description: Option, + /// 评估后端。 + #[serde(default, skip_serializing_if = "Option::is_none")] + pub backend: Option, + /// 增益参数空间。 + #[serde(default)] + pub space: SpaceSpec, + /// 场景。 + #[serde(default)] + pub scenario: ScenarioSpec, + /// 成本权重。 + #[serde(default)] + pub cost: CostSpec, + /// 验收阈值。 + #[serde(default)] + pub acceptance: AcceptanceSpec, + /// 搜索预算。 + #[serde(default)] + pub search: SearchSpec, + /// 离线被控对象。 + #[serde(default)] + pub sim: SimSpec, + /// 真机安全包络。 + #[serde(default)] + pub bench: BenchSpec, +} + +impl Preset { + /// 内置默认：与 `chassis-tune` 历史 CLI 默认值逐项一致。 + /// + /// 这是最低优先级来源，保证任何预设都只需写"与默认不同的部分"。 + #[must_use] + pub fn builtin() -> Self { + Self { + description: Some("内置默认（chassis-tune CLI 默认超参数）".into()), + backend: Some(BackendSpec::Sim), + space: SpaceSpec { + kp: Some(RangeSpec { min: 0.5, max: 30.0, scale: Some(ScaleSpec::Log) }), + ki: Some(RangeSpec { min: 0.1, max: 100.0, scale: Some(ScaleSpec::Log0) }), + kd: Some(RangeSpec { min: 0.01, max: 3.0, scale: Some(ScaleSpec::Log0) }), + feedforward: Some(RangeSpec { min: 0.01, max: 2.5, scale: Some(ScaleSpec::Log0) }), + }, + scenario: ScenarioSpec { + profile: Some(ProfileSpec::RampAndHold), + target_rpm: Some(1500.0), + ramp_ms: Some(200.0), + hold_ms: Some(1800.0), + stair_levels: Some(4), + dt_us: Some(1000.0), + }, + cost: CostSpec { + settle: Some(1.0), + overshoot: Some(1.0), + iae: Some(0.5), + effort: Some(0.1), + }, + acceptance: AcceptanceSpec { + max_overshoot_pct: Some(20.0), + max_ss_error_pct: Some(5.0), + }, + search: SearchSpec { + seed: Some(20_260_101), + samples: Some(512), + top_k: Some(8), + solver: Some(SolverSpec::NelderMead), + max_iters: Some(200), + particles: Some(24), + max_evals: Some(4_000), + threads: Some(0), + }, + sim: SimSpec { + plant: Some(PlantSpec::Gm3508Bench), + ensemble: Some(8), + robust_weight: Some(0.5), + gain_pct: Some(0.2), + tau_pct: Some(0.3), + delay_ticks_max: Some(3), + friction_pct: Some(0.5), + }, + bench: BenchSpec { + max_rpm: Some(3000.0), + max_temp_c: Some(70), + trial_timeout_ms: Some(6_000), + between_trials_ms: Some(500), + startup_timeout_ms: Some(5_000), + continue_on_abort: Some(false), + validate_runs: Some(3), + }, + } + } + + /// 以 `other` 的非空项覆盖自身（`other` 优先）。 + #[must_use] + pub fn overlay(&self, other: &Self) -> Self { + Self { + description: other.description.clone().or_else(|| self.description.clone()), + backend: other.backend.or(self.backend), + space: self.space.overlay(&other.space), + scenario: self.scenario.overlay(&other.scenario), + cost: self.cost.overlay(&other.cost), + acceptance: self.acceptance.overlay(&other.acceptance), + search: self.search.overlay(&other.search), + sim: self.sim.overlay(&other.sim), + bench: self.bench.overlay(&other.bench), + } + } + + /// 展开为可执行的超参数（含全部校验）。 + /// + /// # Errors + /// + /// 缺项（[`PresetError::Missing`]）或区间 / 阈值 / 预算非法 + /// （[`PresetError::Invalid`]）时返回错误。 + pub fn to_tuning(&self) -> Result { + let plant_spec = self.sim.plant.unwrap_or(PlantSpec::Gm3508Bench); + Ok(Tuning { + backend: self.backend.unwrap_or(BackendSpec::Sim), + space: self.gain_space()?, + scenario: self.scenario()?, + weights: self.weights()?, + acceptance: self.acceptance()?, + search: self.search()?, + plant_spec, + plant: plant_spec.params(), + tolerances: self.tolerances()?, + ensemble: self.ensemble()?, + robust_weight: self.robust_weight()?, + threads: self.search.threads.unwrap_or(0), + }) + } + + /// 由 `--kp` 这类 `MIN..MAX` 文本构造覆盖项。 + #[must_use] + pub fn range(min: f64, max: f64) -> RangeSpec { + RangeSpec { min, max, scale: None } + } + + fn gain_space(&self) -> Result { + let space = GainSpace::new( + gain_axis("kp", self.space.kp.as_ref())?, + gain_axis("ki", self.space.ki.as_ref())?, + gain_axis("kd", self.space.kd.as_ref())?, + gain_axis("feedforward", self.space.feedforward.as_ref())?, + ); + space + .validate() + .map_err(|error| PresetError::Invalid { field: "space", reason: error.to_string() })?; + Ok(space) + } + + fn scenario(&self) -> Result { + let spec = &self.scenario; + let dt_us = spec.dt_us.ok_or(PresetError::Missing { field: "scenario.dt_us" })?; + if !(dt_us.is_finite() && dt_us > 0.0) { + return Err(PresetError::Invalid { + field: "scenario.dt_us", + reason: format!("必须为正有限值，得到 {dt_us}"), + }); + } + let target_rpm = + spec.target_rpm.ok_or(PresetError::Missing { field: "scenario.target_rpm" })?; + if !target_rpm.is_finite() { + return Err(PresetError::Invalid { + field: "scenario.target_rpm", + reason: format!("必须有限，得到 {target_rpm}"), + }); + } + let profile = match spec.profile.unwrap_or(ProfileSpec::RampAndHold) { + ProfileSpec::Staircase => { + let levels = spec + .stair_levels + .ok_or(PresetError::Missing { field: "scenario.stair_levels" })?; + if levels == 0 { + return Err(PresetError::Invalid { + field: "scenario.stair_levels", + reason: "阶梯级数必须 ≥ 1".into(), + }); + } + SetpointProfile::Staircase { levels } + } + other => other.into(), + }; + let scenario = Scenario { + dt_s: dt_us / 1_000_000.0, + ramp_s: spec.ramp_ms.ok_or(PresetError::Missing { field: "scenario.ramp_ms" })? + / 1000.0, + hold_s: spec.hold_ms.ok_or(PresetError::Missing { field: "scenario.hold_ms" })? + / 1000.0, + target_rpm, + profile, + }; + if !(scenario.total_s().is_finite() && scenario.total_s() > 0.0) { + return Err(PresetError::Invalid { + field: "scenario", + reason: "总时长必须为正（检查 ramp_ms / hold_ms）".into(), + }); + } + if !(scenario.ramp_s >= 0.0 && scenario.hold_s >= 0.0) { + return Err(PresetError::Invalid { + field: "scenario", + reason: "ramp_ms / hold_ms 不得为负".into(), + }); + } + Ok(scenario) + } + + fn weights(&self) -> Result { + let spec = &self.cost; + let value = |field: &'static str, v: Option| -> Result { + v.ok_or(PresetError::Missing { field }) + }; + let weights = Weights { + settle: value("cost.settle", spec.settle)?, + overshoot: value("cost.overshoot", spec.overshoot)?, + iae: value("cost.iae", spec.iae)?, + effort: value("cost.effort", spec.effort)?, + }; + if !weights.is_valid() { + return Err(PresetError::Invalid { + field: "cost", + reason: "权重必须是非负有限值".into(), + }); + } + Ok(weights) + } + + fn acceptance(&self) -> Result { + let spec = &self.acceptance; + let max_overshoot_pct = spec + .max_overshoot_pct + .ok_or(PresetError::Missing { field: "acceptance.max_overshoot_pct" })?; + let max_ss_error_pct = spec + .max_ss_error_pct + .ok_or(PresetError::Missing { field: "acceptance.max_ss_error_pct" })?; + for (field, value) in [ + ("acceptance.max_overshoot_pct", max_overshoot_pct), + ("acceptance.max_ss_error_pct", max_ss_error_pct), + ] { + if !(value.is_finite() && value >= 0.0) { + return Err(PresetError::Invalid { + field, + reason: format!("必须是非负有限值（%），得到 {value}"), + }); + } + } + Ok(Acceptance { max_overshoot_pct, max_ss_error_pct }) + } + + fn search(&self) -> Result { + let spec = &self.search; + let config = SearchConfig { + seed: spec.seed.ok_or(PresetError::Missing { field: "search.seed" })?, + samples: spec.samples.ok_or(PresetError::Missing { field: "search.samples" })?, + top_k: spec.top_k.ok_or(PresetError::Missing { field: "search.top_k" })?, + solver: SolverKind::from( + spec.solver.ok_or(PresetError::Missing { field: "search.solver" })?, + ), + max_iters: spec.max_iters.ok_or(PresetError::Missing { field: "search.max_iters" })?, + particles: spec.particles.ok_or(PresetError::Missing { field: "search.particles" })?, + max_evals: spec.max_evals.ok_or(PresetError::Missing { field: "search.max_evals" })?, + }; + config + .validate() + .map_err(|error| PresetError::Invalid { field: "search", reason: error.to_string() })?; + Ok(config) + } + + fn tolerances(&self) -> Result { + let spec = &self.sim; + let tolerances = PlantTolerances { + gain_pct: spec.gain_pct.ok_or(PresetError::Missing { field: "sim.gain_pct" })?, + tau_pct: spec.tau_pct.ok_or(PresetError::Missing { field: "sim.tau_pct" })?, + delay_ticks_max: spec + .delay_ticks_max + .ok_or(PresetError::Missing { field: "sim.delay_ticks_max" })?, + friction_pct: spec + .friction_pct + .ok_or(PresetError::Missing { field: "sim.friction_pct" })?, + }; + for (field, value) in [ + ("sim.gain_pct", tolerances.gain_pct), + ("sim.tau_pct", tolerances.tau_pct), + ("sim.friction_pct", tolerances.friction_pct), + ] { + if !(value.is_finite() && value >= 0.0) { + return Err(PresetError::Invalid { + field, + reason: format!("必须是 ± 比例（非负有限值），得到 {value}"), + }); + } + } + Ok(tolerances) + } + + fn ensemble(&self) -> Result { + let ensemble = self.sim.ensemble.ok_or(PresetError::Missing { field: "sim.ensemble" })?; + if ensemble == 0 { + return Err(PresetError::Invalid { + field: "sim.ensemble", + reason: "集成成员数必须 ≥ 1（1 = 只用名义模型）".into(), + }); + } + Ok(ensemble) + } + + fn robust_weight(&self) -> Result { + let weight = + self.sim.robust_weight.ok_or(PresetError::Missing { field: "sim.robust_weight" })?; + if !(weight.is_finite() && (0.0..=1.0).contains(&weight)) { + return Err(PresetError::Invalid { + field: "sim.robust_weight", + reason: format!("必须在 0..=1 之间，得到 {weight}"), + }); + } + Ok(weight) + } + + /// 真机安全包络参数（`--backend bench` 时才被使用）。 + /// + /// # Errors + /// + /// 缺项或数值明显不安全（非正过速上限 / 试验超时等）时返回错误。 + pub fn bench_envelope(&self) -> Result { + let spec = &self.bench; + let envelope = BenchEnvelope { + max_rpm: spec.max_rpm.ok_or(PresetError::Missing { field: "bench.max_rpm" })?, + max_temp_c: spec + .max_temp_c + .ok_or(PresetError::Missing { field: "bench.max_temp_c" })?, + trial_timeout_ms: spec + .trial_timeout_ms + .ok_or(PresetError::Missing { field: "bench.trial_timeout_ms" })?, + between_trials_ms: spec + .between_trials_ms + .ok_or(PresetError::Missing { field: "bench.between_trials_ms" })?, + startup_timeout_ms: spec + .startup_timeout_ms + .ok_or(PresetError::Missing { field: "bench.startup_timeout_ms" })?, + continue_on_abort: spec.continue_on_abort.unwrap_or(false), + validate_runs: spec + .validate_runs + .ok_or(PresetError::Missing { field: "bench.validate_runs" })?, + }; + if !(envelope.max_rpm.is_finite() && envelope.max_rpm > 0.0) { + return Err(PresetError::Invalid { + field: "bench.max_rpm", + reason: format!("过速上限必须为正有限值，得到 {}", envelope.max_rpm), + }); + } + if envelope.trial_timeout_ms == 0 { + return Err(PresetError::Invalid { + field: "bench.trial_timeout_ms", + reason: "试验超时必须 > 0（0 会让试验立刻被判超时）".into(), + }); + } + if envelope.startup_timeout_ms == 0 { + return Err(PresetError::Invalid { + field: "bench.startup_timeout_ms", + reason: "启动等待超时必须 > 0".into(), + }); + } + if envelope.validate_runs == 0 { + return Err(PresetError::Invalid { + field: "bench.validate_runs", + reason: "复验次数必须 ≥ 1".into(), + }); + } + Ok(envelope) + } +} + +/// 把一条轴定义展开并校验为 [`GainAxis`]。 +/// +/// # Errors +/// +/// 轴缺失、缺少刻度、区间非法（`min > max`、对数轴下界 ≤ 0）时返回错误。 +fn gain_axis(name: &'static str, spec: Option<&RangeSpec>) -> Result { + let spec = spec.ok_or(PresetError::Missing { field: name })?; + let scale = match spec.scale { + Some(scale) => Scale::from(scale), + None => { + return Err(PresetError::Invalid { + field: name, + reason: "缺少 scale（应为 linear / log / log0）".into(), + }); + } + }; + let axis = GainAxis::new(name, spec.min, spec.max, scale); + axis.validate() + .map_err(|error| PresetError::Invalid { field: name, reason: error.to_string() })?; + Ok(axis) +} + +/// 解算后的验收阈值。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Acceptance { + /// 允许的最大超调（%）。 + pub max_overshoot_pct: f32, + /// 允许的最大稳态误差（%）。 + pub max_ss_error_pct: f32, +} + +/// 解算后的真机安全包络参数。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct BenchEnvelope { + /// 过速保护上限（rpm）。 + pub max_rpm: f32, + /// 温度保护上限（℃）。 + pub max_temp_c: i8, + /// 单次试验超时（毫秒）。 + pub trial_timeout_ms: u64, + /// 试验之间的静默期（毫秒）。 + pub between_trials_ms: u64, + /// 启动时等待反馈的超时（毫秒）。 + pub startup_timeout_ms: u64, + /// 安全事件后是否继续搜索。 + pub continue_on_abort: bool, + /// 复验次数。 + pub validate_runs: usize, +} + +/// 解算后的一组超参数：可直接喂给搜索与后端。 +#[derive(Debug, Clone, PartialEq)] +pub struct Tuning { + /// 评估后端。 + pub backend: BackendSpec, + /// 增益参数空间。 + pub space: GainSpace, + /// 场景。 + pub scenario: Scenario, + /// 成本权重。 + pub weights: Weights, + /// 验收阈值。 + pub acceptance: Acceptance, + /// 搜索配置。 + pub search: SearchConfig, + /// 名义被控对象（枚举，供显示 / 复现）。 + pub plant_spec: PlantSpec, + /// 名义被控对象（展开后的参数，供仿真重放）。 + pub plant: PlantParams, + /// 域随机化幅度。 + pub tolerances: PlantTolerances, + /// 集成成员数。 + pub ensemble: usize, + /// 鲁棒权重。 + pub robust_weight: f64, + /// 线程数（0 = 自动）。 + pub threads: usize, +} + +// ---------------------------------------------------------------- 文件 + +/// 预设文件：版本 + 默认预设名 + 具名预设集合。 +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct PresetFile { + /// schema 版本，必须等于 [`PRESET_VERSION`]。 + pub version: u32, + /// 未显式指定名字时使用的预设（别名 `default`）。 + #[serde(default, alias = "default", skip_serializing_if = "Option::is_none")] + pub default_preset: Option, + /// 具名预设。 + #[serde(default)] + pub preset: BTreeMap, +} + +impl PresetFile { + /// 内置库：只含 `default`（等价于 [`Preset::builtin`]）。 + /// + /// 具名工况预设放在 `config/tune.toml` 里评审，从而避免"同一组数字在 + /// Rust 与 TOML 里各写一份"的漂移。 + #[must_use] + pub fn builtin() -> Self { + let mut preset = BTreeMap::new(); + preset.insert("default".to_owned(), Preset::builtin()); + Self { version: PRESET_VERSION, default_preset: Some("default".to_owned()), preset } + } + + /// 从文件读取并校验。 + /// + /// # Errors + /// + /// 读取失败（[`PresetError::Io`]）、TOML / schema 非法 + /// （[`PresetError::Load`]）或版本不支持时返回错误。 + pub fn load(path: &Path) -> Result { + let text = std::fs::read_to_string(path) + .map_err(|source| PresetError::Io { path: path.to_path_buf(), source })?; + let file: Self = toml::from_str(&text) + .map_err(|source| PresetError::Load { path: path.to_path_buf(), source })?; + file.validate()?; + Ok(file) + } + + /// 从文本解析并校验（测试与内存场景用）。 + /// + /// # Errors + /// + /// TOML / schema 非法或版本不支持时返回错误。 + pub fn parse_str(text: &str) -> Result { + let file: Self = toml::from_str(text).map_err(|source| PresetError::Parse { source })?; + file.validate()?; + Ok(file) + } + + /// 校验版本与内容。 + /// + /// # Errors + /// + /// 版本不等于 [`PRESET_VERSION`] 或没有任何预设时返回错误。 + pub fn validate(&self) -> Result<(), PresetError> { + if self.version != PRESET_VERSION { + return Err(PresetError::UnsupportedVersion { + found: self.version, + expected: PRESET_VERSION, + }); + } + if self.preset.is_empty() { + return Err(PresetError::Empty); + } + Ok(()) + } + + /// 预设名列表（字典序）。 + #[must_use] + pub fn names(&self) -> Vec { + self.preset.keys().cloned().collect() + } + + /// 选择一个预设：显式名字 > `default_preset` > 唯一预设。 + /// + /// # Errors + /// + /// 文件为空、名字不存在、或存在多个预设却没有 `default_preset` 时返回错误。 + pub fn select(&self, name: Option<&str>) -> Result<&Preset, PresetError> { + self.select_named(name).map(|(_, preset)| preset) + } + + /// 同 [`Self::select`]，但把**生效名**一并返回（报告里要写清"这批数出自哪个预设"）。 + /// + /// # Errors + /// + /// 同 [`Self::select`]。 + pub fn select_named(&self, name: Option<&str>) -> Result<(&str, &Preset), PresetError> { + if self.preset.is_empty() { + return Err(PresetError::Empty); + } + let key = match name { + Some(name) => name, + None => match self.default_preset.as_deref() { + Some(default) => default, + None if self.preset.len() == 1 => { + self.preset.keys().next().map(String::as_str).unwrap_or_default() + } + None => return Err(PresetError::NoDefault { available: self.names() }), + }, + }; + self.preset.get_key_value(key).map_or_else( + || Err(PresetError::UnknownPreset { name: key.to_owned(), available: self.names() }), + |(stored, preset)| Ok((stored.as_str(), preset)), + ) + } +} + +// ---------------------------------------------------------------- 错误 + +/// 预设加载 / 解算错误。 +#[derive(Debug, Error)] +pub enum PresetError { + /// 文件读取失败。 + #[error("读取预设文件 {path} 失败：{source}")] + Io { + /// 路径。 + path: PathBuf, + /// 底层 IO 错误。 + #[source] + source: std::io::Error, + }, + /// 文件解析失败（带路径）。 + #[error("解析预设文件 {path} 失败：{source}")] + Load { + /// 路径。 + path: PathBuf, + /// TOML 错误。 + #[source] + source: toml::de::Error, + }, + /// 文本解析失败。 + #[error("解析预设 TOML 失败：{source}")] + Parse { + /// TOML 错误。 + #[source] + source: toml::de::Error, + }, + /// schema 版本不支持。 + #[error("预设文件 version = {found} 不受支持（本工具支持 version = {expected}）")] + UnsupportedVersion { + /// 文件里的版本。 + found: u32, + /// 支持的版本。 + expected: u32, + }, + /// 没有任何预设。 + #[error("预设文件里没有任何预设")] + Empty, + /// 名字不存在。 + #[error("预设 '{name}' 不存在（可用：{}）", available.join(", "))] + UnknownPreset { + /// 请求的名字。 + name: String, + /// 可选名字。 + available: Vec, + }, + /// 多个预设但没有默认名。 + #[error("预设文件有多个预设但未指定 default_preset（请用 --preset-name 指定，可选：{}）", available.join(", "))] + NoDefault { + /// 可选名字。 + available: Vec, + }, + /// 缺少必需项。 + #[error("超参数 '{field}' 缺失（预设与内置默认都没有提供）")] + Missing { + /// 项名。 + field: &'static str, + }, + /// 项值非法。 + #[error("超参数 '{field}' 非法：{reason}")] + Invalid { + /// 项名。 + field: &'static str, + /// 原因。 + reason: String, + }, +} + +#[cfg(test)] +mod tests { + use super::*; + + const MINIMAL: &str = r#" +version = 1 +default_preset = "fast" + +[preset.fast] +description = "小样本快跑" + +[preset.fast.search] +samples = 32 +top_k = 3 +"#; + + #[test] + fn builtin_satisfies_its_own_schema() { + let text = toml::to_string(&PresetFile::builtin()).unwrap(); + let round_trip = PresetFile::parse_str(&text).unwrap(); + assert_eq!(round_trip, PresetFile::builtin()); + assert!(round_trip.select(None).unwrap().to_tuning().is_ok()); + } + + #[test] + fn builtin_matches_the_historical_cli_defaults() { + // 这些数字是 chassis-tune 历史的 CLI 默认值，改动等于改变整定的默认行为， + // 必须是有意为之。 + let tuning = Preset::builtin().to_tuning().unwrap(); + assert_eq!(tuning.backend, BackendSpec::Sim); + assert!((tuning.space.kp.min - 0.5).abs() < 1e-12); + assert!((tuning.space.kp.max - 30.0).abs() < 1e-12); + assert_eq!(tuning.space.kp.scale, Scale::Log); + assert_eq!(tuning.space.ki.scale, Scale::LogOrZero { p_zero: LOG0_P_ZERO }); + assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6); + assert!((tuning.scenario.dt_s - 0.001).abs() < 1e-9); + assert_eq!(tuning.weights, Weights::default()); + assert!((tuning.acceptance.max_overshoot_pct - 20.0).abs() < 1e-6); + assert_eq!(tuning.search, SearchConfig::default()); + assert_eq!(tuning.ensemble, 8); + assert!((tuning.robust_weight - 0.5).abs() < 1e-12); + assert_eq!(tuning.tolerances, PlantTolerances::default()); + let envelope = Preset::builtin().bench_envelope().unwrap(); + assert!((envelope.max_rpm - 3000.0).abs() < 1e-6); + assert_eq!(envelope.validate_runs, 3); + } + + #[test] + fn preset_overlays_only_the_fields_it_sets() { + let file = PresetFile::parse_str(MINIMAL).unwrap(); + let merged = Preset::builtin().overlay(file.select(None).unwrap()); + let tuning = merged.to_tuning().unwrap(); + assert_eq!(tuning.search.samples, 32, "预设必须生效"); + assert_eq!(tuning.search.top_k, 3); + // 未提及的项沿用内置默认。 + assert_eq!(tuning.search.seed, SearchConfig::default().seed); + assert_eq!(tuning.search.max_evals, SearchConfig::default().max_evals); + assert!((tuning.space.kp.max - 30.0).abs() < 1e-12); + assert_eq!(merged.description.as_deref(), Some("小样本快跑")); + } + + #[test] + fn range_overlay_keeps_the_inherited_scale() { + let mut overlay = Preset::default(); + overlay.space.kp = Some(RangeSpec::new(1.0, 5.0)); + let merged = Preset::builtin().overlay(&overlay); + let tuning = merged.to_tuning().unwrap(); + assert!((tuning.space.kp.min - 1.0).abs() < 1e-12); + assert!((tuning.space.kp.max - 5.0).abs() < 1e-12); + assert_eq!(tuning.space.kp.scale, Scale::Log, "刻度必须从内置默认继承"); + } + + #[test] + fn overlay_does_not_mutate_the_base() { + let base = Preset::builtin(); + let mut overlay = Preset::default(); + overlay.search.samples = Some(1); + let _ = base.overlay(&overlay); + assert_eq!(base.search.samples, Some(512), "覆盖必须是纯函数"); + } + + #[test] + fn select_prefers_explicit_name_then_default_then_single() { + let file = PresetFile::parse_str(MINIMAL).unwrap(); + assert!(file.select(Some("fast")).is_ok()); + assert!(file.select(None).is_ok(), "default_preset 必须生效"); + + let single = PresetFile::parse_str( + "version = 1\n[preset.only]\n[preset.only.search]\nsamples = 2\n", + ) + .unwrap(); + assert!(single.select(None).is_ok(), "唯一预设无需 default_preset"); + + let many = PresetFile::parse_str("version = 1\n[preset.a]\n[preset.b]\n").unwrap(); + assert!(matches!(many.select(None), Err(PresetError::NoDefault { .. }))); + } + + #[test] + fn unknown_preset_lists_the_available_names() { + let file = PresetFile::parse_str(MINIMAL).unwrap(); + let error = file.select(Some("nope")).unwrap_err(); + let text = error.to_string(); + assert!(text.contains("nope"), "{text}"); + assert!(text.contains("fast"), "错误信息必须列出可用名：{text}"); + } + + #[test] + fn unsupported_version_is_rejected() { + let text = "version = 99\n[preset.a]\n"; + assert!(matches!( + PresetFile::parse_str(text), + Err(PresetError::UnsupportedVersion { found: 99, expected: PRESET_VERSION }) + )); + } + + #[test] + fn empty_and_unknown_keys_are_rejected() { + assert!(matches!(PresetFile::parse_str("version = 1\n"), Err(PresetError::Empty))); + // 拼错小节名必须报错，而不是被静默忽略（否则"以为改了其实没改"）。 + let typo = "[preset.a.space]\nkpp = { min = 1.0, max = 2.0 }\n"; + assert!(matches!(PresetFile::parse_str(typo), Err(PresetError::Parse { .. }))); + let typo2 = "version = 1\n[preset.a.searc]\nsamples = 1\n"; + assert!(matches!(PresetFile::parse_str(typo2), Err(PresetError::Parse { .. }))); + } + + #[test] + fn load_reports_the_path_for_missing_files() { + let path = std::path::Path::new("/definitely/not/here/tune.toml"); + let error = PresetFile::load(path).unwrap_err(); + assert!(matches!(error, PresetError::Io { .. })); + assert!(error.to_string().contains("tune.toml"), "{error}"); + } + + #[test] + fn invalid_values_are_rejected_with_the_offending_key() { + // 在**完整**预设（内置默认 + 覆盖）上校验：预设只写要改的项。 + let build = |body: &str| -> Result { + let text = format!("version = 1\n[preset.a]\n{body}"); + let file = PresetFile::parse_str(&text)?; + Preset::builtin().overlay(file.select(None)?).to_tuning() + }; + + let error = build("[preset.a.space]\nkp = { min = -1.0, max = 5.0, scale = \"log\" }\n") + .unwrap_err(); + assert!(matches!(&error, PresetError::Invalid { field: "kp", .. }), "{error}"); + + // 负权重。 + let error = build("[preset.a.cost]\nsettle = -1.0\n").unwrap_err(); + assert!(matches!(&error, PresetError::Invalid { field: "cost", .. }), "{error}"); + + // 零样本（唯一被 `SearchConfig::validate` 拒绝的搜索配置）。 + let error = build("[preset.a.search]\nsamples = 0\n").unwrap_err(); + assert!(matches!(&error, PresetError::Invalid { field: "search", .. }), "{error}"); + + // 预算小于样本数**不是**错误：sweep 会把预算下限抬到 samples。 + assert!(build("[preset.a.search]\nsamples = 100\nmax_evals = 10\n").is_ok()); + + // 鲁棒权重越界。 + let error = build("[preset.a.sim]\nrobust_weight = 1.5\n").unwrap_err(); + assert!( + matches!(&error, PresetError::Invalid { field: "sim.robust_weight", .. }), + "{error}" + ); + + // 退化场景。 + let error = + build("[preset.a.scenario]\nhold_ms = 0.0\nramp_ms = 0.0\nprofile = \"step\"\n") + .unwrap_err(); + assert!(matches!(&error, PresetError::Invalid { field: "scenario", .. }), "{error}"); + + // 缺 scale 的轴（内置换行默认提供了刻度，这里显式抹掉）。 + let mut preset = Preset::builtin(); + preset.space.kd = Some(RangeSpec { min: 0.1, max: 1.0, scale: None }); + let error = preset.to_tuning().unwrap_err(); + assert!(matches!(&error, PresetError::Invalid { field: "kd", .. }), "{error}"); + } + + #[test] + fn missing_keys_are_reported_for_a_bare_preset() { + let error = Preset::default().to_tuning().unwrap_err(); + assert!(matches!(error, PresetError::Missing { .. }), "{error}"); + } + + #[test] + fn staircase_needs_levels() { + let mut preset = Preset::builtin(); + preset.scenario.profile = Some(ProfileSpec::Staircase); + preset.scenario.stair_levels = None; + assert!(matches!( + preset.to_tuning(), + Err(PresetError::Missing { field: "scenario.stair_levels" }) + )); + preset.scenario.stair_levels = Some(0); + assert!(matches!( + preset.to_tuning(), + Err(PresetError::Invalid { field: "scenario.stair_levels", .. }) + )); + preset.scenario.stair_levels = Some(5); + assert!(preset.to_tuning().is_ok()); + } + + #[test] + fn bench_envelope_rejects_unsafe_values() { + let mut preset = Preset::builtin(); + preset.bench.max_rpm = Some(0.0); + assert!(matches!( + preset.bench_envelope(), + Err(PresetError::Invalid { field: "bench.max_rpm", .. }) + )); + preset = Preset::builtin(); + preset.bench.trial_timeout_ms = Some(0); + assert!(preset.bench_envelope().is_err()); + preset = Preset::builtin(); + preset.bench.validate_runs = Some(0); + assert!(preset.bench_envelope().is_err()); + } + + #[test] + fn spec_enums_round_trip_through_toml_text() { + #[derive(Deserialize)] + struct Wrapper { + value: T, + } + + let parse = |text: &str| -> Wrapper { + toml::from_str(&format!("value = \"{text}\"")).unwrap() + }; + for (spec, text) in + [(ScaleSpec::Linear, "linear"), (ScaleSpec::Log, "log"), (ScaleSpec::Log0, "log0")] + { + assert_eq!(spec.name(), text); + assert_eq!(parse(text).value, spec); + } + assert_eq!(PlantSpec::Gm3508Bench.name(), "gm3508-bench"); + assert_eq!(SolverSpec::NelderMead.name(), "nelder-mead"); + assert_eq!(BackendSpec::Bench.name(), "bench"); + let profile: Wrapper = toml::from_str("value = \"ramp-and-hold\"").unwrap(); + assert_eq!(profile.value, ProfileSpec::RampAndHold); + } +} diff --git a/crates/chassis-tune/src/report.rs b/crates/chassis-tune/src/report.rs new file mode 100644 index 0000000..f99f12b --- /dev/null +++ b/crates/chassis-tune/src/report.rs @@ -0,0 +1,713 @@ +//! 报告与产物写出：CSV / TOML / JSON / Markdown（可选 SVG）。 +//! +//! 产物布局（`--out-dir`）： +//! +//! | 文件 | 内容 | +//! | --- | --- | +//! | `trials.csv` | 全部候选：增益 + 指标 + 成本 + 判定（可用任意表格软件复盘） | +//! | `best.toml` | 可直接粘贴进 `chassis.toml` 的 `[[motor]]` 片段 | +//! | `best-trace.csv` | 最优增益的完整步响应轨迹 | +//! | `report.md` | 人类可读总结：设置、Top-N、结论与注意事项 | +//! | `summary.json` | 机器可读结果（CI / 后续工具消费） | +//! | `best.svg` | 步响应三联图（仅 `--features plot`） | + +use std::fmt::Write as _; +use std::path::{Path, PathBuf}; + +use dji_esc_sim::{LoopLimits, Scenario, Trace}; +use serde::Serialize; + +use crate::gains::GainSpace; +use crate::metrics::StepMetrics; +use crate::search::{Ranked, SearchConfig, Trial, verdict_hint}; + +/// 报告写出错误。 +#[derive(Debug, thiserror::Error)] +pub enum ReportError { + /// 目录创建或文件写入失败。 + #[error("io error at {path}: {source}")] + Io { + /// 出错路径。 + path: PathBuf, + /// 底层错误。 + source: std::io::Error, + }, + /// CSV 写出失败。 + #[error("csv error: {0}")] + Csv(#[from] csv::Error), + /// JSON 写出失败。 + #[error("json error: {0}")] + Json(#[from] serde_json::Error), + /// TOML 写出失败。 + #[error("toml error: {0}")] + Toml(#[from] toml::ser::Error), + /// 没有可用的最优结果。 + #[error("no feasible trial to report")] + NoBest, + /// SVG 绘图失败（仅 `--features plot`）。 + #[error("plot error: {0}")] + Plot(String), +} + +fn io(path: impl Into, source: std::io::Error) -> ReportError { + ReportError::Io { path: path.into(), source } +} + +/// 生成产物所需的上下文。 +#[derive(Debug, Clone)] +pub struct ReportContext { + /// 输出目录。 + pub out_dir: PathBuf, + /// 本次搜索使用的参数空间。 + pub space: GainSpace, + /// 搜索配置。 + pub config: SearchConfig, + /// 场景。 + pub scenario: Scenario, + /// 限幅。 + pub limits: LoopLimits, + /// 后端描述（`sim(...)` / `bench(...)`）。 + pub backend: String, + /// 目标轮名（写回 TOML 用）。 + pub motor_name: String, + /// 验收阈值：最大超调（%）。 + pub max_overshoot_pct: f32, + /// 验收阈值：最大稳态误差（%）。 + pub max_ss_error_pct: f32, + /// 最优增益的轨迹（有则写 `best-trace.csv`）。 + pub best_trace: Option, +} + +/// 写出全部产物，返回写出的文件列表。 +/// +/// # Errors +/// +/// 目录创建、文件写入或序列化失败时返回 [`ReportError`]。 +pub fn write_reports(ctx: &ReportContext, ranked: &Ranked) -> Result, ReportError> { + std::fs::create_dir_all(&ctx.out_dir).map_err(|e| io(&ctx.out_dir, e))?; + let mut written = Vec::new(); + + let trials_path = ctx.out_dir.join("trials.csv"); + write_trials_csv(&trials_path, &ranked.trials)?; + written.push(trials_path); + + let best_path = ctx.out_dir.join("best.toml"); + write_best_toml(&best_path, ctx, &ranked.best)?; + written.push(best_path); + + if let Some(trace) = &ctx.best_trace { + let trace_path = ctx.out_dir.join("best-trace.csv"); + write_trace_csv(&trace_path, trace)?; + written.push(trace_path); + } + + let report_path = ctx.out_dir.join("report.md"); + write_markdown(&report_path, ctx, ranked)?; + written.push(report_path); + + let summary_path = ctx.out_dir.join("summary.json"); + write_summary(&summary_path, ctx, ranked)?; + written.push(summary_path); + + #[cfg(feature = "plot")] + if let Some(trace) = &ctx.best_trace { + let svg_path = ctx.out_dir.join("best.svg"); + crate::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?; + written.push(svg_path); + } + + Ok(written) +} + +/// `trials.csv` 的一行。 +#[derive(Debug, Serialize)] +struct TrialRow { + index: u64, + phase: &'static str, + kp: f32, + ki: f32, + kd: f32, + feedforward: f32, + cost: f64, + verdict: &'static str, + settled: bool, + rise_time_s: f32, + overshoot_pct: f32, + settling_time_s: f32, + ss_error_pct: f32, + steady_rpm: f32, + iae: f32, + peak_effort_raw: f32, + rms_effort_raw: f32, + saturated_pct: f32, + max_rpm: f32, +} + +impl TrialRow { + fn from_trial(trial: &Trial) -> Self { + let metrics = trial.score.metrics.unwrap_or_default(); + Self { + index: trial.index, + phase: trial.phase.label(), + kp: trial.gains.kp, + ki: trial.gains.ki, + kd: trial.gains.kd, + feedforward: trial.gains.feedforward, + cost: trial.score.cost, + verdict: trial.score.verdict.label(), + settled: metrics.settled, + rise_time_s: metrics.rise_time_s, + overshoot_pct: metrics.overshoot_pct, + settling_time_s: metrics.settling_time_s, + ss_error_pct: metrics.ss_error_pct, + steady_rpm: metrics.steady_rpm, + iae: metrics.iae, + peak_effort_raw: metrics.peak_effort_raw, + rms_effort_raw: metrics.rms_effort_raw, + saturated_pct: metrics.saturated_pct, + max_rpm: metrics.max_rpm, + } + } +} + +fn write_trials_csv(path: &Path, trials: &[Trial]) -> Result<(), ReportError> { + let mut writer = csv::Writer::from_path(path)?; + for trial in trials { + writer.serialize(TrialRow::from_trial(trial))?; + } + writer.flush().map_err(|e| io(path, e))?; + Ok(()) +} + +/// 轨迹 CSV 的一行。 +#[derive(Debug, Serialize)] +struct TraceRow { + t_s: f32, + setpoint_rpm: f32, + rpm: f32, + effort_raw: i16, + saturated: bool, +} + +fn write_trace_csv(path: &Path, trace: &Trace) -> Result<(), ReportError> { + let mut writer = csv::Writer::from_path(path)?; + for index in 0..trace.len() { + writer.serialize(TraceRow { + t_s: trace.t_s[index], + setpoint_rpm: trace.setpoint_rpm[index], + rpm: trace.rpm[index], + effort_raw: trace.effort_raw[index], + saturated: trace.saturated[index], + })?; + } + writer.flush().map_err(|e| io(path, e))?; + Ok(()) +} + +/// 可粘贴进既有 `[[motor]]` 段落的增益补丁。 +#[derive(Debug, Serialize)] +struct GainPatch { + kp: f32, + ki: f32, + kd: f32, + feedforward: f32, +} + +fn write_best_toml(path: &Path, ctx: &ReportContext, best: &Trial) -> Result<(), ReportError> { + let gains_patch = GainPatch { + kp: best.gains.kp, + ki: best.gains.ki, + kd: best.gains.kd, + feedforward: best.gains.feedforward, + }; + let mut text = String::new(); + writeln!(text, "# 由 chassis-tune 生成（seed={}, backend={}）", ctx.config.seed, ctx.backend) + .ok(); + writeln!( + text, + "# 轮子：{}｜场景：{}｜限流 {} raw", + ctx.motor_name, + crate::search::scenario_summary(&ctx.scenario), + ctx.limits.current_limit_raw + ) + .ok(); + writeln!(text, "# 参数空间：{}", ctx.space.describe()).ok(); + text.push_str("# 把下面四行粘进 chassis.toml 里对应的 [[motor]] 段落即可。\n"); + text.push_str(&toml::to_string(&gains_patch)?); + std::fs::write(path, text).map_err(|e| io(path, e))?; + Ok(()) +} + +/// `summary.json` 的结构。 +#[derive(Debug, Serialize)] +struct Summary<'a> { + backend: &'a str, + seed: u64, + samples: usize, + top_k: usize, + solver: &'static str, + evals: u64, + ok_trials: usize, + rejected_trials: usize, + scenario: String, + gain_space: String, + limits: LimitSummary, + thresholds: Thresholds, + best: BestSummary, + top: Vec, + notes: Vec, +} + +#[derive(Debug, Serialize)] +struct LimitSummary { + current_limit_raw: i16, +} + +#[derive(Debug, Serialize)] +struct Thresholds { + max_overshoot_pct: f32, + max_ss_error_pct: f32, +} + +#[derive(Debug, Serialize)] +struct BestSummary { + index: u64, + phase: &'static str, + kp: f32, + ki: f32, + kd: f32, + feedforward: f32, + cost: f64, + verdict: &'static str, + metrics: Option, +} + +#[derive(Debug, Serialize)] +struct MetricsSummary { + rise_time_s: f32, + overshoot_pct: f32, + settling_time_s: f32, + ss_error_pct: f32, + steady_rpm: f32, + iae: f32, + rms_effort_raw: f32, + saturated_pct: f32, + max_rpm: f32, +} + +impl MetricsSummary { + fn from_metrics(metrics: &StepMetrics) -> Self { + Self { + rise_time_s: metrics.rise_time_s, + overshoot_pct: metrics.overshoot_pct, + settling_time_s: metrics.settling_time_s, + ss_error_pct: metrics.ss_error_pct, + steady_rpm: metrics.steady_rpm, + iae: metrics.iae, + rms_effort_raw: metrics.rms_effort_raw, + saturated_pct: metrics.saturated_pct, + max_rpm: metrics.max_rpm, + } + } +} + +fn best_summary(trial: &Trial) -> BestSummary { + BestSummary { + index: trial.index, + phase: trial.phase.label(), + kp: trial.gains.kp, + ki: trial.gains.ki, + kd: trial.gains.kd, + feedforward: trial.gains.feedforward, + cost: trial.score.cost, + verdict: trial.score.verdict.label(), + metrics: trial.score.metrics.as_ref().map(MetricsSummary::from_metrics), + } +} + +fn write_summary(path: &Path, ctx: &ReportContext, ranked: &Ranked) -> Result<(), ReportError> { + let mut notes = Vec::new(); + if !ranked.best.score.verdict.is_ok() { + notes.push(format!( + "没有任何候选达标：最优判定为 {}（{}）", + ranked.best.score.verdict.label(), + verdict_hint(ranked.best.score.verdict) + )); + } + if let Some(metrics) = ranked.best.score.metrics { + if metrics.overshoot_pct > ctx.max_overshoot_pct { + notes.push(format!( + "最优解超调 {:.1}% 超过阈值 {:.1}%", + metrics.overshoot_pct, ctx.max_overshoot_pct + )); + } + if metrics.ss_error_pct > ctx.max_ss_error_pct { + notes.push(format!( + "最优解稳态误差 {:.1}% 超过阈值 {:.1}%", + metrics.ss_error_pct, ctx.max_ss_error_pct + )); + } + if metrics.saturated_pct > 5.0 { + notes.push(format!("最优解饱和占比 {:.1}%，限流可能偏紧", metrics.saturated_pct)); + } + } + // 贴边界的警告排在"通用提醒"之前：它是本次搜索最具体的一条结论。 + notes.extend(ctx.space.boundary_hits(&ranked.best.gains)); + notes.push( + "仿真整定依赖被控对象模型；上真机前先用 `validate` 在台架上复验（--backend bench）。" + .into(), + ); + + let summary = Summary { + backend: &ctx.backend, + seed: ctx.config.seed, + samples: ctx.config.samples, + top_k: ctx.config.top_k, + solver: ctx.config.solver.name(), + evals: ranked.evals, + ok_trials: ranked.ok_count(), + rejected_trials: ranked.rejected, + scenario: crate::search::scenario_summary(&ctx.scenario), + gain_space: ctx.space.describe(), + limits: LimitSummary { current_limit_raw: ctx.limits.current_limit_raw }, + thresholds: Thresholds { + max_overshoot_pct: ctx.max_overshoot_pct, + max_ss_error_pct: ctx.max_ss_error_pct, + }, + best: best_summary(&ranked.best), + top: ranked.top(10).iter().map(best_summary).collect(), + notes, + }; + let text = serde_json::to_string_pretty(&summary)?; + std::fs::write(path, text).map_err(|e| io(path, e))?; + Ok(()) +} + +fn write_markdown(path: &Path, ctx: &ReportContext, ranked: &Ranked) -> Result<(), ReportError> { + let mut text = String::new(); + let best = &ranked.best; + + writeln!(text, "# 速度环自动整定报告\n").ok(); + write_markdown_conclusion(&mut text, ctx, best); + write_markdown_settings(&mut text, ctx, ranked); + write_markdown_top(&mut text, ranked); + write_markdown_artifacts(&mut text); + write_markdown_notes(&mut text, ctx, best); + + std::fs::write(path, text).map_err(|e| io(path, e))?; + Ok(()) +} + +/// 结论段：最优增益、指标、验收、无解时的原因。 +fn write_markdown_conclusion(text: &mut String, ctx: &ReportContext, best: &Trial) { + writeln!(text, "## 结论\n").ok(); + writeln!( + text, + "**kp = {:.3}　ki = {:.3}　kd = {:.3}　feedforward = {:.3}**（成本 {:.4}，判定 {}）\n", + best.gains.kp, + best.gains.ki, + best.gains.kd, + best.gains.feedforward, + best.cost(), + best.score.verdict.label() + ) + .ok(); + if let Some(metrics) = best.score.metrics { + writeln!( + text, + "- 上升时间 {:.0} ms｜超调 {:.1}%｜调节时间 {:.0} ms｜稳态误差 {:.2}%", + metrics.rise_time_s * 1000.0, + metrics.overshoot_pct, + metrics.settling_time_s * 1000.0, + metrics.ss_error_pct + ) + .ok(); + writeln!( + text, + "- 峰值电流 {:.0} raw｜RMS 电流 {:.0} raw｜饱和 {:.1}%｜峰值转速 {:.0} rpm", + metrics.peak_effort_raw, metrics.rms_effort_raw, metrics.saturated_pct, metrics.max_rpm + ) + .ok(); + writeln!( + text, + "- 验收：{}（阈值 超调 ≤ {:.1}%，稳态误差 ≤ {:.1}%）\n", + if ctx.best_passes(&metrics) { "✓ 通过" } else { "✗ 未通过" }, + ctx.max_overshoot_pct, + ctx.max_ss_error_pct + ) + .ok(); + } + if !best.score.verdict.is_ok() { + writeln!( + text, + "> ⚠ 没有任何候选达标（最优：{}）。{}\n", + best.score.verdict.label(), + verdict_hint(best.score.verdict) + ) + .ok(); + } +} + +/// 设置段：复现本次搜索所需的全部输入。 +fn write_markdown_settings(text: &mut String, ctx: &ReportContext, ranked: &Ranked) { + writeln!(text, "## 设置\n").ok(); + writeln!(text, "| 项 | 值 |").ok(); + writeln!(text, "| --- | --- |").ok(); + writeln!(text, "| 后端 | `{}` |", ctx.backend).ok(); + writeln!(text, "| 场景 | `{}` |", crate::search::scenario_summary(&ctx.scenario)).ok(); + writeln!(text, "| 参数空间 | `{}` |", ctx.space.describe()).ok(); + writeln!(text, "| 限流 | {} raw |", ctx.limits.current_limit_raw).ok(); + writeln!(text, "| 种子 | {} |", ctx.config.seed).ok(); + writeln!( + text, + "| 搜索 | samples={}, top_k={}, solver={}, max_iters={} |", + ctx.config.samples, + ctx.config.top_k, + ctx.config.solver.name(), + ctx.config.max_iters + ) + .ok(); + writeln!( + text, + "| 评估 | {} 次（达标 {} / 共 {}） |", + ranked.evals, + ranked.ok_count(), + ranked.trials.len() + ) + .ok(); + writeln!(text, "| 目标轮 | `{}` |\n", ctx.motor_name).ok(); +} + +/// Top-10 表格。 +fn write_markdown_top(text: &mut String, ranked: &Ranked) { + writeln!(text, "## Top 10\n").ok(); + writeln!( + text, + "| # | 阶段 | kp | ki | kd | ff | 成本 | 判定 | 超调 % | 稳态误差 % | 调节 ms |" + ) + .ok(); + writeln!(text, "| --- | --- | --- | --- | --- | --- | --- | --- | --- | --- | --- |").ok(); + for (rank, trial) in ranked.top(10).iter().enumerate() { + let metrics = trial.score.metrics.unwrap_or_default(); + writeln!( + text, + "| {} | {} | {:.3} | {:.3} | {:.3} | {:.3} | {:.4} | {} | {:.1} | {:.2} | {:.0} |", + rank + 1, + trial.phase.label(), + trial.gains.kp, + trial.gains.ki, + trial.gains.kd, + trial.gains.feedforward, + trial.cost(), + trial.score.verdict.label(), + metrics.overshoot_pct, + metrics.ss_error_pct, + metrics.settling_time_s * 1000.0 + ) + .ok(); + } + writeln!(text).ok(); +} + +/// 产物清单。 +fn write_markdown_artifacts(text: &mut String) { + writeln!(text, "## 产物\n").ok(); + writeln!(text, "- `trials.csv`：全部候选（含未达标者与判定原因）").ok(); + writeln!(text, "- `best.toml`：可粘贴的 `[[motor]]` 增益片段").ok(); + writeln!(text, "- `best-trace.csv`：最优增益的完整轨迹").ok(); + writeln!(text, "- `hil-validation.csv`：真机重复性复验（仅 `--backend bench`）").ok(); + writeln!(text, "- `summary.json`：机器可读结果\n").ok(); +} + +/// 注意事项：把"仿真整定不能替代实车复验"写进报告，而不是只写在 README 里。 +fn write_markdown_notes(text: &mut String, ctx: &ReportContext, best: &Trial) { + let hits = ctx.space.boundary_hits(&best.gains); + writeln!(text, "## 注意事项\n").ok(); + writeln!(text, "1. 仿真整定只保证在被控对象模型内最优；模型参数（摩擦 / 延迟）是量级估计，") + .ok(); + writeln!( + text, + " 上真机前请用同一 `--scenario` 跑 `chassis-tune sweep --backend bench` 复验。" + ) + .ok(); + writeln!( + text, + "2. 台架搜索会真的驱动轮子：必须架空或整车固定，且加 `--i-know-the-wheel-spins`。" + ) + .ok(); + writeln!( + text, + "3. `current_limit` 属于安全约束，不参与搜索；它会钳住 PID 输出，也决定饱和判定。" + ) + .ok(); + writeln!(text, "4. 复现本次结果：同一 `--seed` + 同一参数空间 → 逐位一致。").ok(); + for hit in &hits { + writeln!(text, "- ⚠ 搜索落在参数空间边界：{hit}").ok(); + } + writeln!( + text, + "5. 本模型不含测量噪声：仿真里更高的增益不会被噪声惩罚，因此最优解可能偏激进——\n 台架复验（`--backend bench`）是把它拉回现实的唯一手段。" + ) + .ok(); +} + +impl ReportContext { + /// 一组指标是否满足验收阈值。 + #[must_use] + pub fn best_passes(&self, metrics: &StepMetrics) -> bool { + metrics.settled + && metrics.overshoot_pct <= self.max_overshoot_pct + && metrics.ss_error_pct <= self.max_ss_error_pct + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::backend::{Evaluator, SimEvaluator}; + use crate::cost::{Objective, Weights}; + use crate::gains::GainSpace; + use crate::search::{SearchConfig, sweep}; + use dji_esc_sim::{LoopLimits, PlantEnsemble, PlantParams}; + + fn ranked(name: &str) -> (GainSpace, ReportContext, Ranked) { + let space = GainSpace::default_gm3508(); + let scenario = Scenario::step(1500.0, 1.0); + let limits = LoopLimits::gm3508(); + let objective = Objective::new(Weights::default(), scenario, limits); + let ensemble = PlantEnsemble::homogeneous(PlantParams::gm3508(), 1); + let evaluator = SimEvaluator::new(ensemble, scenario, limits, objective, 0.0); + let config = SearchConfig { + seed: 3, + samples: 48, + top_k: 2, + max_iters: 40, + max_evals: 900, + ..SearchConfig::default() + }; + let ranked = sweep(&space, &config, &evaluator, None).unwrap(); + let trace = + dji_esc_sim::simulate(ranked.best.gains, &PlantParams::gm3508(), &scenario, limits) + .unwrap(); + let ctx = ReportContext { + out_dir: std::env::temp_dir().join(format!("chassis-tune-report-{name}")), + space, + config, + scenario, + limits, + backend: evaluator.describe(), + motor_name: "front_left".into(), + max_overshoot_pct: 20.0, + max_ss_error_pct: 5.0, + best_trace: Some(trace), + }; + (space, ctx, ranked) + } + + #[test] + fn writes_every_artifact() { + let (_, ctx, ranked) = ranked("writes_every_artifact"); + let _ = std::fs::remove_dir_all(&ctx.out_dir); + let written = write_reports(&ctx, &ranked).unwrap(); + let names: Vec = written + .iter() + .map(|p| p.file_name().unwrap_or_default().to_string_lossy().into_owned()) + .collect(); + for expected in ["trials.csv", "best.toml", "best-trace.csv", "report.md", "summary.json"] { + assert!(names.iter().any(|n| n == expected), "missing {expected}: {names:?}"); + } + for path in &written { + assert!(path.exists(), "{} must exist", path.display()); + } + } + + #[test] + fn trials_csv_has_a_stable_header_and_one_row_per_trial() { + let (_, ctx, ranked) = ranked("trials_csv_has_a_stable_header_and_one_row_per_trial"); + write_reports(&ctx, &ranked).unwrap(); + let text = std::fs::read_to_string(ctx.out_dir.join("trials.csv")).unwrap(); + let mut lines = text.lines(); + let header = lines.next().unwrap(); + for column in ["index", "phase", "kp", "ki", "kd", "feedforward", "cost", "verdict"] { + assert!(header.contains(column), "header missing {column}: {header}"); + } + assert_eq!(lines.count(), ranked.trials.len()); + } + + #[test] + fn best_toml_parses_back_through_the_config_schema() { + let (_, ctx, ranked) = ranked("best_toml_parses_back_through_the_config_schema"); + write_reports(&ctx, &ranked).unwrap(); + let text = std::fs::read_to_string(ctx.out_dir.join("best.toml")).unwrap(); + // 片段本体是合法 TOML：与其它段落拼起来即可被 chassis-config 读回。 + let patch = text + .lines() + .filter(|line| !line.trim_start().starts_with('#')) + .collect::>() + .join("\n"); + let full = format!( + "[can]\ninterface = \"can0\"\n[mecanum]\nhalf_length = 0.2\nhalf_width = 0.25\nwheel_radius = 0.0762\n\n[[motor]]\nname = \"front_left\"\nesc_id = 1\ndirection = 1\ngear_ratio = 19.203\n{patch}" + ); + let mut motors = Vec::new(); + for index in 1..4 { + motors.push(format!( + "[[motor]]\nname = \"w{index}\"\nesc_id = {}\ndirection = 1\ngear_ratio = 19.203\n", + index + 1 + )); + } + let full = format!("{full}\n{}", motors.join("\n")); + let config = chassis_config::Config::from_toml(&full).expect("best.toml must be readable"); + let gains = config.motors[0].velocity_gains(); + assert_eq!(gains, ranked.best.gains); + } + + #[test] + fn summary_json_is_machine_readable() { + let (_, ctx, ranked) = ranked("summary_json_is_machine_readable"); + write_reports(&ctx, &ranked).unwrap(); + let text = std::fs::read_to_string(ctx.out_dir.join("summary.json")).unwrap(); + let value: serde_json::Value = serde_json::from_str(&text).unwrap(); + assert_eq!(value["seed"], serde_json::json!(3)); + assert_eq!(value["samples"], serde_json::json!(48)); + assert!(value["best"]["kp"].as_f64().unwrap() > 0.0); + assert!(value["top"].as_array().unwrap().len() <= 10); + assert!(!value["notes"].as_array().unwrap().is_empty()); + } + + #[test] + fn markdown_mentions_the_gains_and_thresholds() { + let (_, ctx, ranked) = ranked("markdown_mentions_the_gains_and_thresholds"); + write_reports(&ctx, &ranked).unwrap(); + let text = std::fs::read_to_string(ctx.out_dir.join("report.md")).unwrap(); + assert!(text.contains("kp ="), "{text}"); + assert!(text.contains("Top 10"), "{text}"); + assert!(text.contains("front_left"), "{text}"); + assert!(text.contains("注意事项"), "{text}"); + } + + #[test] + fn best_trace_csv_matches_the_trace_length() { + let (_, ctx, ranked) = ranked("best_trace_csv_matches_the_trace_length"); + write_reports(&ctx, &ranked).unwrap(); + let text = std::fs::read_to_string(ctx.out_dir.join("best-trace.csv")).unwrap(); + let rows = text.lines().count(); + let expected = ctx.best_trace.as_ref().unwrap().len() + 1; // + 表头 + assert_eq!(rows, expected); + } + + #[test] + fn best_passes_respects_the_thresholds() { + let (_, ctx, _) = ranked("t"); + let passing = StepMetrics { + settled: true, + overshoot_pct: 10.0, + ss_error_pct: 1.0, + ..StepMetrics::default() + }; + assert!(ctx.best_passes(&passing)); + let overshooting = StepMetrics { overshoot_pct: 50.0, ..passing }; + assert!(!ctx.best_passes(&overshooting)); + let unsettled = StepMetrics { settled: false, ..passing }; + assert!(!ctx.best_passes(&unsettled)); + } +} diff --git a/crates/chassis-tune/src/search.rs b/crates/chassis-tune/src/search.rs new file mode 100644 index 0000000..14feeaf --- /dev/null +++ b/crates/chassis-tune/src/search.rs @@ -0,0 +1,628 @@ +//! 两阶段参数空间搜索：种子化分层粗扫 → `argmin` 多起点局部精修。 +//! +//! # 为什么是两阶段 +//! +//! 成本函数有一大片"罚值平原"（不收敛 / 饱和的候选成本都等于 `PENALTY`）， +//! 纯局部搜索会陷在平原上；而纯网格搜索在四个数量级跨度上需要天文数字的 +//! 评估次数。两阶段各取所长： +//! +//! 1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数把 +//! 空间铺满，拿到若干个互不相邻的可达解（top-k）； +//! 2. **精修**：以每个可达解为起点做多起点 `argmin` 局部搜索（Nelder-Mead +//! 或 PSO），在**坐标空间**（对数刻度）里收敛到局部最优。 +//! +//! # 可复现性 +//! +//! 每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，结果与线程 +//! 调度无关；粗扫结果按下标排序后再取 top-k，因此同 seed 两次运行逐位一致。 + +use std::sync::atomic::{AtomicU64, Ordering}; + +use argmin::core::{CostFunction, Error as ArgminError, Executor}; +use argmin::solver::neldermead::NelderMead; +use argmin::solver::particleswarm::ParticleSwarm; +use dji_esc_sim::Scenario; +use indicatif::ProgressBar; +use motor_core::VelocityGains; +use rand::SeedableRng as _; +use rand::rngs::StdRng; +use rayon::prelude::*; +use thiserror::Error; + +use crate::backend::Evaluator; +use crate::cost::{Score, Verdict}; +use crate::gains::{AXIS_COUNT, GainSpace}; + +/// 搜索阶段的错误。 +#[derive(Debug, Clone, PartialEq, Eq, Error)] +pub enum SearchError { + /// 参数空间非法。 + #[error("invalid gain space: {0}")] + InvalidSpace(String), + /// 粗扫样本数为 0。 + #[error("samples must be >= 1")] + NoSamples, +} + +/// 候选来自哪个阶段。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum Phase { + /// 分层粗扫。 + Coarse, + /// 局部精修。 + Refine, + /// 用户指定 / 基线点。 + Seed, +} + +impl Phase { + /// CSV / 报告标签。 + #[must_use] + pub fn label(self) -> &'static str { + match self { + Self::Coarse => "coarse", + Self::Refine => "refine", + Self::Seed => "seed", + } + } +} + +/// 一次被评估的候选。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Trial { + /// 评估序号（按评估顺序递增，报告可复现）。 + pub index: u64, + /// 所属阶段。 + pub phase: Phase, + /// 增益。 + pub gains: VelocityGains, + /// 评估结果。 + pub score: Score, +} + +impl Trial { + /// 成本。 + #[must_use] + pub fn cost(&self) -> f64 { + self.score.cost + } +} + +/// 局部精修求解器。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] +pub enum SolverKind { + /// Nelder-Mead（无导数、对罚值平原鲁棒），默认。 + #[default] + NelderMead, + /// 粒子群（更全局，评估次数更高）。 + Pso, +} + +impl SolverKind { + /// 文本名。 + #[must_use] + pub fn name(self) -> &'static str { + match self { + Self::NelderMead => "nelder-mead", + Self::Pso => "pso", + } + } + + /// 从文本解析。 + /// + /// # Errors + /// + /// 未知名字时返回错误。 + pub fn parse(text: &str) -> Result { + match text { + "nelder-mead" | "neldermead" | "nm" => Ok(Self::NelderMead), + "pso" | "particle-swarm" => Ok(Self::Pso), + other => Err(SearchError::InvalidSpace(format!( + "unknown solver '{other}' (expected nelder-mead|pso)" + ))), + } + } +} + +/// 搜索配置。 +#[derive(Debug, Clone, PartialEq)] +pub struct SearchConfig { + /// 种子（决定分层采样与多起点顺序）。 + pub seed: u64, + /// 粗扫样本数。 + pub samples: usize, + /// 进入精修的种子数量。 + pub top_k: usize, + /// 精修求解器。 + pub solver: SolverKind, + /// 每个起点的最大迭代数。 + pub max_iters: usize, + /// PSO 粒子数（`SolverKind::Pso` 时生效）。 + pub particles: usize, + /// 评估预算上限（含粗扫）。 + pub max_evals: u64, +} + +impl Default for SearchConfig { + fn default() -> Self { + Self { + seed: 20_260_101, + samples: 512, + top_k: 8, + solver: SolverKind::NelderMead, + max_iters: 200, + particles: 24, + max_evals: 4_000, + } + } +} + +impl SearchConfig { + /// 校验。 + /// + /// 预算小于样本数**不是**错误：[`sweep`] 会把预算下限抬到 `samples` + /// （粗扫必须能把铺满空间的样本跑完），因此只有"没有样本"才是硬错误。 + /// + /// # Errors + /// + /// 样本数为 0 时返回 [`SearchError::NoSamples`]。 + pub fn validate(&self) -> Result<(), SearchError> { + if self.samples == 0 { + return Err(SearchError::NoSamples); + } + Ok(()) + } +} + +/// 搜索结果（按成本升序）。 +#[derive(Debug, Clone)] +pub struct Ranked { + /// 全部候选（升序）。 + pub trials: Vec, + /// 最优候选。 + pub best: Trial, + /// 实际评估次数。 + pub evals: u64, + /// 被硬约束拦下的候选数。 + pub rejected: usize, +} + +impl Ranked { + /// 前 `n` 名。 + #[must_use] + pub fn top(&self, n: usize) -> &[Trial] { + let n = n.min(self.trials.len()); + &self.trials[..n] + } + + /// 达到 `Verdict::Ok` 的候选数。 + #[must_use] + pub fn ok_count(&self) -> usize { + self.trials.iter().filter(|t| t.score.verdict.is_ok()).count() + } +} + +/// 共享的评估预算与计数器。 +struct Budget { + used: AtomicU64, + limit: u64, +} + +impl Budget { + fn new(limit: u64) -> Self { + Self { used: AtomicU64::new(0), limit } + } + + /// 取一个评估配额；返回 `None` 表示预算耗尽。 + /// + /// 用 CAS 而不是 `fetch_add`：预算耗尽后优化器仍会持续询问，若仍自增， + /// `spent()` 就会把"被拒绝的询问"也算成评估次数，报告里的数字便失去意义。 + fn take(&self) -> Option { + let mut current = self.used.load(Ordering::Relaxed); + loop { + if current >= self.limit { + return None; + } + match self.used.compare_exchange_weak( + current, + current + 1, + Ordering::Relaxed, + Ordering::Relaxed, + ) { + Ok(_) => return Some(current), + Err(observed) => current = observed, + } + } + } + + fn spent(&self) -> u64 { + self.used.load(Ordering::Relaxed) + } +} + +/// 按预算评估一组增益（预算耗尽时直接给罚值，不再花时间算）。 +fn evaluate_with_budget( + budget: &Budget, + evaluator: &dyn Evaluator, + gains: &VelocityGains, +) -> Score { + if budget.take().is_some() { + evaluator.evaluate(gains) + } else { + Score::penalized(Verdict::Rejected) + } +} + +/// 记录本次精修见过的最优解。 +/// +/// 不依赖 `argmin` 的结果访问器（跨版本不稳定），而是让成本函数自己记账： +/// 谁算出过最低成本，谁就是这次精修的结果。 +#[derive(Default)] +struct BestTracker { + best: std::sync::Mutex>, +} + +impl BestTracker { + fn new() -> Self { + Self::default() + } + + fn record(&self, cost: f64, coords: [f64; AXIS_COUNT]) { + let mut best = self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + if best.is_none_or(|(current, _)| cost < current) { + *best = Some((cost, coords)); + } + } + + fn take(&self) -> Option<(f64, [f64; AXIS_COUNT])> { + *self.best.lock().unwrap_or_else(std::sync::PoisonError::into_inner) + } +} + +/// 把 [`Evaluator`] 适配成 `argmin` 的成本函数（坐标空间参数）。 +struct CoordProblem<'a> { + evaluator: &'a dyn Evaluator, + space: &'a GainSpace, + budget: &'a Budget, + tracker: &'a BestTracker, +} + +impl CostFunction for CoordProblem<'_> { + type Param = Vec; + type Output = f64; + + fn cost(&self, param: &Self::Param) -> Result { + let coords: [f64; AXIS_COUNT] = match param.as_slice() { + [a, b, c, d] => [*a, *b, *c, *d], + _ => return Ok(crate::cost::PENALTY), + }; + let gains = self.space.from_coords(coords); + let cost = match self.budget.take() { + Some(_) => self.evaluator.evaluate(&gains).cost, + None => crate::cost::PENALTY, + }; + if cost.is_finite() { + self.tracker.record(cost, coords); + } + Ok(cost) + } +} + +/// 以 `start` 为起点做一次局部精修，返回见过的最优增益。 +fn refine( + space: &GainSpace, + evaluator: &dyn Evaluator, + budget: &Budget, + config: &SearchConfig, + start: VelocityGains, + seed: u64, +) -> VelocityGains { + let (lower, upper) = space.coord_bounds(); + let tracker = BestTracker::new(); + let problem = CoordProblem { evaluator, space, budget, tracker: &tracker }; + let start_coords = space.to_coords(&start); + + // 两种求解器的 `OptimizationResult` 类型不同，这里只保留"成功 / 失败"。 + let outcome: Result<(), ArgminError> = match config.solver { + SolverKind::NelderMead => { + // n 维需要 n+1 个初始顶点：起点 + 每轴一小步（轴跨度的 5%）。 + let mut simplex: Vec> = Vec::with_capacity(AXIS_COUNT + 1); + simplex.push(start_coords.to_vec()); + for axis in 0..AXIS_COUNT { + let mut vertex = start_coords; + let span = (upper[axis] - lower[axis]).abs(); + vertex[axis] = (vertex[axis] + 0.05 * span.max(1.0e-3)).min(upper[axis]); + simplex.push(vertex.to_vec()); + } + let solver: NelderMead, f64> = NelderMead::new(simplex); + Executor::new(problem, solver) + .configure(|state| state.max_iters(config.max_iters as u64)) + .run() + .map(|_| ()) + } + SolverKind::Pso => { + let solver: ParticleSwarm, f64, StdRng> = + ParticleSwarm::new((lower.to_vec(), upper.to_vec()), config.particles.max(2)) + .with_rng_generator(StdRng::seed_from_u64(seed)); + Executor::new(problem, solver) + .configure(|state| state.max_iters(config.max_iters as u64)) + .run() + .map(|_| ()) + } + }; + // 求解器失败（预算耗尽 / 数值问题）不算错：退化到起点，搜索继续。 + if let Err(error) = outcome { + tracing::debug!(%error, "local refinement stopped early"); + } + tracker.take().map_or(start, |(_, coords)| space.from_coords(coords)) +} + +/// 执行两阶段搜索。 +/// +/// # Errors +/// +/// 参数空间或配置非法时返回 [`SearchError`]。 +pub fn sweep( + space: &GainSpace, + config: &SearchConfig, + evaluator: &dyn Evaluator, + progress: Option<&ProgressBar>, +) -> Result { + space.validate().map_err(|e| SearchError::InvalidSpace(e.to_string()))?; + config.validate()?; + + let budget = Budget::new(config.max_evals.max(config.samples as u64)); + + // ---------------- 阶段 1：分层粗扫（并行） ---------------- + let mut rng = StdRng::seed_from_u64(config.seed); + let candidates = space.stratified(config.samples, &mut rng); + let mut trials: Vec = candidates + .into_par_iter() + .enumerate() + .map(|(index, gains)| Trial { + index: index as u64, + phase: Phase::Coarse, + gains, + score: evaluate_with_budget(&budget, evaluator, &gains), + }) + .collect(); + if let Some(bar) = progress { + bar.inc(trials.len() as u64); + } + + // 排序后再取 top-k：与线程调度无关，同 seed 必定选出同一批种子。 + trials.sort_by(|a, b| a.cost().total_cmp(&b.cost()).then(a.index.cmp(&b.index))); + let seeds: Vec = trials.iter().take(config.top_k.max(1)).copied().collect(); + + // ---------------- 阶段 2：多起点精修（并行） ---------------- + let coarse_count = trials.len() as u64; + let refined: Vec = seeds + .par_iter() + .enumerate() + .map(|(slot, seed_trial)| { + let seed = config.seed.wrapping_add((slot as u64 + 1).wrapping_mul(0x9E37_79B9)); + let gains = refine(space, evaluator, &budget, config, seed_trial.gains, seed); + let score = evaluate_with_budget(&budget, evaluator, &gains); + Trial { index: coarse_count + slot as u64, phase: Phase::Refine, gains, score } + }) + .collect(); + if let Some(bar) = progress { + bar.inc(refined.len() as u64); + } + trials.extend(refined); + + trials.sort_by(|a, b| a.cost().total_cmp(&b.cost()).then(a.index.cmp(&b.index))); + let evals = budget.spent(); + let rejected = trials.iter().filter(|t| !t.score.verdict.is_ok()).count(); + let best = trials.first().copied().ok_or(SearchError::NoSamples)?; + Ok(Ranked { trials, best, evals, rejected }) +} + +/// 只评估一组增益（`validate` 子命令）。 +#[must_use] +pub fn evaluate_once(evaluator: &dyn Evaluator, gains: &VelocityGains) -> Trial { + Trial { index: 0, phase: Phase::Seed, gains: *gains, score: evaluator.evaluate(gains) } +} + +/// 一次评估都未达标时的提示（报告用）。 +#[must_use] +pub fn verdict_hint(verdict: Verdict) -> &'static str { + match verdict { + Verdict::Ok => "达标", + Verdict::Rejected => "参数非法或场景退化", + Verdict::Diverged => "数值发散：增益过大", + Verdict::Overspeed => "过速：增益过大或前馈过高", + Verdict::Saturated => "长时间饱和：限流太紧或积分过大", + Verdict::NoSettle => "未收敛：增益过小或积分不足（空载台架考虑 ff=0）", + } +} + +/// 报告用的场景摘要。 +#[must_use] +pub fn scenario_summary(scenario: &Scenario) -> String { + format!( + "{:?} target={:.0} rpm ramp={:.0}ms hold={:.0}ms dt={:.1}ms", + scenario.profile, + scenario.target_rpm, + scenario.ramp_s * 1000.0, + scenario.hold_s * 1000.0, + scenario.dt_s * 1000.0 + ) +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::backend::SimEvaluator; + use crate::cost::{Objective, Weights}; + use dji_esc_sim::{LoopLimits, PlantEnsemble, PlantParams, PlantTolerances}; + + fn space() -> GainSpace { + GainSpace::default_gm3508() + } + + fn evaluator(plant: PlantParams, ensemble: usize, robust_weight: f64) -> SimEvaluator { + let scenario = Scenario::step(1500.0, 2.0); + let limits = LoopLimits::gm3508(); + let objective = Objective::new(Weights::default(), scenario, limits); + let ensemble = PlantEnsemble::randomized(plant, PlantTolerances::default(), ensemble, 7); + SimEvaluator::new(ensemble, scenario, limits, objective, robust_weight) + } + + fn small_config(seed: u64) -> SearchConfig { + SearchConfig { + seed, + samples: 64, + top_k: 3, + max_iters: 60, + max_evals: 2_000, + ..SearchConfig::default() + } + } + + #[test] + fn sweep_finds_a_usable_configuration() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let ranked = sweep(&space(), &small_config(1), &evaluator, None).unwrap(); + let best = ranked.best; + assert_eq!(best.score.verdict, Verdict::Ok, "{best:?}"); + let metrics = best.score.metrics.unwrap(); + assert!(metrics.ss_error_pct < 5.0, "ss = {}", metrics.ss_error_pct); + assert!(metrics.overshoot_pct < 30.0, "overshoot = {}", metrics.overshoot_pct); + } + + #[test] + fn sweep_beats_the_documented_hand_tuned_baseline() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + // README 记录的空载台架起步整定：kp=2, ki=10, kd=0, ff=0。 + let baseline = evaluator.evaluate(&VelocityGains::new(2.0, 10.0, 0.0, 0.0)); + let ranked = sweep(&space(), &small_config(17), &evaluator, None).unwrap(); + assert!( + ranked.best.cost() < baseline.cost, + "search {} must beat baseline {}", + ranked.best.cost(), + baseline.cost + ); + } + + #[test] + fn results_are_reproducible_for_a_fixed_seed() { + let evaluator = evaluator(PlantParams::gm3508_bench(), 4, 0.5); + let a = sweep(&space(), &small_config(99), &evaluator, None).unwrap(); + let b = sweep(&space(), &small_config(99), &evaluator, None).unwrap(); + assert_eq!(a.best.gains, b.best.gains, "同 seed 必须给出同一组增益"); + assert_eq!(a.best.cost().to_bits(), b.best.cost().to_bits()); + assert_eq!(a.trials.len(), b.trials.len()); + for (x, y) in a.trials.iter().zip(&b.trials) { + assert_eq!(x.gains, y.gains); + assert_eq!(x.cost().to_bits(), y.cost().to_bits()); + } + } + + #[test] + fn different_seeds_explore_differently() { + let evaluator = evaluator(PlantParams::gm3508_bench(), 2, 0.5); + let a = sweep(&space(), &small_config(1), &evaluator, None).unwrap(); + let b = sweep(&space(), &small_config(2), &evaluator, None).unwrap(); + let same_phase1: Vec<_> = + a.trials.iter().filter(|t| t.phase == Phase::Coarse).map(|t| t.gains).collect(); + let other: Vec<_> = + b.trials.iter().filter(|t| t.phase == Phase::Coarse).map(|t| t.gains).collect(); + assert_ne!(same_phase1, other); + } + + #[test] + fn refined_trials_are_not_worse_than_their_seed() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let ranked = sweep(&space(), &small_config(5), &evaluator, None).unwrap(); + let coarse_best = ranked + .trials + .iter() + .filter(|t| t.phase == Phase::Coarse) + .map(Trial::cost) + .fold(f64::INFINITY, f64::min); + assert!( + ranked.best.cost() <= coarse_best + 1e-9, + "best {} vs coarse best {coarse_best}", + ranked.best.cost() + ); + assert!(ranked.trials.iter().any(|t| t.phase == Phase::Refine)); + } + + #[test] + fn pso_solver_also_improves_or_holds() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let mut config = small_config(3); + config.solver = SolverKind::Pso; + config.particles = 8; + config.max_iters = 12; + let ranked = sweep(&space(), &config, &evaluator, None).unwrap(); + assert_eq!(ranked.best.score.verdict, Verdict::Ok, "{:?}", ranked.best); + } + + #[test] + fn budget_bounds_the_number_of_evaluations() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let mut config = small_config(4); + config.max_evals = 80; + let ranked = sweep(&space(), &config, &evaluator, None).unwrap(); + assert!(ranked.evals <= 84, "evals = {}", ranked.evals); + } + + #[test] + fn invalid_space_is_rejected() { + let mut bad = space(); + bad.kp.min = -1.0; + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + assert!(matches!( + sweep(&bad, &small_config(1), &evaluator, None), + Err(SearchError::InvalidSpace(_)) + )); + } + + #[test] + fn zero_samples_is_rejected() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let mut config = small_config(1); + config.samples = 0; + assert_eq!(sweep(&space(), &config, &evaluator, None).err(), Some(SearchError::NoSamples)); + } + + #[test] + fn solver_kind_parsing_round_trips() { + assert_eq!(SolverKind::parse("nelder-mead").unwrap(), SolverKind::NelderMead); + assert_eq!(SolverKind::parse("pso").unwrap(), SolverKind::Pso); + assert!(SolverKind::parse("simulated-annealing").is_err()); + assert_eq!(SolverKind::default().name(), "nelder-mead"); + } + + #[test] + fn evaluate_once_reports_the_same_verdict() { + let evaluator = evaluator(PlantParams::gm3508(), 1, 0.0); + let trial = evaluate_once(&evaluator, &VelocityGains::new(4.0, 12.0, 0.0, 0.0)); + assert_eq!(trial.phase, Phase::Seed); + assert_eq!(trial.score.verdict, Verdict::Ok); + assert!(trial.score.metrics.is_some()); + } + + #[test] + fn verdict_hints_cover_every_verdict() { + for verdict in [ + Verdict::Ok, + Verdict::Rejected, + Verdict::Diverged, + Verdict::Overspeed, + Verdict::Saturated, + Verdict::NoSettle, + ] { + assert!(!verdict_hint(verdict).is_empty()); + } + } + + #[test] + fn scenario_summary_mentions_the_key_numbers() { + let text = scenario_summary(&Scenario::step(1200.0, 1.0)); + assert!(text.contains("1200 rpm"), "{text}"); + assert!(text.contains("1000ms"), "{text}"); + } +} diff --git a/crates/chassisd/Cargo.toml b/crates/chassisd/Cargo.toml index b0b0e83..3389aae 100644 --- a/crates/chassisd/Cargo.toml +++ b/crates/chassisd/Cargo.toml @@ -10,16 +10,16 @@ description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety workspace = true [dependencies] +chassis-config = { workspace = true } motor-core = { workspace = true } dji-esc = { workspace = true } +dji-esc-sim = { workspace = true } dji-esc-socketcan = { workspace = true } mecanum = { workspace = true } tokio = { workspace = true } thiserror = { workspace = true } tracing = { workspace = true } tracing-subscriber = { workspace = true } -serde = { workspace = true } -toml = { workspace = true } clap = { workspace = true } [dev-dependencies] diff --git a/crates/chassisd/src/controller.rs b/crates/chassisd/src/controller.rs index d007abc..709caa7 100644 --- a/crates/chassisd/src/controller.rs +++ b/crates/chassisd/src/controller.rs @@ -179,16 +179,10 @@ mod tests { let missing = MockMotor::new(2); // 无反馈 let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)]; let _ = mocks_ready; - let wheels = wrap_wheels( - [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], - [GEAR; 4], - ); - let ctrl = ChassisController::new( - kinematics, - wheels, - (3.0, 3.0, 6.0), - Duration::from_millis(200), - ); + let wheels = + wrap_wheels([ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [GEAR; 4]); + let ctrl = + ChassisController::new(kinematics, wheels, (3.0, 3.0, 6.0), Duration::from_millis(200)); let (tx, rx) = watch::channel(Twist::ZERO); let handle = ctrl.spawn(rx, Duration::from_millis(1)); diff --git a/crates/chassisd/src/lib.rs b/crates/chassisd/src/lib.rs index 850bf4b..a2ade32 100644 --- a/crates/chassisd/src/lib.rs +++ b/crates/chassisd/src/lib.rs @@ -1,13 +1,18 @@ -//! `chassisd` 库部分：配置模型、底盘控制循环与指令源抽象。 +//! `chassisd` 库部分：底盘控制循环与指令源抽象。 +//! +//! 配置 schema 已独立为 [`chassis_config`]（守护进程与整定工具共用一份 +//! 定义），这里以 `config` 别名重新导出，保持 `chassisd::config::Config` +//! 这一历史路径可用。 //! //! 二进制入口见 `src/main.rs`。 #![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] -pub mod config; +pub use chassis_config as config; + pub mod controller; pub mod source; -pub use config::{ +pub use chassis_config::{ CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig, ProtocolFamily, }; diff --git a/crates/chassisd/src/main.rs b/crates/chassisd/src/main.rs index 860feec..3175118 100644 --- a/crates/chassisd/src/main.rs +++ b/crates/chassisd/src/main.rs @@ -87,7 +87,7 @@ async fn run( let link = if mock { let (bus_side, plant_side) = dji_esc::CanLink::pair(512); let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); - dji_esc::spawn_plant(plant_side, &esc_ids, bus_options.period); + dji_esc_sim::spawn_plant(plant_side, &esc_ids, bus_options.period); info!(?esc_ids, "mock 模式：进程内电机仿真"); bus_side } else { diff --git a/crates/chassisd/tests/daemon.rs b/crates/chassisd/tests/daemon.rs index 20e6791..c8bdf65 100644 --- a/crates/chassisd/tests/daemon.rs +++ b/crates/chassisd/tests/daemon.rs @@ -7,7 +7,8 @@ use std::time::Duration; use chassisd::config::Config; use chassisd::controller::{ChassisController, wrap_wheels}; -use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant}; +use dji_esc::{CanLink, EscBus, EscBusHandle}; +use dji_esc_sim::spawn_plant; use mecanum::Twist; use motor_core::MotorFeedback; use tokio::sync::watch; diff --git a/crates/dji-esc-sim/Cargo.toml b/crates/dji-esc-sim/Cargo.toml new file mode 100644 index 0000000..cf82802 --- /dev/null +++ b/crates/dji-esc-sim/Cargo.toml @@ -0,0 +1,22 @@ +[package] +name = "dji-esc-sim" +description = "C620/C610 plant simulation: friction/delay model, scenario harness, and async plant rig" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can-link = { workspace = true } +motor-core = { workspace = true } +dji-esc = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } +rand = { workspace = true } + +[dev-dependencies] +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/dji-esc-sim/src/ensemble.rs b/crates/dji-esc-sim/src/ensemble.rs new file mode 100644 index 0000000..09c8a70 --- /dev/null +++ b/crates/dji-esc-sim/src/ensemble.rs @@ -0,0 +1,186 @@ +//! 被控对象集成：用同一组参数的**抖动样本**表达模型不确定性。 +//! +//! 单点模型整定出来的增益往往对增益 / 时间常数 / 延迟 / 摩擦高度敏感 +//! （真车轮子之间、电量与温度变化都会漂移）。整定应针对一个**集成**做 +//! 鲁棒优化，而不是针对一条理想曲线做最优化。 + +use rand::Rng as _; +use rand::SeedableRng as _; +use rand::rngs::StdRng; + +use crate::plant::PlantParams; + +/// 集成相对基准参数的抖动幅度（比例，0 表示不抖动）。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct PlantTolerances { + /// 空载增益 ± 比例。 + pub gain_pct: f32, + /// 时间常数 ± 比例。 + pub tau_pct: f32, + /// 命令延迟在 `0..=delay_ticks_max` 拍内均匀取整。 + pub delay_ticks_max: u32, + /// 摩擦三件套 ± 比例。 + pub friction_pct: f32, +} + +impl Default for PlantTolerances { + fn default() -> Self { + Self { gain_pct: 0.2, tau_pct: 0.3, delay_ticks_max: 3, friction_pct: 0.5 } + } +} + +/// 一组被控对象参数（成员之间只有参数差异，模型结构相同）。 +#[derive(Debug, Clone, PartialEq)] +pub struct PlantEnsemble { + members: Vec, +} + +impl PlantEnsemble { + /// `members` 个完全相同的样本（下限 1）。 + #[must_use] + pub fn homogeneous(base: PlantParams, members: usize) -> Self { + Self { members: vec![base; members.max(1)] } + } + + /// 以 `base` 为基准做确定性抖动。 + /// + /// 第 `i` 个成员的随机流由 `seed` 与 `i` 派生——**与线程调度无关**， + /// 因此整定结果可复现。`members` 下限 1（首个成员恰好是基准参数本身， + /// 保证集成里始终包含"名义模型"这一样本）。 + #[must_use] + pub fn randomized( + base: PlantParams, + tolerances: PlantTolerances, + members: usize, + seed: u64, + ) -> Self { + let members = members.max(1); + let mut out = Vec::with_capacity(members); + out.push(base); + for index in 1..members { + let mut rng = StdRng::seed_from_u64(member_seed(seed, index)); + let mut params = base; + params.gain_rpm_per_raw = base.gain_rpm_per_raw * scale(&mut rng, tolerances.gain_pct); + params.tau_s = (base.tau_s * scale(&mut rng, tolerances.tau_pct)).max(1.0e-4); + if tolerances.delay_ticks_max > 0 { + params.delay_ticks = rng.random_range(0..=tolerances.delay_ticks_max); + } + let friction = scale(&mut rng, tolerances.friction_pct); + params.static_raw = base.static_raw * friction; + params.coulomb_raw = base.coulomb_raw * friction; + params.viscous_raw_per_rpm = base.viscous_raw_per_rpm * friction; + out.push(params); + } + Self { members: out } + } + + /// 成员数。 + #[must_use] + pub fn len(&self) -> usize { + self.members.len() + } + + /// 是否为空（构造时已保证 ≥ 1，恒为 `false`）。 + #[must_use] + pub fn is_empty(&self) -> bool { + self.members.is_empty() + } + + /// 成员参数。 + #[must_use] + pub fn members(&self) -> &[PlantParams] { + &self.members + } + + /// 逐成员迭代。 + pub fn iter(&self) -> std::slice::Iter<'_, PlantParams> { + self.members.iter() + } +} + +impl<'a> IntoIterator for &'a PlantEnsemble { + type Item = &'a PlantParams; + type IntoIter = std::slice::Iter<'a, PlantParams>; + + fn into_iter(self) -> Self::IntoIter { + self.members.iter() + } +} + +/// 成员的随机种子：黄金比例常数散列，避免相邻种子产生相关样本。 +fn member_seed(seed: u64, index: usize) -> u64 { + seed ^ (index as u64).wrapping_mul(0x9E37_79B9_7F4A_7C15) +} + +/// 在 `[1 − pct, 1 + pct]` 内均匀取一个比例（`pct < 0` 视为 0）。 +fn scale(rng: &mut StdRng, pct: f32) -> f32 { + let pct = pct.max(0.0); + if pct <= 0.0 { + return 1.0; + } + rng.random_range(1.0 - pct..=1.0 + pct) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn homogeneous_repeats_the_same_params() { + let ensemble = PlantEnsemble::homogeneous(PlantParams::gm3508(), 4); + assert_eq!(ensemble.len(), 4); + assert!(ensemble.iter().all(|p| *p == PlantParams::gm3508())); + } + + #[test] + fn homogeneous_never_empty() { + assert_eq!(PlantEnsemble::homogeneous(PlantParams::gm3508(), 0).len(), 1); + } + + #[test] + fn randomized_is_deterministic_and_includes_nominal() { + let base = PlantParams::gm3508_bench(); + let tol = PlantTolerances::default(); + let a = PlantEnsemble::randomized(base, tol, 8, 42); + let b = PlantEnsemble::randomized(base, tol, 8, 42); + assert_eq!(a, b, "同 seed 必须逐位一致"); + assert_eq!(a.members()[0], base, "首个成员是名义模型"); + } + + #[test] + fn different_seeds_differ() { + let base = PlantParams::gm3508_bench(); + let tol = PlantTolerances::default(); + let a = PlantEnsemble::randomized(base, tol, 8, 1); + let b = PlantEnsemble::randomized(base, tol, 8, 2); + assert_ne!(a, b); + } + + #[test] + fn jitter_stays_within_tolerances() { + let base = PlantParams::gm3508_bench(); + let tol = + PlantTolerances { gain_pct: 0.2, tau_pct: 0.3, delay_ticks_max: 3, friction_pct: 0.5 }; + let ensemble = PlantEnsemble::randomized(base, tol, 64, 7); + for member in &ensemble { + let gain_ratio = member.gain_rpm_per_raw / base.gain_rpm_per_raw; + assert!((0.8..=1.2).contains(&gain_ratio), "gain ratio {gain_ratio}"); + let tau_ratio = member.tau_s / base.tau_s; + assert!((0.7..=1.3).contains(&tau_ratio), "tau ratio {tau_ratio}"); + assert!(member.delay_ticks <= 3); + let friction_ratio = member.coulomb_raw / base.coulomb_raw; + assert!((0.5..=1.5).contains(&friction_ratio), "friction ratio {friction_ratio}"); + } + // 抖动确实发生（不是所有成员都等于基准）。 + assert!(ensemble.iter().skip(1).any(|p| *p != base)); + } + + #[test] + fn zero_tolerances_collapse_to_nominal() { + let base = PlantParams::gm3508_bench(); + let tol = + PlantTolerances { gain_pct: 0.0, tau_pct: 0.0, delay_ticks_max: 0, friction_pct: 0.0 }; + let ensemble = PlantEnsemble::randomized(base, tol, 4, 3); + assert!(ensemble.iter().all(|p| *p == base)); + } +} diff --git a/crates/dji-esc-sim/src/harness.rs b/crates/dji-esc-sim/src/harness.rs new file mode 100644 index 0000000..7fcdbdb --- /dev/null +++ b/crates/dji-esc-sim/src/harness.rs @@ -0,0 +1,258 @@ +//! 离线场景评估：同步、无 I/O、无 async 的闭环重放。 +//! +//! 本模块是自动整定的**求值内核**：给定一组速度环增益、一个被控对象与一条 +//! 场景，逐拍推进「控制器 → 限幅 → 被控对象」，返回完整 [`Trace`]。 +//! +//! 与 `dji-esc` 的 `EscBus` 刻意保持**同一套物理帧语义**（见 +//! `tests/harness_parity.rs`）： +//! +//! ```text +//! out = velocity_controller.update(setpoint, measured, dt) // 已按 ±current_limit 限幅 +//! raw = (out * direction) as i16 // 物理帧（direction = +1） +//! cmd = clamp_current(model, raw.clamp(±current_limit)) // 电调满量程再夹一次 +//! ``` +//! +//! 单次评估约 10^3~10^4 拍纯浮点运算，因此可以在 `rayon` 下并行搜索 +//! 上千组参数——这是"参数空间搜索"能落地的前提。 + +use dji_esc::{EscModel, clamp_current}; +use motor_core::{VelocityController, VelocityGains}; +use thiserror::Error; + +use crate::ensemble::PlantEnsemble; +use crate::plant::{PlantMotor, PlantParams}; +use crate::scenario::Scenario; +use crate::trace::Trace; + +/// 闭环限幅与电调型号（决定 effort 的可达范围）。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct LoopLimits { + /// effort 限幅（raw）——同时是 PID 输出钳位点。 + pub current_limit_raw: i16, + /// 电调型号（决定满量程）。 + pub model: EscModel, +} + +impl LoopLimits { + /// `GM3508 + C620`、限流 12000 raw（≈14.6 A）——与 `EscConfig::gm3508` 一致。 + #[must_use] + pub const fn gm3508() -> Self { + Self { current_limit_raw: 12_000, model: EscModel::C620 } + } +} + +impl Default for LoopLimits { + fn default() -> Self { + Self::gm3508() + } +} + +/// 离线评估失败原因。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)] +pub enum SimError { + /// 增益含 NaN / inf（`Pid::new` 会 panic，必须在此拦下）。 + #[error("gains contain NaN or infinity")] + NonFiniteGains, + /// 场景或限幅退化（`dt <= 0`、总时长为 0、限流为 0 等）。 + #[error("degenerate scenario or limits")] + DegenerateScenario, + /// 数值发散（转速超出被控对象量程 100 倍）。 + #[error("simulation diverged at step {step}")] + Diverged { + /// 发散发生的采样序号。 + step: usize, + }, +} + +/// 在单台被控对象上重放一条场景。 +/// +/// # Errors +/// +/// 增益非有限、场景退化或数值发散时返回 [`SimError`]——调用方（整定器） +/// 应把它折算成罚值，而不是把非法参数交给控制器。 +pub fn simulate( + gains: VelocityGains, + plant: &PlantParams, + scenario: &Scenario, + limits: LoopLimits, +) -> Result { + if !gains.is_finite() { + return Err(SimError::NonFiniteGains); + } + let dt = scenario.dt_s; + let total = scenario.total_s(); + let dt_ok = dt.is_finite() && dt > 0.0; + let total_ok = total.is_finite() && total > 0.0; + let target_ok = scenario.target_rpm.is_finite(); + if !dt_ok || !total_ok || !target_ok || limits.current_limit_raw <= 0 { + return Err(SimError::DegenerateScenario); + } + + let mut controller = VelocityController::from_gains(limits.current_limit_raw, gains); + let mut motor = PlantMotor::new(*plant); + let bound = plant.max_rpm.abs().max(1.0) * 100.0; + let steps = scenario.samples(); + let mut trace = Trace::with_capacity(steps); + + for step in 0..steps { + let t_s = step as f32 * dt; + let setpoint = scenario.setpoint_at(t_s); + let measured = motor.speed_rpm(); + + let out = controller.update(setpoint, measured, dt); + // 物理帧：direction = +1（安装方向由驱动层负责翻转）。 + let raw = out as i16; + let command = clamp_current( + limits.model, + raw.clamp(-limits.current_limit_raw, limits.current_limit_raw), + ); + let saturated = raw.unsigned_abs() >= limits.current_limit_raw.unsigned_abs(); + + let feedback = motor.step(command, dt); + trace.push(t_s, setpoint, f32::from(feedback.speed_rpm), command, saturated); + + if !motor.speed_rpm().is_finite() || motor.speed_rpm().abs() > bound { + return Err(SimError::Diverged { step }); + } + } + Ok(trace) +} + +/// 在一组被控对象上重放同一条场景（集成鲁棒性评估）。 +/// +/// 返回 `(成员参数, 结果)` 列表；单个成员的失败不影响其他成员（结果以 +/// `Result` 逐项保留，由调用方决定如何聚合）。 +#[must_use] +pub fn simulate_ensemble( + gains: VelocityGains, + ensemble: &PlantEnsemble, + scenario: &Scenario, + limits: LoopLimits, +) -> Vec<(PlantParams, Result)> { + ensemble.iter().map(|plant| (*plant, simulate(gains, plant, scenario, limits))).collect() +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::scenario::SetpointProfile; + + const GAINS: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0); + const STEP: Scenario = Scenario::step(3000.0, 1.0); + + #[test] + fn ideal_plant_tracks_step() { + let trace = simulate(GAINS, &PlantParams::gm3508(), &STEP, LoopLimits::gm3508()).unwrap(); + assert!(trace.is_consistent()); + assert_eq!(trace.len(), 1000); + let tail = trace.window(0.5, 1.0); + assert!((tail.mean_rpm() - 3000.0).abs() < 30.0, "steady state = {}", tail.mean_rpm()); + } + + #[test] + fn non_finite_gains_are_rejected_not_panicking() { + let bad = VelocityGains::new(f32::NAN, 0.0, 0.0, 0.0); + assert_eq!( + simulate(bad, &PlantParams::gm3508(), &STEP, LoopLimits::gm3508()).err(), + Some(SimError::NonFiniteGains) + ); + let infinite = VelocityGains::new(1.0, f32::INFINITY, 0.0, 0.0); + assert_eq!( + simulate(infinite, &PlantParams::gm3508(), &STEP, LoopLimits::gm3508()).err(), + Some(SimError::NonFiniteGains) + ); + } + + #[test] + fn degenerate_scenario_is_rejected() { + let zero_limit = LoopLimits { current_limit_raw: 0, model: EscModel::C620 }; + assert_eq!( + simulate(GAINS, &PlantParams::gm3508(), &STEP, zero_limit).err(), + Some(SimError::DegenerateScenario) + ); + let zero_dt = STEP.with_dt_s(0.0); + assert_eq!( + simulate(GAINS, &PlantParams::gm3508(), &zero_dt, LoopLimits::gm3508()).err(), + Some(SimError::DegenerateScenario) + ); + } + + #[test] + fn huge_gains_diverge_and_are_reported() { + let aggressive = VelocityGains::new(1.0e6, 1.0e6, 0.0, 0.0); + let long = Scenario::step(3000.0, 5.0); + let result = simulate(aggressive, &PlantParams::gm3508(), &long, LoopLimits::gm3508()); + match result { + Err(SimError::Diverged { .. }) => {} + Err(SimError::NonFiniteGains | SimError::DegenerateScenario) => { + panic!("expected divergence") + } + Ok(trace) => { + // 有限但剧烈振荡：至少必须反映在饱和与高转速上。 + assert!(trace.peak_rpm() > 3000.0); + assert!(trace.saturated_fraction() > 0.0); + } + } + } + + #[test] + fn output_respects_current_limit() { + let limits = LoopLimits { current_limit_raw: 5_000, model: EscModel::C620 }; + let trace = simulate( + VelocityGains::new(50.0, 10.0, 0.0, 0.0), + &PlantParams::gm3508(), + &STEP, + limits, + ) + .unwrap(); + assert!( + trace.effort_raw.iter().all(|raw| raw.abs() <= 5_000), + "effort must stay inside the limit" + ); + assert!(trace.saturated_fraction() > 0.0, "大误差下应当饱和"); + } + + #[test] + fn c610_full_scale_clamps_below_configured_limit() { + let limits = LoopLimits { current_limit_raw: 12_000, model: EscModel::C610 }; + let trace = simulate( + VelocityGains::new(1000.0, 0.0, 0.0, 0.0), + &PlantParams::gm3508(), + &Scenario::step(9000.0, 0.2), + limits, + ) + .unwrap(); + assert!(trace.effort_raw.iter().all(|raw| raw.abs() <= EscModel::C610.max_current_raw())); + } + + #[test] + fn ensemble_runs_every_member() { + let ensemble = PlantEnsemble::homogeneous(PlantParams::gm3508(), 3); + let results = simulate_ensemble(GAINS, &ensemble, &STEP, LoopLimits::gm3508()); + assert_eq!(results.len(), 3); + assert!(results.iter().all(|(_, result)| result.is_ok())); + } + + #[test] + fn updown_profile_returns_to_rest() { + let scenario = + Scenario::ramp_and_hold(2000.0, 0.2, 0.3).with_profile(SetpointProfile::UpDown); + let trace = + simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap(); + let last = trace.rpm.last().copied().unwrap_or_default(); + assert!(last.abs() < 100.0, "final rpm = {last}"); + } + + #[test] + fn friction_plant_needs_more_effort_than_ideal() { + let scenario = Scenario::ramp_and_hold(1000.0, 0.2, 1.0); + let ideal = + simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()).unwrap(); + let bench = + simulate(GAINS, &PlantParams::gm3508_bench(), &scenario, LoopLimits::gm3508()).unwrap(); + assert!( + bench.window(0.6, 1.2).mean_rpm() <= ideal.window(0.6, 1.2).mean_rpm(), + "摩擦 + 延迟只会拖慢跟随" + ); + } +} diff --git a/crates/dji-esc-sim/src/lib.rs b/crates/dji-esc-sim/src/lib.rs new file mode 100644 index 0000000..68147fe --- /dev/null +++ b/crates/dji-esc-sim/src/lib.rs @@ -0,0 +1,39 @@ +//! DJI 电调被控对象仿真：模型、场景与**离线评估 harness**。 +//! +//! 本 crate 从 `dji-esc` 剥离而来，只负责"电调对面的那台电机"以及如何对它 +//! 做实验，不碰任何真实 I/O——总线接口是一对 `CanLink` 通道。 +//! +//! # 模块职责 +//! +//! - [`plant`]：被控对象模型（一阶转速 + 静/库仑/黏滞摩擦 + 命令延迟 + 负载）； +//! - [`ensemble`]：参数集成（域随机化），表达模型不确定性； +//! - [`scenario`]：设定值曲线与采样节拍（阶跃 / 斜坡 / 阶梯 / 升降）； +//! - [`trace`]：试验轨迹（设定值、转速、effort、饱和标志）； +//! - [`harness`]：**同步**闭环重放——自动整定的求值内核（纯 CPU、可并行）； +//! - [`rig`]：**异步**假电调组，挂在 [`CanLink`] 上供 `EscBus` 联调。 +//! +//! # 两条路径为什么都要有 +//! +//! - `rig` 用于端到端联调（`chassisd --mock`、闭环集成测试），语义是 +//! "挂在总线上的假电调"； +//! - `harness` 用于参数搜索，语义是"一条场景的数值积分"，单次评估只有 +//! 浮点运算、没有 tokio 任务与通道，因此可以在 `rayon` 下并行跑上千组 +//! 参数。两者的物理帧语义由 `tests/harness_parity.rs` 对齐。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod ensemble; +pub mod harness; +pub mod plant; +pub mod rig; +pub mod scenario; +pub mod trace; + +pub use ensemble::{PlantEnsemble, PlantTolerances}; +pub use harness::{LoopLimits, SimError, simulate, simulate_ensemble}; +pub use plant::{PlantMotor, PlantParams}; +pub use rig::{spawn_plant, spawn_plant_with}; +pub use scenario::{Scenario, SetpointProfile}; +pub use trace::{Trace, Window}; + +/// 复用的链路类型（`dji-esc` 的协议层与假电调通过它对接）。 +pub use can_link::{CanFrame, CanLink}; diff --git a/crates/dji-esc-sim/src/plant.rs b/crates/dji-esc-sim/src/plant.rs new file mode 100644 index 0000000..e164d21 --- /dev/null +++ b/crates/dji-esc-sim/src/plant.rs @@ -0,0 +1,361 @@ +//! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。 +//! +//! 电调坐标系（正电流 → 正转速），逐拍离散（`dt` 秒）： +//! +//! 1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才真正作用（模拟 +//! 链路 + 采样延迟；`0` 表示立即作用）。 +//! 2. **摩擦**：先由 `u` 与当前运动状态算出净转矩输入 +//! （静摩擦死区 → 库仑摩擦 → 黏滞阻尼，见 [`PlantMotor::step`]）。 +//! 3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`， +//! 转速以时间常数 `tau` 一阶趋近（`alpha = 1 − exp(−dt/tau)`）。 +//! 4. **编码器积分与量化**：机械角按转速积分并对 8192 取模；反馈帧是 +//! `EscFeedback`，因此角度被截断为 `u16`、转速被截断为 `i16`——量化是 +//! 协议类型自带的，不需要额外开关。 +//! +//! [`PlantParams::gm3508`] 复现最初的**无摩擦一阶模型**（`chassisd --mock` +//! 与闭环测试依赖它，行为逐位不变）；[`PlantParams::gm3508_bench`] 是带摩擦 +//! 与延迟的**台架量级**预设，供自动整定使用。 + +use std::collections::VecDeque; + +use dji_esc::{ENCODER_TICKS, EscFeedback}; + +/// 转速绝对值低于该值（rpm）视为静止——静摩擦判别用。 +const STICK_RPM_EPS: f32 = 1.0; + +/// 被控对象参数（电调坐标系）。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct PlantParams { + /// 空载增益（rpm / raw）：`GM3508` 实测约 0.565（≈ 9254 / 16384）。 + pub gain_rpm_per_raw: f32, + /// 机电时间常数（秒）。 + pub tau_s: f32, + /// 转速上限（rpm）。 + pub max_rpm: f32, + /// 静摩擦阈值（raw）：静止且 |u| ≤ 该值时不产生任何运动（stick）。 + pub static_raw: f32, + /// 库仑摩擦（raw）：与运动方向相反的恒定阻力。 + pub coulomb_raw: f32, + /// 黏滞阻尼（raw / rpm）。 + pub viscous_raw_per_rpm: f32, + /// 命令传输延迟（拍，`dt` 的整数倍）。 + pub delay_ticks: u32, + /// 恒定外载（折算成 rpm，直接被目标转速减去）。 + pub load_rpm: f32, +} + +impl PlantParams { + /// 无摩擦、无延迟的理想一阶模型（与历史 `PlantMotor::gm3508` 逐位一致）。 + #[must_use] + pub const fn gm3508() -> Self { + Self { + gain_rpm_per_raw: 9254.0 / 16384.0, + tau_s: 0.12, + max_rpm: 9254.0, + static_raw: 0.0, + coulomb_raw: 0.0, + viscous_raw_per_rpm: 0.0, + delay_ticks: 0, + load_rpm: 0.0, + } + } + + /// 台架量级预设：在 [`Self::gm3508`] 上叠加静摩擦 / 库仑摩擦 / 黏滞阻尼 + /// 与 2 拍命令延迟。 + /// + /// 摩擦量级取自 `docs/architecture.md` 与台架记录的空载实测（起动需 + /// 400~600 raw），**是量级估计而非辨识结果**；需要更贴合实车时可用 + /// [`Self::with_friction`] / [`Self::with_delay_ticks`] 覆盖。 + #[must_use] + pub const fn gm3508_bench() -> Self { + let mut params = Self::gm3508(); + params.static_raw = 450.0; + params.coulomb_raw = 500.0; + params.viscous_raw_per_rpm = 0.05; + params.delay_ticks = 2; + params + } + + /// 设置摩擦三件套（静摩擦阈值 / 库仑 / 黏滞）。 + #[must_use] + pub const fn with_friction( + mut self, + static_raw: f32, + coulomb_raw: f32, + viscous_raw_per_rpm: f32, + ) -> Self { + self.static_raw = static_raw; + self.coulomb_raw = coulomb_raw; + self.viscous_raw_per_rpm = viscous_raw_per_rpm; + self + } + + /// 设置命令延迟（拍）。 + #[must_use] + pub const fn with_delay_ticks(mut self, ticks: u32) -> Self { + self.delay_ticks = ticks; + self + } + + /// 设置恒定外载（rpm）。 + #[must_use] + pub const fn with_load_rpm(mut self, rpm: f32) -> Self { + self.load_rpm = rpm; + self + } +} + +/// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。 +#[derive(Debug, Clone)] +pub struct PlantMotor { + params: PlantParams, + speed_rpm: f32, + angle_raw: f32, + /// 命令延迟 FIFO（`delay_ticks == 0` 时恒为空）。 + delay_line: VecDeque, +} + +impl PlantMotor { + /// 用给定参数创建静止的电机。 + #[must_use] + pub fn new(params: PlantParams) -> Self { + Self { params, speed_rpm: 0.0, angle_raw: 0.0, delay_line: VecDeque::new() } + } + + /// 理想 `GM3508`（无摩擦、无延迟）——历史默认，`--mock` 与闭环测试沿用。 + #[must_use] + pub fn gm3508() -> Self { + Self::new(PlantParams::gm3508()) + } + + /// 当前参数。 + #[must_use] + pub const fn params(&self) -> &PlantParams { + &self.params + } + + /// 当前转子转速（rpm，电调侧）。 + #[must_use] + pub const fn speed_rpm(&self) -> f32 { + self.speed_rpm + } + + /// 当前机械角（编码器计数，`0..8192`）。 + #[must_use] + pub const fn angle_raw(&self) -> f32 { + self.angle_raw + } + + /// 回到静止初态（保留参数）。 + pub fn reset(&mut self) { + self.speed_rpm = 0.0; + self.angle_raw = 0.0; + self.delay_line.clear(); + } + + /// 推进一个时间步，返回本步反馈帧内容。 + /// + /// 摩擦模型（`net` 即折算到电流量纲的净驱动量）： + /// + /// - 运动时（`|rpm| > 1`）：`net = u − sign(rpm)·coulomb − viscous·rpm`； + /// - 静止但 `|u| > static`：`net = u − sign(u)·coulomb`（突破静摩擦）； + /// - 静止且 `|u| ≤ static`：`net = 0`（粘住）。 + /// + /// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`， + /// 即历史的一阶模型。 + pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback { + let applied = self.apply_delay(f32::from(current_raw)); + let net = self.net_torque_raw(applied); + let target = (net * self.params.gain_rpm_per_raw) + .clamp(-self.params.max_rpm, self.params.max_rpm) + - self.params.load_rpm; + if self.params.tau_s > 0.0 && dt.is_finite() && dt > 0.0 { + let alpha = 1.0 - (-dt / self.params.tau_s).exp(); + self.speed_rpm += (target - self.speed_rpm) * alpha; + } else { + // 非正时间常数 = 无惯性：立即到位（防御非法 dt）。 + self.speed_rpm = target; + } + let ticks = f32::from(ENCODER_TICKS); + self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * ticks * dt).rem_euclid(ticks); + EscFeedback { + angle_raw: self.angle_raw as u16, + speed_rpm: self.speed_rpm as i16, + current_raw, + temperature_c: 30, + } + } + + /// 命令延迟：返回 `delay_ticks` 拍之前写入的命令（未填满时返回 0）。 + fn apply_delay(&mut self, u: f32) -> f32 { + let depth = self.params.delay_ticks as usize; + if depth == 0 { + return u; + } + self.delay_line.push_back(u); + while self.delay_line.len() > depth { + if let Some(delayed) = self.delay_line.pop_front() { + return delayed; + } + } + 0.0 + } + + /// 折算到电流量纲的净驱动量（含静/库仑/黏滞摩擦）。 + fn net_torque_raw(&self, u: f32) -> f32 { + let (static_raw, coulomb_raw, viscous) = + (self.params.static_raw, self.params.coulomb_raw, self.params.viscous_raw_per_rpm); + if self.speed_rpm.abs() > STICK_RPM_EPS { + let dir = if self.speed_rpm > 0.0 { 1.0 } else { -1.0 }; + u - dir * coulomb_raw - viscous * self.speed_rpm + } else if u.abs() > static_raw { + let dir = if u > 0.0 { 1.0 } else { -1.0 }; + u - dir * coulomb_raw + } else { + 0.0 + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const DT: f32 = 0.001; + + /// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。 + #[track_caller] + fn assert_close(actual: f32, expected: f32, what: &str) { + assert!((actual - expected).abs() < 1e-3, "{what}: actual {actual}, expected {expected}"); + } + + #[test] + fn ideal_model_matches_legacy_first_order_response() { + // 与历史实现（无摩擦一阶模型）逐位一致的黄金值。 + let mut plant = PlantMotor::gm3508(); + let gain = 9254.0 / 16384.0; + let mut expected = 0.0f32; + for _ in 0..50 { + let target = (10_000f32 * gain).clamp(-9254.0, 9254.0); + let alpha = 1.0 - (-DT / 0.12).exp(); + expected += (target - expected) * alpha; + let fb = plant.step(10_000, DT); + assert_eq!(fb.speed_rpm, expected as i16, "legacy step mismatch"); + } + // 50 拍（50 ms）只走完约 34% 的一阶响应 → 约 1925 rpm（远未到稳态）。 + // 精确数值由上面的逐拍比对保证，这里只锁定量级。 + assert!((1900.0..1950.0).contains(&plant.speed_rpm()), "rpm = {}", plant.speed_rpm()); + } + + #[test] + fn zero_current_stays_still() { + let mut plant = PlantMotor::gm3508(); + for _ in 0..100 { + let fb = plant.step(0, DT); + assert_eq!(fb.speed_rpm, 0); + } + assert!((plant.angle_raw()).abs() < f32::EPSILON); + } + + #[test] + fn static_friction_holds_below_threshold() { + let params = PlantParams::gm3508().with_friction(450.0, 500.0, 0.0); + let mut plant = PlantMotor::new(params); + for _ in 0..1000 { + plant.step(400, DT); + } + assert_close(plant.speed_rpm(), 0.0, "低于静摩擦阈值的电流不得产生运动"); + } + + #[test] + fn breakaway_above_static_threshold_moves() { + let params = PlantParams::gm3508().with_friction(450.0, 500.0, 0.0); + let mut plant = PlantMotor::new(params); + for _ in 0..1000 { + plant.step(1000, DT); + } + // 净驱动 = 1000 − 500 = 500 raw → 稳态 ≈ 500 × 0.565 = 282 rpm。 + let expected = 500.0 * (9254.0 / 16384.0); + assert!( + (plant.speed_rpm() - expected).abs() < 5.0, + "rpm = {} expected ≈ {expected}", + plant.speed_rpm() + ); + } + + #[test] + fn coulomb_friction_opposes_motion_in_both_directions() { + let params = PlantParams::gm3508().with_friction(0.0, 500.0, 0.0); + let mut forward = PlantMotor::new(params); + let mut backward = PlantMotor::new(params); + for _ in 0..2000 { + forward.step(2000, DT); + backward.step(-2000, DT); + } + assert!(forward.speed_rpm() > 0.0); + assert!(backward.speed_rpm() < 0.0); + assert!((forward.speed_rpm() + backward.speed_rpm()).abs() < 1.0, "对称性"); + } + + #[test] + fn delay_shifts_response_by_exact_ticks() { + let immediate = PlantParams::gm3508(); + let delayed = PlantParams::gm3508().with_delay_ticks(3); + let mut a = PlantMotor::new(immediate); + let mut b = PlantMotor::new(delayed); + let mut a_trace = Vec::new(); + let mut b_trace = Vec::new(); + for _ in 0..100 { + a_trace.push(a.step(6000, DT).speed_rpm); + b_trace.push(b.step(6000, DT).speed_rpm); + } + // 延迟 3 拍：前 3 拍反馈恒为 0，之后 b[i] == a[i-3]。 + assert_eq!(&b_trace[..3], &[0, 0, 0]); + for i in 3..100 { + assert_eq!(b_trace[i], a_trace[i - 3], "tick {i}"); + } + } + + #[test] + fn load_rpm_offsets_steady_state() { + let unloaded = PlantParams::gm3508(); + let loaded = unloaded.with_load_rpm(200.0); + let mut a = PlantMotor::new(unloaded); + let mut b = PlantMotor::new(loaded); + for _ in 0..5000 { + a.step(16384, DT); + b.step(16384, DT); + } + assert!((a.speed_rpm() - 9254.0).abs() < 0.1, "空载稳态应顶到上限: {}", a.speed_rpm()); + assert!( + (b.speed_rpm() - 9054.0).abs() < 0.1, + "外载直接把稳态拉低 200 rpm: {}", + b.speed_rpm() + ); + } + + #[test] + fn feedback_is_quantized_by_protocol_types() { + let mut plant = PlantMotor::gm3508(); + for _ in 0..10_000 { + let fb = plant.step(9000, DT); + assert!(fb.angle_raw < 8192, "angle must stay inside one revolution"); + } + let fb = plant.step(9000, DT); + // 内部转速是浮点，反馈按协议被截断为 i16——量化由类型自带。 + assert_eq!(fb.speed_rpm, plant.speed_rpm() as i16); + assert_eq!(fb.temperature_c, 30); + } + + #[test] + fn reset_returns_to_rest() { + let mut plant = PlantMotor::gm3508(); + for _ in 0..100 { + plant.step(8000, DT); + } + assert!(plant.speed_rpm() > 0.0); + plant.reset(); + assert_close(plant.speed_rpm(), 0.0, "复位后转速归零"); + assert_close(plant.angle_raw(), 0.0, "复位后角度归零"); + } +} diff --git a/crates/dji-esc-sim/src/rig.rs b/crates/dji-esc-sim/src/rig.rs new file mode 100644 index 0000000..f81b5c9 --- /dev/null +++ b/crates/dji-esc-sim/src/rig.rs @@ -0,0 +1,147 @@ +//! 异步 plant rig：把被控对象挂到一对 [`CanLink`] 通道上。 +//! +//! 与真实电调对调位置的"假电调"：消费命令帧（`0x200` / `0x1FF`），按 +//! `tick` 节拍产出反馈帧。`EscBus` 完全看不出区别，因此 +//! **协议 → 驱动 → 控制回路**的整条链路都能在无硬件环境下跑通 +//! （`chassisd --mock`、闭环集成测试）。 + +use std::collections::HashMap; +use std::time::Duration; + +use dji_esc::{ + COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink, encode_feedback_frame, +}; +use tokio::task::JoinHandle; +use tokio::time::{MissedTickBehavior, interval}; +use tracing::trace; + +use crate::plant::{PlantMotor, PlantParams}; + +/// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。 +/// +/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可（丢弃句柄 = 让任务独立运行）。 +pub fn spawn_plant(link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> { + let specs = esc_ids.iter().map(|&id| (id, PlantParams::gm3508())); + spawn_plant_with(link, specs, tick) +} + +/// 用逐电调的参数启动假电调组（异构轮子 / 域随机化场景）。 +pub fn spawn_plant_with( + mut link: CanLink, + specs: impl IntoIterator, + tick: Duration, +) -> JoinHandle<()> { + let specs: Vec<(u8, PlantParams)> = specs.into_iter().collect(); + let mut motors: HashMap = + specs.iter().map(|&(id, params)| (id, PlantMotor::new(params))).collect(); + let mut commands: HashMap = specs.iter().map(|&(id, _)| (id, 0)).collect(); + let dt = tick.as_secs_f32(); + let mut ticker = interval(tick); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + + tokio::spawn(async move { + loop { + tokio::select! { + frame = link.incoming_rx.recv() => match frame { + Some(frame) => apply_command_frame(&frame, &mut commands), + None => break, + }, + _ = ticker.tick() => { + for (&id, motor) in &mut motors { + let cmd = commands.get(&id).copied().unwrap_or(0); + let feedback = motor.step(cmd, dt); + if link + .outgoing_tx + .send(encode_feedback_frame(id, &feedback)) + .await + .is_err() + { + return; + } + } + } + } + } + trace!("plant exiting"); + }) +} + +/// 把一帧组控制帧拆到各电调的当前命令上（非命令帧忽略）。 +fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap) { + let group_start = match frame.id { + COMMAND_FRAME_ID_PRIMARY => 1u8, + COMMAND_FRAME_ID_SECONDARY => 5u8, + _ => return, + }; + for slot in 0..4u8 { + if let Some(cmd) = commands.get_mut(&(group_start + slot)) { + *cmd = i16::from_be_bytes([ + frame.data[(slot * 2) as usize], + frame.data[(slot * 2 + 1) as usize], + ]); + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use dji_esc::{COMMAND_FRAME_ID_PRIMARY, decode_feedback}; + use tokio::time::timeout; + + #[test] + fn command_frame_splits_into_slots() { + let mut commands: HashMap = (1..=8).map(|id| (id, 0)).collect(); + let frame = CanFrame::new(COMMAND_FRAME_ID_PRIMARY, [0x04, 0xD2, 0xFD, 0xC9, 0, 0, 0, 0]); + apply_command_frame(&frame, &mut commands); + assert_eq!(commands[&1], 1234); + assert_eq!(commands[&2], -567); + assert_eq!(commands[&3], 0); + // 副组帧只影响电调 5–8：槽位 2 → 电调 7，数据取自字节 4..6。 + let secondary = CanFrame::new(COMMAND_FRAME_ID_SECONDARY, [0, 0, 0, 0, 0, 1, 0, 0]); + apply_command_frame(&secondary, &mut commands); + assert_eq!(commands[&5], 0); + assert_eq!(commands[&7], 1); + assert_eq!(commands[&1], 1234, "副组帧不得影响主组"); + // 非命令帧被忽略。 + apply_command_frame(&CanFrame::new(0x999, [0xFF; 8]), &mut commands); + assert_eq!(commands[&1], 1234); + } + + #[tokio::test(start_paused = true)] + async fn plant_answers_with_moving_feedback() { + let (mut bus_side, plant_side) = CanLink::pair(64); + let plant = spawn_plant(plant_side, &[1], Duration::from_millis(1)); + // 下发 10000 raw，一阶模型必然转起来。 + let command = CanFrame::new(COMMAND_FRAME_ID_PRIMARY, [0x27, 0x10, 0, 0, 0, 0, 0, 0]); + bus_side.outgoing_tx.send(command).await.expect("link open"); + + let mut saw_positive = false; + for _ in 0..20 { + let frame = timeout(Duration::from_millis(50), bus_side.incoming_rx.recv()) + .await + .expect("plant must keep emitting feedback") + .expect("link open"); + let (id, feedback) = decode_feedback(&frame).expect("feedback frame"); + assert_eq!(id, 1); + saw_positive |= feedback.speed_rpm > 0; + } + assert!(saw_positive, "假电调必须按命令转起来"); + plant.abort(); + } + + #[tokio::test(start_paused = true)] + async fn plant_with_custom_params_emits_feedback_during_delay() { + let (mut bus_side, plant_side) = CanLink::pair(64); + let specs = vec![(2u8, PlantParams::gm3508_bench())]; + let plant = spawn_plant_with(plant_side, specs, Duration::from_millis(1)); + let frame = timeout(Duration::from_millis(50), bus_side.incoming_rx.recv()) + .await + .expect("延迟期间也应持续产出反馈帧") + .expect("link open"); + let (id, feedback) = decode_feedback(&frame).expect("feedback frame"); + assert_eq!(id, 2); + assert_eq!(feedback.speed_rpm, 0); + plant.abort(); + } +} diff --git a/crates/dji-esc-sim/src/scenario.rs b/crates/dji-esc-sim/src/scenario.rs new file mode 100644 index 0000000..b9eaf67 --- /dev/null +++ b/crates/dji-esc-sim/src/scenario.rs @@ -0,0 +1,236 @@ +//! 场景定义：设定值曲线 + 采样节拍。 +//! +//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、 +//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放， +//! 因此整定结果可以跨后端比较。 + +/// 设定值曲线。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum SetpointProfile { + /// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。 + Step, + /// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。 + RampAndHold, + /// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。 + Staircase { + /// 阶梯数（≥ 1）。 + levels: u32, + }, + /// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。 + UpDown, +} + +/// 一次试验的设定值与时间规格。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Scenario { + /// 控制 / 采样节拍（秒）。 + pub dt_s: f32, + /// 斜坡时间（秒）；`0` 表示瞬时。 + pub ramp_s: f32, + /// 保持时间（秒）。 + pub hold_s: f32, + /// 目标转子转速（rpm）。 + pub target_rpm: f32, + /// 设定值曲线。 + pub profile: SetpointProfile, +} + +impl Scenario { + /// 默认控制节拍（1 kHz，与 C620 命令周期一致）。 + pub const DEFAULT_DT_S: f32 = 0.001; + + /// 阶跃 + 保持。 + #[must_use] + pub const fn step(target_rpm: f32, hold_s: f32) -> Self { + Self { + dt_s: Self::DEFAULT_DT_S, + ramp_s: 0.0, + hold_s, + target_rpm, + profile: SetpointProfile::Step, + } + } + + /// 斜坡 + 保持（台架默认整定场景）。 + #[must_use] + pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self { + Self { + dt_s: Self::DEFAULT_DT_S, + ramp_s, + hold_s, + target_rpm, + profile: SetpointProfile::RampAndHold, + } + } + + /// 换一个采样节拍。 + #[must_use] + pub const fn with_dt_s(mut self, dt_s: f32) -> Self { + self.dt_s = dt_s; + self + } + + /// 换一条设定值曲线。 + #[must_use] + pub const fn with_profile(mut self, profile: SetpointProfile) -> Self { + self.profile = profile; + self + } + + /// 场景总时长（秒）。 + #[must_use] + pub fn total_s(&self) -> f32 { + match self.profile { + SetpointProfile::Step => self.hold_s, + SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => { + self.ramp_s + self.hold_s + } + SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s), + } + } + + /// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。 + #[must_use] + pub fn samples(&self) -> usize { + let total = self.total_s(); + let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0; + let total_ok = total.is_finite() && total > 0.0; + if !dt_ok || !total_ok { + return 1; + } + ((total / self.dt_s).round() as usize).max(1) + } + + /// `t_s` 时刻的设定值（rpm）。 + #[must_use] + pub fn setpoint_at(&self, t_s: f32) -> f32 { + let target = self.target_rpm; + let ramp = self.ramp_s; + match self.profile { + SetpointProfile::Step => target, + SetpointProfile::RampAndHold => { + if ramp > 0.0 && t_s < ramp { + target * (t_s / ramp) + } else { + target + } + } + SetpointProfile::Staircase { levels } => { + let levels = levels.max(1); + let total = ramp + self.hold_s; + if total <= 0.0 { + return target; + } + let rung_s = total / levels as f32; + let index = (t_s / rung_s).floor().max(0.0) as u32; + target * ((index.min(levels - 1) + 1) as f32 / levels as f32) + } + SetpointProfile::UpDown => { + let up_end = ramp; + let hold_end = up_end + self.hold_s; + let down_end = hold_end + ramp; + if t_s < up_end { + if ramp > 0.0 { target * (t_s / ramp) } else { target } + } else if t_s < hold_end { + target + } else if t_s < down_end { + let progress = (t_s - hold_end) / ramp; + target * (1.0 - progress) + } else { + 0.0 + } + } + } + } + + /// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。 + #[must_use] + pub fn steady_start_s(&self) -> f32 { + match self.profile { + SetpointProfile::Step => 0.0, + SetpointProfile::RampAndHold => self.ramp_s, + SetpointProfile::Staircase { levels } => { + let level_s = self.level_s(levels); + ((levels.max(1) - 1) as f32) * level_s + } + SetpointProfile::UpDown => self.ramp_s + self.hold_s, + } + } + + /// 稳态评价窗口的终点（秒）。 + #[must_use] + pub fn steady_end_s(&self) -> f32 { + match self.profile { + // 升降场景只评价上升段后的保持段。 + SetpointProfile::UpDown => self.ramp_s + self.hold_s, + _ => self.total_s(), + } + } + + /// 阶梯的单级时长（秒）。 + fn level_s(&self, levels: u32) -> f32 { + let levels = levels.max(1); + let total = self.ramp_s + self.hold_s; + if total > 0.0 { total / levels as f32 } else { 0.0 } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。 + #[track_caller] + fn assert_close(actual: f32, expected: f32) { + assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}"); + } + + #[test] + fn step_is_immediate_and_lasts_hold() { + let s = Scenario::step(1000.0, 0.5); + assert!((s.total_s() - 0.5).abs() < 1e-6); + assert_close(s.setpoint_at(0.0), 1000.0); + assert_close(s.setpoint_at(0.4), 1000.0); + assert_eq!(s.samples(), 500); + } + + #[test] + fn ramp_is_linear_then_holds() { + let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0); + assert_close(s.setpoint_at(0.0), 0.0); + assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3); + assert_close(s.setpoint_at(0.2), 1000.0); + assert_close(s.setpoint_at(1.1), 1000.0); + assert!((s.total_s() - 1.2).abs() < 1e-6); + } + + #[test] + fn staircase_rises_in_equal_levels() { + let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0) + .with_profile(SetpointProfile::Staircase { levels: 4 }); + assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3); + assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3); + assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3); + assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3); + // 越界采样被夹在最后一级。 + assert_close(s.setpoint_at(5.0), 1000.0); + } + + #[test] + fn up_down_returns_to_zero() { + let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown); + assert!((s.total_s() - 0.6).abs() < 1e-6); + assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3); + assert_close(s.setpoint_at(0.2), 600.0); + assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3); + assert_close(s.setpoint_at(0.59), 0.0); + } + + #[test] + fn degenerate_scenario_reports_one_sample() { + let s = Scenario::step(1.0, 0.0); + assert_eq!(s.samples(), 1); + let s = Scenario::step(1.0, 1.0).with_dt_s(0.0); + assert_eq!(s.samples(), 1); + } +} diff --git a/crates/dji-esc-sim/src/trace.rs b/crates/dji-esc-sim/src/trace.rs new file mode 100644 index 0000000..8903429 --- /dev/null +++ b/crates/dji-esc-sim/src/trace.rs @@ -0,0 +1,230 @@ +//! 试验轨迹：一次场景重放的全部采样。 + +/// 一条完整试验轨迹（各列等长，索引即采样序号）。 +#[derive(Debug, Clone, Default, PartialEq)] +pub struct Trace { + /// 采样时刻（秒）。 + pub t_s: Vec, + /// 该时刻的设定值（rpm）。 + pub setpoint_rpm: Vec, + /// 该时刻的实测转速（rpm，物理方向）。 + pub rpm: Vec, + /// 该时刻下发的 effort（raw，已限幅）。 + pub effort_raw: Vec, + /// 该时刻控制器输出是否贴着限幅（饱和）。 + pub saturated: Vec, +} + +impl Trace { + /// 预留容量的空轨迹。 + #[must_use] + pub fn with_capacity(capacity: usize) -> Self { + Self { + t_s: Vec::with_capacity(capacity), + setpoint_rpm: Vec::with_capacity(capacity), + rpm: Vec::with_capacity(capacity), + effort_raw: Vec::with_capacity(capacity), + saturated: Vec::with_capacity(capacity), + } + } + + /// 追加一个采样。 + pub fn push( + &mut self, + t_s: f32, + setpoint_rpm: f32, + rpm: f32, + effort_raw: i16, + saturated: bool, + ) { + self.t_s.push(t_s); + self.setpoint_rpm.push(setpoint_rpm); + self.rpm.push(rpm); + self.effort_raw.push(effort_raw); + self.saturated.push(saturated); + } + + /// 采样数。 + #[must_use] + pub fn len(&self) -> usize { + self.rpm.len() + } + + /// 是否为空。 + #[must_use] + pub fn is_empty(&self) -> bool { + self.rpm.is_empty() + } + + /// 最后一段采样（稳态窗口用；越界时返回空切片）。 + #[must_use] + pub fn window(&self, from_s: f32, to_s: f32) -> Window<'_> { + let start = self.t_s.partition_point(|&t| t < from_s); + let end = self.t_s.partition_point(|&t| t <= to_s); + let end = end.max(start); + Window { trace: self, range: start..end } + } + + /// 全轨迹最大转速（rpm）。 + #[must_use] + pub fn peak_rpm(&self) -> f32 { + self.rpm.iter().copied().fold(0.0f32, |acc, v| acc.max(v.abs())) + } + + /// 全轨迹最小 / 最大转速（rpm）。 + #[must_use] + pub fn rpm_range(&self) -> (f32, f32) { + let min = self.rpm.iter().copied().fold(f32::INFINITY, f32::min); + let max = self.rpm.iter().copied().fold(f32::NEG_INFINITY, f32::max); + if self.rpm.is_empty() { (0.0, 0.0) } else { (min, max) } + } + + /// 饱和采样占比（0..=1）。 + #[must_use] + pub fn saturated_fraction(&self) -> f32 { + if self.saturated.is_empty() { + return 0.0; + } + let count = self.saturated.iter().filter(|&&s| s).count(); + count as f32 / self.saturated.len() as f32 + } + + /// effort 符号翻转次数（振荡 / 抖振指标）。 + #[must_use] + pub fn effort_reversals(&self) -> usize { + let mut reversals = 0usize; + let mut last_sign = 0i16; + for &raw in &self.effort_raw { + let sign = raw.signum(); + if sign != 0 { + if last_sign != 0 && sign != last_sign { + reversals += 1; + } + last_sign = sign; + } + } + reversals + } + + /// 全部列长度一致（不变量自检）。 + #[must_use] + pub fn is_consistent(&self) -> bool { + self.t_s.len() == self.rpm.len() + && self.setpoint_rpm.len() == self.rpm.len() + && self.effort_raw.len() == self.rpm.len() + && self.saturated.len() == self.rpm.len() + } +} + +/// 轨迹上的一个时间窗口视图。 +#[derive(Debug, Clone)] +pub struct Window<'a> { + trace: &'a Trace, + range: std::ops::Range, +} + +impl Window<'_> { + /// 窗口内采样数。 + #[must_use] + pub fn len(&self) -> usize { + self.range.len() + } + + /// 窗口是否为空。 + #[must_use] + pub fn is_empty(&self) -> bool { + self.range.is_empty() + } + + /// 窗口内的实测转速。 + #[must_use] + pub fn rpm(&self) -> &[f32] { + &self.trace.rpm[self.range.clone()] + } + + /// 窗口内的设定值。 + #[must_use] + pub fn setpoint_rpm(&self) -> &[f32] { + &self.trace.setpoint_rpm[self.range.clone()] + } + + /// 窗口内的 effort。 + #[must_use] + pub fn effort_raw(&self) -> &[i16] { + &self.trace.effort_raw[self.range.clone()] + } + + /// 窗口内的时刻。 + #[must_use] + pub fn t_s(&self) -> &[f32] { + &self.trace.t_s[self.range.clone()] + } + + /// 窗口内转速均值。 + #[must_use] + pub fn mean_rpm(&self) -> f32 { + let values = self.rpm(); + if values.is_empty() { + return 0.0; + } + values.iter().sum::() / values.len() as f32 + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn sample_trace() -> Trace { + let mut trace = Trace::with_capacity(8); + for i in 0..8 { + let t = i as f32 * 0.1; + trace.push(t, 100.0, t * 100.0, if i % 2 == 0 { 10 } else { -10 }, i > 5); + } + trace + } + + #[test] + fn push_keeps_columns_aligned() { + let trace = sample_trace(); + assert_eq!(trace.len(), 8); + assert!(trace.is_consistent()); + } + + #[test] + fn window_selects_half_open_range() { + let trace = sample_trace(); + let window = trace.window(0.2, 0.4); + assert_eq!(window.len(), 3); // t = 0.2, 0.3, 0.4 + assert_eq!(window.t_s(), &[0.2, 0.3, 0.4]); + assert!((window.mean_rpm() - 30.0).abs() < 1e-3); + } + + #[test] + fn window_out_of_range_is_empty() { + let trace = sample_trace(); + let window = trace.window(5.0, 6.0); + assert!(window.is_empty()); + assert!(window.mean_rpm().abs() < f32::EPSILON); + } + + #[test] + fn peak_and_range_are_absolute() { + let trace = sample_trace(); + assert!((trace.peak_rpm() - 70.0).abs() < 1e-3); + assert_eq!(trace.rpm_range(), (0.0, 70.0)); + } + + #[test] + fn saturated_fraction_counts_flags() { + let trace = sample_trace(); + assert!((trace.saturated_fraction() - 0.25).abs() < 1e-6); + } + + #[test] + fn effort_reversals_counts_sign_changes() { + let trace = sample_trace(); + // +,−,+,−,+,−,+,−：每次翻转都计数（首个样本建立方向）。 + assert_eq!(trace.effort_reversals(), 7); + } +} diff --git a/crates/dji-esc/tests/closed_loop.rs b/crates/dji-esc-sim/tests/closed_loop.rs similarity index 60% rename from crates/dji-esc/tests/closed_loop.rs rename to crates/dji-esc-sim/tests/closed_loop.rs index ddb2103..d690f92 100644 --- a/crates/dji-esc/tests/closed_loop.rs +++ b/crates/dji-esc-sim/tests/closed_loop.rs @@ -8,12 +8,15 @@ use std::time::Duration; use dji_esc::{ BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback, - encode_feedback_frame, spawn_plant, + encode_feedback_frame, }; -use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive}; -use tokio::time::{sleep, timeout}; +use dji_esc_sim::spawn_plant; +use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive, VelocityGains}; +use tokio::sync::mpsc; +use tokio::task::JoinHandle; +use tokio::time::{MissedTickBehavior, interval, sleep, timeout}; -/// 与 `PlantMotor::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。 +/// 与 `PlantParams::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。 fn tuned(esc_id: u8, direction: f32) -> EscConfig { let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction); cfg.kp = 8.0; @@ -36,6 +39,35 @@ async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool { cond() } +/// 与 [`wait_until`] 相同，但每次轮询都重新下发 `target_rpm`。 +/// +/// **命令是租约**：驱动在 [`BusOptions::command_timeout`]（默认 50ms）内未收到 +/// 新命令就强制 0 输出。真实底盘循环每 1ms 重发一次，这里以 5ms 节拍模拟。 +async fn hold_until( + total_ms: u64, + motor: &M, + target_rpm: f32, + mut cond: impl FnMut() -> bool, +) -> bool { + let mut waited = 0u64; + loop { + motor.set_velocity(target_rpm); // 续租 + if cond() { + return true; + } + if waited >= total_ms { + return false; + } + sleep(Duration::from_millis(5)).await; + waited += 5; + } +} + +/// 保持 `target_rpm` 共 `ms` 毫秒（期间持续续租），仅推进仿真时间。 +async fn hold(motor: &M, target_rpm: f32, ms: u64) { + let _ = hold_until(ms, motor, target_rpm, || false).await; +} + /// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。 async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) { for &id in esc_ids { @@ -65,12 +97,12 @@ async fn velocity_loop_converges() { let motor = bus.motor(1).unwrap().clone(); motor.set_velocity(3000.0); - let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 2950.0).await; + let ok = hold_until(2000, &motor, 3000.0, || motor.state().rotor_speed_rpm >= 2950.0).await; assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm); - // 稳态保持（速度无漂移、反馈持续流入）。 + // 稳态保持（速度无漂移、反馈持续流入）；期间必须继续续租命令。 let count_before = motor.state().feedback_count; - sleep(Duration::from_millis(300)).await; + hold(&motor, 3000.0, 300).await; let st = motor.state(); assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}"); assert!( @@ -93,7 +125,7 @@ async fn reversed_motor_converges_in_physical_frame() { let motor = bus.motor(2).unwrap().clone(); motor.set_velocity(2000.0); - let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 1950.0).await; + let ok = hold_until(2000, &motor, 2000.0, || motor.state().rotor_speed_rpm >= 1950.0).await; assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm); let st = motor.state(); @@ -135,6 +167,7 @@ async fn current_limit_clamps_saturation() { let mut saw_saturated = false; for _ in 0..5 { + bus.motor(1).unwrap().set_velocity(30000.0); // 续租，避免读到租约到期后的 0 let frame = recv_command(&mut peer).await; let raw = slot_current(&frame, 0); assert!(raw <= 5000, "current {raw} exceeds limit 5000"); @@ -144,6 +177,71 @@ async fn current_limit_clamps_saturation() { bus.shutdown().await; } +// ---------------------------------------------------------------- 命令租约 + +/// 反馈喂食器：每 5ms 注入一帧所有电机的反馈，让电机保持"在线"， +/// 从而把「租约到期归零」与「离线保护归零」区分开。 +fn spawn_feedback(yield_tx: mpsc::Sender, esc_ids: Vec) -> JoinHandle<()> { + tokio::spawn(async move { + let mut ticker = interval(Duration::from_millis(5)); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 }; + loop { + ticker.tick().await; + for &id in &esc_ids { + if yield_tx.send(encode_feedback_frame(id, &fb)).await.is_err() { + return; + } + } + } + }) +} + +/// 一次性 `set_velocity` 在租约内出力，租约到期后必须自动归零； +/// 重新下发又能恢复出力——这就是"命令是租约"契约的可执行规范。 +#[tokio::test(start_paused = true)] +async fn command_lease_expires_and_renews() { + let (bus_link, mut peer) = CanLink::pair(256); + let feeder = spawn_feedback(peer.outgoing_tx.clone(), vec![1]); + let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + let motor = bus.motor(1).unwrap().clone(); + + // 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。 + motor.set_velocity(3000.0); + let mut live = false; + for _ in 0..30 { + if slot_current(&recv_command(&mut peer).await, 0) != 0 { + live = true; + break; + } + } + assert!(live, "租约内应输出非零电流"); + + // 2) 沉默 200ms（> 50ms 租约）→ 必须强制归零。 + sleep(Duration::from_millis(200)).await; + // 排空沉默期间积压的帧：其中前段仍带着租约到期前的旧命令。 + while peer.incoming_rx.try_recv().is_ok() {} + sleep(Duration::from_millis(10)).await; + for _ in 0..5 { + let raw = slot_current(&recv_command(&mut peer).await, 0); + assert_eq!(raw, 0, "租约到期后必须归零，实测 {raw}"); + } + + // 3) 续租 → 恢复出力。 + motor.set_velocity(3000.0); + let mut renewed = false; + for _ in 0..30 { + if slot_current(&recv_command(&mut peer).await, 0) != 0 { + renewed = true; + break; + } + } + assert!(renewed, "续租后应恢复非零电流"); + + feeder.abort(); + bus.shutdown().await; +} + // ---------------------------------------------------------------- 组帧与路由 #[tokio::test(start_paused = true)] @@ -160,6 +258,9 @@ async fn current_commands_route_to_group_slots() { let mut saw_200 = false; let mut saw_1ff = false; for _ in 0..20 { + // 续租两条直发命令，避免读到租约到期后的 0 槽。 + bus.motor(3).unwrap().set_effort_raw(1234); + bus.motor(5).unwrap().set_effort_raw(-567); let frame = recv_command(&mut peer).await; match frame.id { 0x200 if !saw_200 => { @@ -273,3 +374,46 @@ fn spawn_rejects_bad_configs() { "expected InvalidConfig, got {err:?}" ); } + +// ---------------------------------------------------------------- 增益热更新 + +/// 自动整定的真机后端靠这条路径逐次试验：不重启总线，只换增益。 +/// 契约：① 读取即得到最新增益；② 换增益后控制器**状态复位**（无旧积分残留）； +/// ③ 限幅等安全字段不受影响。 +#[tokio::test(start_paused = true)] +async fn velocity_gains_can_be_hot_swapped() { + let (bus_link, plant_link) = CanLink::pair(256); + let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1)); + let cfg = tuned(1, 1.0); + let limit = i32::from(cfg.current_limit); + let bus = EscBus::spawn(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap(); + let motor = bus.motor(1).unwrap().clone(); + + // ① 初始增益来自 EscConfig（含前馈）。 + assert_eq!(motor.velocity_gains(), cfg.velocity_gains()); + + // 让旧增益的积分先积累起来（大静差 + 深度饱和）。 + motor.set_velocity(9000.0); + hold(&motor, 9000.0, 200).await; + let stale = motor.velocity_gains(); + + // ② 热更新：控制器被重建。 + let swapped = VelocityGains::new(1.0, 0.0, 0.0, 0.0); + motor.set_velocity_gains(swapped); + assert_eq!(motor.velocity_gains(), swapped); + assert_ne!(motor.velocity_gains(), stale); + + // 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。 + motor.set_velocity(30_000.0); + let saturated = + hold_until(500, &motor, 30_000.0, || i32::from(motor.state().effort_raw) >= limit).await; + assert!(saturated, "effort = {} 未达到限幅 {}", motor.state().effort_raw, limit); + + // ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。 + let effort = i32::from(motor.state().effort_raw); + assert_eq!(effort, limit, "饱和值必须正好是 current_limit"); + assert_eq!(motor.velocity_gains(), swapped, "热更新后增益保持不变"); + + plant.abort(); + bus.shutdown().await; +} diff --git a/crates/dji-esc-sim/tests/harness_parity.rs b/crates/dji-esc-sim/tests/harness_parity.rs new file mode 100644 index 0000000..803e483 --- /dev/null +++ b/crates/dji-esc-sim/tests/harness_parity.rs @@ -0,0 +1,99 @@ +//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。 +//! +//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是 +//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移， +//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与 +//! 同一条场景下比较稳态与 effort。 +//! +//! 允许的差异来自**离散化细节**（异步侧每拍实测 `dt`、反馈比命令晚一拍）， +//! 因此比较的是稳态量级而不是逐点相等。 + +#![allow(clippy::unwrap_used, clippy::expect_used)] + +use std::time::Duration; + +use dji_esc::{BusOptions, CanLink, EscBus, EscConfig}; +use dji_esc_sim::{LoopLimits, PlantParams, Scenario, simulate, spawn_plant}; +use motor_core::{MotorFeedback, VelocityDrive, VelocityGains}; +use tokio::time::sleep; + +/// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。 +const GAINS: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0); +const TARGET_RPM: f32 = 3000.0; +const HOLD_MS: u64 = 2000; +/// 稳态采样窗口（虚拟时间，毫秒）。 +const SAMPLE_FROM_MS: u64 = 1500; + +/// 同步路径：最后 500 ms 的稳态统计。 +fn sync_steady_state() -> (f32, f32) { + let scenario = Scenario::step(TARGET_RPM, HOLD_MS as f32 / 1000.0); + let trace = simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508()) + .expect("simulation must succeed"); + let window = trace.window(SAMPLE_FROM_MS as f32 / 1000.0, HOLD_MS as f32 / 1000.0); + let mean_effort = + window.effort_raw().iter().map(|&raw| f32::from(raw)).sum::() / window.len() as f32; + (window.mean_rpm(), mean_effort) +} + +/// 异步路径：`EscBus` + 假电调，逐拍续租直到 2 秒（虚拟时间）。 +async fn async_steady_state() -> (f32, f32) { + let (bus_link, plant_link) = CanLink::pair(512); + let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1)); + let mut cfg = EscConfig::gm3508("front_left", 1, 1.0); + cfg.kp = GAINS.kp; + cfg.ki = GAINS.ki; + cfg.kd = GAINS.kd; + cfg.feedforward = GAINS.feedforward; + cfg.current_limit = LoopLimits::gm3508().current_limit_raw; + let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).expect("bus must start"); + let motor = bus.motor(1).expect("motor 1 exists").clone(); + + // 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。 + let mut waited = 0u64; + let mut effort_sum = 0.0f32; + let mut rpm_sum = 0.0f32; + let mut samples = 0u32; + while waited < HOLD_MS { + motor.set_velocity(TARGET_RPM); + if waited >= SAMPLE_FROM_MS { + let state = motor.state(); + effort_sum += f32::from(state.effort_raw); + rpm_sum += state.rotor_speed_rpm; + samples += 1; + } + sleep(Duration::from_millis(5)).await; + waited += 5; + } + + plant.abort(); + bus.shutdown().await; + (rpm_sum / samples as f32, effort_sum / samples as f32) +} + +#[tokio::test(start_paused = true)] +async fn synchronous_harness_matches_async_closed_loop() { + let (sync_rpm, sync_effort) = sync_steady_state(); + let (async_rpm, async_effort) = async_steady_state().await; + + // 稳态转速：两条路径都应收敛到目标附近（理想模型 + 精确前馈 → 稳态无差）。 + let rpm_tolerance = 0.05 * TARGET_RPM; + assert!( + (sync_rpm - async_rpm).abs() < rpm_tolerance, + "steady rpm mismatch: sync {sync_rpm}, async {async_rpm}" + ); + assert!( + (sync_rpm - TARGET_RPM).abs() < rpm_tolerance, + "sync steady state off target: {sync_rpm}" + ); + assert!( + (async_rpm - TARGET_RPM).abs() < rpm_tolerance, + "async steady state off target: {async_rpm}" + ); + + // effort：前馈主导，两条路径量级必须接近（±20% 或 ±300 raw 取大者）。 + let effort_tolerance = (0.2 * sync_effort.abs()).max(300.0); + assert!( + (sync_effort - async_effort).abs() < effort_tolerance, + "steady effort mismatch: sync {sync_effort}, async {async_effort}" + ); +} diff --git a/crates/dji-esc-slcan/Cargo.toml b/crates/dji-esc-slcan/Cargo.toml new file mode 100644 index 0000000..f3f4cc1 --- /dev/null +++ b/crates/dji-esc-slcan/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "dji-esc-slcan" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "SLCAN serial backend (e.g. /dev/ttyACM0) bridging to can-link channels" + +[lints] +workspace = true + +[dependencies] +can-link = { workspace = true } +slcan = "0.1.3" +serial = "0.4" +tokio = { workspace = true } +tracing = { workspace = true } + diff --git a/crates/dji-esc-slcan/src/lib.rs b/crates/dji-esc-slcan/src/lib.rs new file mode 100644 index 0000000..370bc43 --- /dev/null +++ b/crates/dji-esc-slcan/src/lib.rs @@ -0,0 +1,348 @@ +//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can_link::CanLink`]。 +//! +//! 基于 [`slcan`] crate（Lawicel ASCII 协议）：`S8` = 1 Mbps（C620 标准），`O` 打开通道。 +//! 阻塞串口 I/O 跑在专用线程上，与 SocketCAN 桥一样只暴露通道端点——对本 crate +//! 的调用方而言，`dji-esc` 看到的是一个普通 [`CanLink`]，与传输介质无关。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +use std::io; +use std::sync::Arc; +use std::sync::atomic::{AtomicU64, Ordering}; +use std::thread; +use std::time::{Duration, Instant}; + +use can_link::{CanFrame as LinkFrame, CanLink}; +use serial::prelude::*; +use slcan::{BitRate, CanSocket}; +use tokio::sync::mpsc; +use tracing::{debug, error, info, warn}; + +/// 链路通道缓冲（**故意很浅**）。 +/// +/// 两个方向的需求完全相反： +/// +/// - **命令方向**（bus → 桥）：必须浅。驱动按 1kHz 生产命令，而 slcan 一次 write +/// 在 CDC ACM 上要约 2ms，下行只能到几百帧/秒。若缓冲很深，队列会被"过时命令" +/// 灌满，ESC 实际执行的是**一秒多以前**算出来的电流——速度环等于开了 1.4s 的 +/// 延迟，必然失控跑飞。取小值让 `send().await` 立刻回压，驱动自然降速到链路 +/// 能承受的节拍，每一帧都是**按最新反馈现算**的。 +/// - **反馈方向**（桥 → bus）：是采样流，来不及就丢（见 `pump` 的 `try_send`）， +/// 因此不需要深队列。 +const CHANNEL_BOUND: usize = 8; + +/// 经典 CAN 标准帧 ID 上界（11 位）。超出者不是标准帧，一律丢弃： +/// SLCAN 的 `t` 命令只承载 3 位十六进制 ID，截断后会把扩展帧冒充成标准帧。 +const MAX_STANDARD_ID: u32 = 0x7FF; + +/// 反馈方向帧过滤：只接受「标准 ID + 8 字节」的数据帧。 +/// +/// C620 反馈固定 8 字节；DLC 不足的帧若放行，协议层会按越界读取拒绝， +/// 但更早丢弃可以省掉一次无谓的解码。 +#[must_use] +fn accepted_rx(frame: &slcan::CanFrame) -> Option { + if frame.id > MAX_STANDARD_ID || frame.dlc != 8 { + return None; + } + Some(LinkFrame::new(frame.id, frame.data)) +} + +/// slcan CAN 波特率档位（对应协议 `Sn` 命令）。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] +pub enum CanSpeed { + /// `S6`：500 kbps。 + K500, + /// `S8`：1 Mbps（C620 标准）。 + #[default] + M1, +} + +impl CanSpeed { + fn to_slcan(self) -> BitRate { + match self { + Self::K500 => BitRate::Setup500Kbit, + Self::M1 => BitRate::Setup1Mbit, + } + } +} + +/// 打开参数。 +#[derive(Debug, Clone, Copy)] +pub struct SlcanOptions { + /// 串口波特率（USB CDC ACM 常见 2_000_000；部分适配器 115_200）。 + pub serial_baud: u32, + /// CAN 波特率档位。 + pub speed: CanSpeed, + /// 串口超时。**读写共用**（见 `pump` 的说明）：`serial` crate 的 + /// `TTYPort::write` 也先按本超时 `poll(POLLOUT)`。 + /// + /// 太短会把正常的"这一拍写不出去"误判成写失败。总线繁忙时读几乎立即返回， + /// 因此放大它只影响"总线完全静默"时的下行延迟，默认 50ms。 + pub read_timeout: Duration, +} + +impl Default for SlcanOptions { + fn default() -> Self { + Self { + serial_baud: 2_000_000, + speed: CanSpeed::M1, + read_timeout: Duration::from_millis(50), + } + } +} + +/// 打开 slcan 设备并桥接为 [`CanLink`]（默认 1 Mbps / 2 Mbaud）。 +/// +/// # Errors +/// +/// 设备不存在、权限不足、波特率不被支持等 `io::Error`。 +pub fn open_link(device: &str) -> io::Result { + open_link_with(device, SlcanOptions::default()) +} + +/// 按指定参数打开 slcan 设备。 +/// +/// # Errors +/// +/// 同 [`open_link`]。 +pub fn open_link_with(device: &str, options: SlcanOptions) -> io::Result { + let mut port = serial::open(device)?; + port.reconfigure(&|settings| { + // `serial::BaudOther` 以 `usize` 计数；u32 → usize 在 32/64 位目标上均无损。 + settings.set_baud_rate(serial::BaudOther(options.serial_baud as usize))?; + Ok(()) + })?; + port.set_timeout(options.read_timeout)?; + + let mut socket = CanSocket::new(port); + socket.open(options.speed.to_slcan())?; + info!(device, serial_baud = options.serial_baud, speed = ?options.speed, "slcan opened"); + + let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND); + let CanLink { outgoing_tx, incoming_rx } = bridge_side; + let stats = Arc::new(LinkStats::default()); + + thread::Builder::new() + .name("slcan-bridge".into()) + .spawn(move || pump(socket, outgoing_tx, incoming_rx, stats))?; + Ok(bus_side) +} + +#[derive(Default)] +struct LinkStats { + rx: AtomicU64, + /// 因 bus 侧来不及消费而丢弃的反馈帧（反馈是采样流，丢弃是正常降级）。 + rx_dropped: AtomicU64, + tx: AtomicU64, + /// 写通道未就绪而推迟到下一轮重试的命令帧次数（帧被保留，不丢）。 + tx_retry: AtomicU64, + quiet_bucket: AtomicU64, +} + +/// 每轮最多连续发送的命令帧数。 +/// +/// 电调总线按 1kHz 持续产出命令帧；若不加界，一次"排空"就可能永远排空—— +/// 尤其在某次写 poll 超时之后，新帧仍在流入，接收方向会被**永久饿死**。 +const TX_BURST: usize = 8; + +/// 每轮最多读取的反馈帧数。 +/// +/// 一次 `write` 在 CDC ACM 上可能要 ~2ms，而发送上界是 8 帧，于是"每轮只读一帧" +/// 会让接收被发送压到几十 Hz。这里每轮把已缓冲的反馈尽量读完（无数据即刻返回）， +/// 双向都不会饿死。 +const RX_BURST: usize = 64; + +/// 瞬时（可恢复）I/O 错误：错过这一拍不代表链路坏了。 +/// +/// `serial` crate 的 `TTYPort` 把**同一个** timeout 同时用于读和写—— +/// `write()` 先做 `poll(POLLOUT, timeout)`，超时即返回 `TimedOut`。 +/// 这类事件在 USB CDC ACM 上很常见，绝不能当成致命错误。 +fn is_transient(e: &io::Error) -> bool { + matches!( + e.kind(), + io::ErrorKind::TimedOut | io::ErrorKind::WouldBlock | io::ErrorKind::Interrupted + ) +} + +/// 写一帧，并区分**瞬时**与**致命**错误。 +/// +/// 返回 `Ok(true)` = 已写出；`Ok(false)` = 通道暂时写不动，调用方应保留该帧、 +/// **立刻让位给接收方向**，下一轮再试；`Err` = 真·致命（设备拔出、BrokenPipe 等）。 +/// +/// 关键：**写不动绝不能让接收方向一起停摆**。早期实现既无发送上界、又把 +/// `TimedOut` 当致命错误直接 return，结果第一拍命令没写出去就把整条链路带停了。 +fn write_frame(socket: &mut CanSocket, frame: &LinkFrame) -> io::Result { + match socket.write(frame.id, &frame.data) { + Ok(_) => Ok(true), + Err(e) if is_transient(&e) => { + debug!(error = %e, id = frame.id, "slcan write not ready, retry next round"); + Ok(false) + } + Err(e) => Err(e), + } +} + +/// 单线程双向泵：优先排空待发命令，随后读一帧反馈。 +/// +/// `serial::SystemPort` 即 `serial::open` 返回的平台原生端口类型 +/// （Unix = `TTYPort`），因此本函数在 Linux 目标板与 macOS 开发机上同源。 +/// +/// 发送端与统计按值传入是有意的：它们随泵线程独占所有权，线程退出时 +/// `tx` 被 drop，总线侧据此观察到链路关闭。 +#[allow(clippy::needless_pass_by_value)] +fn pump( + mut socket: CanSocket, + tx: mpsc::Sender, + mut rx: mpsc::Receiver, + stats: Arc, +) { + let mut last_rx_ok = Instant::now(); + let mut last_stats = Instant::now(); + // 上一轮没能写出去的帧：保留而非丢弃，下一轮优先重试。 + let mut pending: Option = None; + loop { + // 写：**有界**排空待发命令；一旦通道写不动就交给接收方向。 + for _ in 0..TX_BURST { + let frame = match pending.take() { + Some(frame) => frame, + None => match rx.try_recv() { + Ok(frame) => frame, + Err(mpsc::error::TryRecvError::Empty) => break, + Err(mpsc::error::TryRecvError::Disconnected) => { + debug!("bus side closed, slcan bridge exiting"); + let _ = socket.close(); + return; + } + }, + }; + // P-1：拒绝非标准 ID，避免 `as u16` 截断后冒充标准帧。 + if frame.id > MAX_STANDARD_ID { + debug!(id = frame.id, "drop frame with non-standard CAN id"); + continue; + } + match write_frame(&mut socket, &frame) { + Ok(true) => { + stats.tx.fetch_add(1, Ordering::Relaxed); + } + Ok(false) => { + stats.tx_retry.fetch_add(1, Ordering::Relaxed); + pending = Some(frame); + break; + } + Err(e) => { + error!(error = %e, "slcan write failed, bridge exiting"); + return; + } + } + } + + // 周期性链路统计：现场判断"命令到底发出去没有"靠它。 + if last_stats.elapsed() >= Duration::from_secs(5) { + last_stats = Instant::now(); + debug!( + rx = stats.rx.load(Ordering::Relaxed), + rx_dropped = stats.rx_dropped.load(Ordering::Relaxed), + tx = stats.tx.load(Ordering::Relaxed), + tx_retry = stats.tx_retry.load(Ordering::Relaxed), + "slcan link stats" + ); + } + + // 读：**有界**批量读取已缓冲的反馈；无数据立即让位给发送方向。 + for _ in 0..RX_BURST { + match socket.read() { + Ok(frame) => { + last_rx_ok = Instant::now(); + stats.rx.fetch_add(1, Ordering::Relaxed); + let Some(frame) = accepted_rx(&frame) else { + debug!( + id = frame.id, + dlc = frame.dlc, + "drop non-standard or short slcan frame" + ); + continue; + }; + // 反馈是采样流：来不及就丢，绝不阻塞泵线程（否则又会饿死反方向）。 + match tx.try_send(frame) { + Ok(()) => {} + Err(mpsc::error::TrySendError::Full(_)) => { + stats.rx_dropped.fetch_add(1, Ordering::Relaxed); + } + Err(mpsc::error::TrySendError::Closed(_)) => { + debug!("bus side closed, slcan rx exiting"); + let _ = socket.close(); + return; + } + } + } + Err(e) if is_transient(&e) => { + // S-5：长时间无反馈 → 怀疑接线/bus-off/电调未上电。 + let quiet = last_rx_ok.elapsed(); + if quiet > Duration::from_secs(1) { + let bucket = quiet.as_secs(); + if stats.quiet_bucket.swap(bucket, Ordering::Relaxed) != bucket { + warn!( + quiet_ms = quiet.as_millis() as u64, + rx = stats.rx.load(Ordering::Relaxed), + tx = stats.tx.load(Ordering::Relaxed), + tx_retry = stats.tx_retry.load(Ordering::Relaxed), + "slcan: no feedback for >1s (wiring / bus-off / powered ESC?)" + ); + } + } + break; // 缓冲已空，回到发送方向 + } + Err(e) if e.kind() == io::ErrorKind::InvalidData => { + // 单帧解析失败（噪声 / 半帧）不该杀掉整条链路。 + debug!(error = %e, "slcan: malformed frame dropped"); + } + Err(e) => { + error!(error = %e, "slcan read failed, bridge exiting"); + return; + } + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + fn slcan_frame(id: u32, dlc: usize) -> slcan::CanFrame { + slcan::CanFrame::new(id, dlc, &[0xAA; 8]) + } + + #[test] + fn accepts_standard_8_byte_frames() { + let frame = slcan::CanFrame::new(0x201, 8, &[1, 2, 3, 4, 5, 6, 7, 8]); + let link = accepted_rx(&frame).expect("0x201/8B must be accepted"); + assert_eq!(link.id, 0x201); + assert_eq!(link.data, [1, 2, 3, 4, 5, 6, 7, 8]); + } + + #[test] + fn rejects_extended_id() { + assert!(accepted_rx(&slcan_frame(0x800, 8)).is_none(), "11 位 ID 上界之外必须丢弃"); + assert!(accepted_rx(&slcan_frame(0x1FFF_FFFF, 8)).is_none()); + } + + #[test] + fn rejects_short_frames() { + for dlc in 0..8 { + assert!(accepted_rx(&slcan_frame(0x201, dlc)).is_none(), "dlc {dlc} 必须丢弃"); + } + } + + #[test] + fn max_standard_id_is_boundary() { + assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID, 8)).is_some()); + assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID + 1, 8)).is_none()); + } + + #[test] + fn speed_maps_to_lawicel_setup_command() { + assert_eq!(CanSpeed::M1.to_slcan() as u8, b'8'); + assert_eq!(CanSpeed::K500.to_slcan() as u8, b'6'); + assert_eq!(CanSpeed::default(), CanSpeed::M1); + } +} diff --git a/crates/dji-esc/src/bus.rs b/crates/dji-esc/src/bus.rs index 6656415..a44165d 100644 --- a/crates/dji-esc/src/bus.rs +++ b/crates/dji-esc/src/bus.rs @@ -14,7 +14,7 @@ use std::time::Duration; use motor_core::{ Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController, - PositionDrive, VelocityController, VelocityDrive, + PositionDrive, VelocityController, VelocityDrive, VelocityGains, }; use thiserror::Error; use tokio::sync::{mpsc, watch}; @@ -44,6 +44,9 @@ pub struct EscConfig { /// 电调型号。 pub model: EscModel, /// 减速比（电机转子 : 输出轴）。 + /// + /// 驱动层**不使用**此字段；由上层 [`motor_core::OutputShaft`] 应用。 + /// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。 pub gear_ratio: f32, /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转， /// 上层永远以"输出轴正转 = 前进方向"工作。 @@ -99,6 +102,12 @@ impl EscConfig { } } + /// 速度环增益组（可整定参数）。 + #[must_use] + pub fn velocity_gains(&self) -> VelocityGains { + VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward) + } + fn validate(&self) -> Result<(), String> { if command_frame_id(self.esc_id).is_none() { return Err(format!("esc_id {} out of range 1..=8", self.esc_id)); @@ -130,11 +139,25 @@ pub struct BusOptions { pub period: Duration, /// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。 pub feedback_timeout: Duration, + /// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。 + /// + /// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` / + /// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发 + /// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可）， + /// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。 + /// + /// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。 + pub command_timeout: Duration, } impl Default for BusOptions { fn default() -> Self { - Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) } + Self { + period: Duration::from_millis(1), + feedback_timeout: Duration::from_millis(100), + // 1kHz 下约 50 拍未刷新即失效。 + command_timeout: Duration::from_millis(50), + } } } @@ -162,6 +185,8 @@ struct MotorShared { cfg: EscConfig, state: Mutex, cmd: Mutex, + /// 命令写入时刻（看门狗用）。 + cmd_at: Mutex>, last_mode: Mutex, vel_ctrl: Mutex, pos_ctrl: Mutex, @@ -181,6 +206,7 @@ impl MotorShared { cfg, state: Mutex::new(MotorState::default()), cmd: Mutex::new(Command::Disabled), + cmd_at: Mutex::new(None), last_mode: Mutex::new(CommandMode::Disabled), turn: Mutex::new(TurnTracker::new()), last_feedback: Mutex::new(None), @@ -188,6 +214,26 @@ impl MotorShared { } } + fn store_command(&self, cmd: Command) { + *lock(&self.cmd) = cmd; + *lock(&self.cmd_at) = Some(Instant::now()); + } + + /// 当前速度环增益（来自控制器，而非启动时的 `cfg`）。 + fn velocity_gains(&self) -> VelocityGains { + lock(&self.vel_ctrl).gains() + } + + /// 热更新速度环增益：重建控制器（等价于 reset，避免积分冲击）。 + /// + /// 只替换增益，不动限幅 / 型号 / 方向 —— 那些是安全与机械约束， + /// 不属于可在线试错的整定量。 + fn set_velocity_gains(&self, gains: VelocityGains) { + let limit = self.cfg.current_limit; + let mut controller = lock(&self.vel_ctrl); + *controller = VelocityController::from_gains(limit, gains); + } + /// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。 fn apply_feedback(&self, fb: EscFeedback, now: Instant) { let esc_rev = lock(&self.turn).update(fb.angle_raw); @@ -212,7 +258,13 @@ impl MotorShared { } /// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。 - fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 { + fn compute_effort( + &self, + dt: f32, + feedback_timeout: Duration, + command_timeout: Duration, + now: Instant, + ) -> i16 { let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout); if !online { if !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) { @@ -223,6 +275,14 @@ impl MotorShared { *lock(&self.last_mode) = CommandMode::Disabled; return 0; } + // 命令看门狗：上层任务死亡/卡死后旧指令不得持续输出。 + let cmd_fresh = + matches!(*lock(&self.cmd_at), Some(t) if now.duration_since(t) <= command_timeout); + if !cmd_fresh { + self.reset_controllers(); + *lock(&self.last_mode) = CommandMode::Disabled; + return 0; + } let cmd = *lock(&self.cmd); // 模式切换复位：避免积分/微分记忆冲击。 { @@ -257,6 +317,13 @@ impl MotorShared { } /// 电机的公共句柄：可克隆，跨任务共享。 +/// +/// # 命令租约 +/// +/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] / +/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。 +/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出）， +/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。 #[derive(Clone)] pub struct EscMotor { shared: Arc, @@ -286,6 +353,25 @@ impl EscMotor { pub fn last_feedback(&self) -> Option { *lock(&self.shared.last_feedback) } + + /// 当前速度环增益。 + #[must_use] + pub fn velocity_gains(&self) -> VelocityGains { + self.shared.velocity_gains() + } + + /// 热更新速度环增益（**不需重启总线**，下一发送节拍生效）。 + /// + /// 用于自动整定：一次真机试验只改增益，不必反复开关串口 / CAN 接口。 + /// 控制器被重建，历史积分被丢弃——这正是一次新试验想要的起点。 + /// + /// # Panics + /// + /// 增益含 NaN / inf 时 panic（同 [`Pid::new`]）；调用方应先检查 + /// [`VelocityGains::is_finite`]。 + pub fn set_velocity_gains(&self, gains: VelocityGains) { + self.shared.set_velocity_gains(gains); + } } impl MotorFeedback for EscMotor { @@ -296,29 +382,38 @@ impl MotorFeedback for EscMotor { fn state(&self) -> MotorState { *lock(&self.shared.state) } + + fn feedback_age(&self) -> Option { + let t = *lock(&self.shared.last_feedback); + Some(Instant::now().saturating_duration_since(t?)) + } } impl VelocityDrive for EscMotor { + /// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。 fn set_velocity(&self, rotor_rpm: f32) { - *lock(&self.shared.cmd) = Command::Velocity(rotor_rpm); + self.shared.store_command(Command::Velocity(rotor_rpm)); } } impl EffortDrive for EscMotor { + /// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。 fn set_effort_raw(&self, raw: i16) { - *lock(&self.shared.cmd) = Command::EffortRaw(raw); + self.shared.store_command(Command::EffortRaw(raw)); } } impl PositionDrive for EscMotor { + /// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。 fn set_position(&self, rotor_rev: f32) { - *lock(&self.shared.cmd) = Command::Position(rotor_rev); + self.shared.store_command(Command::Position(rotor_rev)); } } impl Disable for EscMotor { + /// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。 fn disable(&self) { - *lock(&self.shared.cmd) = Command::Disabled; + self.shared.store_command(Command::Disabled); } } @@ -423,14 +518,20 @@ async fn tx_loop( options: BusOptions, mut shutdown: watch::Receiver, ) { - let dt = options.period.as_secs_f32(); + let nominal_dt = options.period.as_secs_f32(); let mut ticker = tick_every(options.period); + // 实测 dt：丢拍/负载抖动时 PID 不得按名义 1ms 硬算（F-1）。 + let mut last_tick = Instant::now(); loop { tokio::select! { _ = shutdown.changed() => break, _ = ticker.tick() => {} } let now = Instant::now(); + let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32(); + last_tick = now; + // 夹在 [0.5, 4] × 名义周期，避免首拍/长卡死把积分或微分打飞。 + let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0); for (frame_id, group_start) in [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)] { @@ -444,7 +545,12 @@ async fn tx_loop( let mut currents = [0i16; 4]; for motor in &group { let slot = usize::from(motor.id() - group_start); - currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now); + currents[slot] = motor.shared.compute_effort( + dt, + options.feedback_timeout, + options.command_timeout, + now, + ); } if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() { info!("can link closed, tx loop exiting"); diff --git a/crates/dji-esc/src/lib.rs b/crates/dji-esc/src/lib.rs index 9f17fcd..6960c73 100644 --- a/crates/dji-esc/src/lib.rs +++ b/crates/dji-esc/src/lib.rs @@ -5,17 +5,18 @@ //! - [`codec`]：纯函数帧编解码（C620 手册协议）； //! - [`turn`]：单圈编码器 → 连续多圈角； //! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）； -//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全； -//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。 +//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全。 //! //! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction` //! 完成方向修正。 +//! +//! 被控对象仿真（假电调）**不在本 crate**：见 `dji-esc-sim`。本 crate 只描述 +//! 真实电调在总线上的行为，不假设对面是谁。 #![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] pub mod bus; pub mod codec; pub mod frame; -pub mod sim; pub mod turn; pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor}; @@ -25,5 +26,4 @@ pub use codec::{ decode_feedback, encode_current_frame, encode_feedback_frame, }; pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE}; -pub use sim::{PlantMotor, spawn_plant}; pub use turn::{ENCODER_TICKS, TurnTracker}; diff --git a/crates/dji-esc/src/sim.rs b/crates/dji-esc/src/sim.rs deleted file mode 100644 index 7ecbf05..0000000 --- a/crates/dji-esc/src/sim.rs +++ /dev/null @@ -1,116 +0,0 @@ -//! 电机被控对象仿真：一阶转速模型 + 编码器积分。 -//! -//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环 -//! （测试、`chassisd --mock` 均复用）。 - -use std::collections::HashMap; -use std::time::Duration; - -use tokio::task::JoinHandle; -use tokio::time::{MissedTickBehavior, interval}; -use tracing::trace; - -use crate::codec::{EscFeedback, encode_feedback_frame}; -use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink}; - -/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。 -/// -/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数 -/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。 -#[derive(Debug, Clone)] -pub struct PlantMotor { - gain_rpm_per_raw: f32, - tau: f32, - max_rpm: f32, - speed_rpm: f32, - angle_raw: f32, -} - -impl PlantMotor { - /// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。 - #[must_use] - pub fn gm3508() -> Self { - Self { - gain_rpm_per_raw: 9254.0 / 16384.0, - tau: 0.12, - max_rpm: 9254.0, - speed_rpm: 0.0, - angle_raw: 0.0, - } - } - - /// 当前转子转速（rpm，电调侧）。 - #[must_use] - pub fn speed_rpm(&self) -> f32 { - self.speed_rpm - } - - /// 推进一个时间步，返回本步反馈帧内容。 - pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback { - let target = - (f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm); - let alpha = 1.0 - (-dt / self.tau).exp(); - self.speed_rpm += (target - self.speed_rpm) * alpha; - self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0); - EscFeedback { - angle_raw: self.angle_raw as u16, - speed_rpm: self.speed_rpm as i16, - current_raw, - temperature_c: 30, - } - } -} - -/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。 -/// -/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。 -pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> { - let mut plants: HashMap = - esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect(); - let mut commands: HashMap = esc_ids.iter().map(|&id| (id, 0)).collect(); - let dt = tick.as_secs_f32(); - let mut ticker = interval(tick); - ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); - - tokio::spawn(async move { - loop { - tokio::select! { - frame = link.incoming_rx.recv() => match frame { - Some(frame) => apply_command_frame(&frame, &mut commands), - None => break, - }, - _ = ticker.tick() => { - for (&id, plant) in &mut plants { - let cmd = commands.get(&id).copied().unwrap_or(0); - let feedback = plant.step(cmd, dt); - if link - .outgoing_tx - .send(encode_feedback_frame(id, &feedback)) - .await - .is_err() - { - return; - } - } - } - } - } - trace!("plant exiting"); - }) -} - -fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap) { - let group_start = match frame.id { - COMMAND_FRAME_ID_PRIMARY => 1u8, - COMMAND_FRAME_ID_SECONDARY => 5u8, - _ => return, - }; - for slot in 0..4u8 { - if let Some(cmd) = commands.get_mut(&(group_start + slot)) { - *cmd = i16::from_be_bytes([ - frame.data[(slot * 2) as usize], - frame.data[(slot * 2 + 1) as usize], - ]); - } - } -} diff --git a/crates/motor-core/src/gearbox.rs b/crates/motor-core/src/gearbox.rs index f423f3b..8877747 100644 --- a/crates/motor-core/src/gearbox.rs +++ b/crates/motor-core/src/gearbox.rs @@ -3,6 +3,8 @@ //! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。 //! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。 +use std::time::Duration; + use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive}; /// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。 @@ -66,6 +68,10 @@ impl MotorFeedback for OutputShaft { // 只透传转子量；不伪造输出轴字段。 self.inner.state() } + + fn feedback_age(&self) -> Option { + self.inner.feedback_age() + } } impl VelocityDrive for OutputShaft { @@ -116,6 +122,9 @@ mod tests { fn state(&self) -> MotorState { MotorState::default() } + fn feedback_age(&self) -> Option { + Some(Duration::ZERO) + } } impl VelocityDrive for Stub { fn set_velocity(&self, rotor_rpm: f32) { diff --git a/crates/motor-core/src/lib.rs b/crates/motor-core/src/lib.rs index d29624f..aa157ef 100644 --- a/crates/motor-core/src/lib.rs +++ b/crates/motor-core/src/lib.rs @@ -41,7 +41,7 @@ pub use position::{PositionController, PositionLoop}; pub use state::MotorState; pub use testing::{MockHeading, MockMotor}; pub use traits::{Disable, EffortDrive, Motor, MotorFeedback, PositionDrive, VelocityDrive}; -pub use velocity::VelocityController; +pub use velocity::{VelocityController, VelocityGains}; #[cfg(test)] mod tests { diff --git a/crates/motor-core/src/testing.rs b/crates/motor-core/src/testing.rs index c4a55a3..0283447 100644 --- a/crates/motor-core/src/testing.rs +++ b/crates/motor-core/src/testing.rs @@ -2,6 +2,7 @@ use std::future::Future; use std::sync::{Arc, Mutex, MutexGuard}; +use std::time::{Duration, Instant}; use crate::{ Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive, @@ -20,18 +21,30 @@ fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { pub struct MockMotor { id: u8, published: Arc>, + last_feedback: Arc>>, } impl MockMotor { /// 创建指定 ID 的空电机。 #[must_use] pub fn new(id: u8) -> Self { - Self { id, published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))) } + Self { + id, + published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))), + last_feedback: Arc::new(Mutex::new(None)), + } } - /// 注入反馈快照。 + /// 注入反馈快照（记为「刚刚收到」）。 pub fn inject_state(&self, state: MotorState) { + self.inject_state_with_age(state, Duration::ZERO); + } + + /// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。 + pub fn inject_state_with_age(&self, state: MotorState, age: Duration) { lock(&self.published).0 = state; + let at = Instant::now().checked_sub(age); + *lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now)); } /// 最近一次命令。 @@ -48,6 +61,10 @@ impl MotorFeedback for MockMotor { fn state(&self) -> MotorState { lock(&self.published).0 } + fn feedback_age(&self) -> Option { + let at = *lock(&self.last_feedback); + Some(at?.elapsed()) + } } impl VelocityDrive for MockMotor { @@ -110,7 +127,19 @@ mod tests { fn mock_motor_injects_state() { let m = MockMotor::new(1); assert!(!m.state().has_feedback()); + assert!(m.feedback_age().is_none()); m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() }); assert!(m.state().has_feedback()); + assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50))); + } + + #[test] + fn mock_motor_feedback_age_can_be_stale() { + let m = MockMotor::new(1); + m.inject_state_with_age( + MotorState { feedback_count: 1, ..MotorState::default() }, + Duration::from_millis(250), + ); + assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200))); } } diff --git a/crates/motor-core/src/traits.rs b/crates/motor-core/src/traits.rs index 247ae06..c03ddca 100644 --- a/crates/motor-core/src/traits.rs +++ b/crates/motor-core/src/traits.rs @@ -6,6 +6,8 @@ //! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`] //! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`] +use std::time::Duration; + use crate::MotorState; /// 读反馈。所有可观察设备的最小面。 @@ -15,6 +17,12 @@ pub trait MotorFeedback: Send + Sync { /// 最新反馈快照。 fn state(&self) -> MotorState; + + /// 距最近一帧反馈的时长；`None` 表示从未收到。 + /// + /// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`] + /// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。 + fn feedback_age(&self) -> Option; } /// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。 diff --git a/crates/motor-core/src/velocity.rs b/crates/motor-core/src/velocity.rs index 521cc0b..227d9ab 100644 --- a/crates/motor-core/src/velocity.rs +++ b/crates/motor-core/src/velocity.rs @@ -2,6 +2,43 @@ use crate::pid::Pid; +/// 速度环增益组（可整定参数）。 +/// +/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)， +/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间 +/// 传递、落盘与热更新，而无需重建整个速度环。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct VelocityGains { + /// 比例增益（raw / rpm）。 + pub kp: f32, + /// 积分增益（raw / (rpm·s)）。 + pub ki: f32, + /// 微分增益（raw / (rpm/s)）。 + pub kd: f32, + /// 速度前馈（raw / rpm）。 + pub feedforward: f32, +} + +impl VelocityGains { + /// 全零增益。 + pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 }; + + /// 由四个分量构造。 + #[must_use] + pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self { + Self { kp, ki, kd, feedforward } + } + + /// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。 + #[must_use] + pub fn is_finite(&self) -> bool { + self.kp.is_finite() + && self.ki.is_finite() + && self.kd.is_finite() + && self.feedforward.is_finite() + } +} + /// 速度环封装：在 [`Pid`] 之上叠加比例前馈。 /// /// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量 @@ -23,6 +60,29 @@ impl VelocityController { Self { pid, feedforward } } + /// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。 + /// + /// # Panics + /// + /// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。 + #[must_use] + pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self { + let limit = f32::from(output_limit_raw); + let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit); + Self { pid, feedforward: gains.feedforward } + } + + /// 当前增益组（含前馈），用于诊断与热更新前的读取。 + #[must_use] + pub fn gains(&self) -> VelocityGains { + VelocityGains { + kp: self.pid.kp(), + ki: self.pid.ki(), + kd: self.pid.kd(), + feedforward: self.feedforward, + } + } + /// 单步更新，返回 effort_raw（已含 PID 输出限幅）。 pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 { let pid_out = self.pid.update(target_rpm, measured_rpm, dt); @@ -84,4 +144,37 @@ mod tests { let out = vc.update(100.0, 0.0, 0.001); assert!((out - 1010.0).abs() < 1e-3, "out = {out}"); } + + #[test] + fn from_gains_round_trips_through_gains() { + let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77); + let mut from_data = VelocityController::from_gains(12_000, gains); + let mut by_hand = + VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77); + + assert_eq!(from_data.gains(), gains); + // 两条构造路径必须逐位一致。 + for step in 0..16 { + let target = 1000.0 + step as f32; + let measured = 900.0 + step as f32 * 3.0; + assert_eq!( + from_data.update(target, measured, 0.001).to_bits(), + by_hand.update(target, measured, 0.001).to_bits() + ); + } + } + + #[test] + fn from_gains_clamps_to_output_limit() { + let mut vc = VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0)); + assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3); + } + + #[test] + fn gains_finiteness_is_checkable() { + assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite()); + assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite()); + assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite()); + assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0)); + } } diff --git a/docs/architecture.md b/docs/architecture.md index b63ccc4..398326f 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -3,33 +3,102 @@ ## 分层 ``` - ┌─────────────────────────────────────────────┐ - │ chassisd（守护进程） │ - │ 配置 / CLI / 1kHz 控制循环 / 安全逻辑 / 日志 │ - └──────┬───────────────────────────┬──────────┘ - │ mecanum（麦轮正逆解） │ - ▼ ▼ - ┌────────────┐ ┌──────────────────┐ - │ motor-core │◄─实现 trait──│ dji-esc │ - │ Motor trait│ │ C620/C610 协议 │ - │ MotorState │ │ EscBus 异步驱动 │ - │ Pid │ │ TurnTracker 多圈 │ - └────────────┘ └────────┬─────────┘ - │ CanLink（tokio mpsc 通道对） - ▼ - ┌──────────────────────────┐ - │ dji-esc-socketcan(Linux) │ - │ socketcan ↔ CanLink 桥接 │ - └──────────────────────────┘ + ┌────────────────┐ ┌──────────────────────┐ ┌─────────────────────────┐ + │ chassisd │ │ chassis-tune │ │ chassis-bench │ + │ 守护进程 │ │ 自动整定（搜索/报告） │ │ 台架（激励/包络/采样） │ + │ 1kHz 循环 / 安全│ │ sim ↓ bench ↓ │ │ chassis-spin 等 │ + └───┬────────┬───┘ └────┬──────────┬──────┘ └────┬───────────────────┘ + │ │ │ │ │ + │ │ │ └───────┬───────┘ + ▼ ▼ ▼ ▼ + ┌───────────┐ ┌──────────────────┐ ┌───────────────────────┐ + │ mecanum │ │ dji-esc-sim │ │ dji-esc-slcan │ + │ 麦轮运动学 │ │ 被控对象 / 场景 │ │ dji-esc-socketcan │ + └───────────┘ │ 离线 harness │ │ 真实总线 ↔ CanLink │ + │ 异步假电调 │ └──────────┬────────────┘ + └────────┬─────────┘ │ + │ CanLink（tokio mpsc 通道对） + ▼ ▼ + ┌────────────────────┐ ┌──────────────────┐ + │ motor-core │◄─│ dji-esc │ + │ 能力 trait / Pid │ │ C620/C610 协议 │ + │ VelocityGains │ │ EscBus 异步驱动 │ + └────────────────────┘ └──────────────────┘ + + ┌────────────────────┐ + │ chassis-config │ chassis.toml schema（chassisd + chassis-tune 共用） + └────────────────────┘ ``` 核心解耦点：`CanLink` 是一对 tokio 通道（发送 / 接收 `CanFrame`）。`dji-esc` 的协议状态机 只面向通道工作，因此： -- 测试中用内存通道 + 一阶电机模型（`dji_esc::sim`）构成闭环仿真，全平台可跑； -- `dji-esc-socketcan` 把真实 SocketCAN 接口桥接到通道，只在实车上出现； +- 闭环仿真由 `dji-esc-sim` 提供两条路径：**异步假电调**（挂在同一对通道上，`chassisd --mock` + 与集成测试用）与**同步 harness**（纯浮点积分、无任务无通道，自动整定用，可并行上千次）； +- `dji-esc-socketcan`（板载原生 SocketCAN）与 `dji-esc-slcan`（CANable 等 USB 适配器， + 串口 Lawicel 协议）只把真实总线桥接到通道，**不含任何应用逻辑**；应用住在 `chassis-bench` + （台架）与 `chassis-tune`（整定）； - 未来更换总线介质（串口转 CAN、MCU 转发）只需新增一个桥接 crate。 +## 被控对象模型（dji-esc-sim） + +逐拍离散（`dt` 秒），电调坐标系（正电流 → 正转速）： + +1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才作用（链路 + 采样延迟）； +2. **摩擦**：运动时 `net = u − sign(rpm)·coulomb − viscous·rpm`；静止且 `|u| > static` 时 + `net = u − sign(u)·coulomb`；静止且 `|u| ≤ static` 时 `net = 0`（粘住）； +3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`， + 以时间常数 `tau` 一阶趋近； +4. **量化**：机械角按转速积分并对 8192 取模，反馈帧是 `EscFeedback`， + 因此角度被截断为 `u16`、转速为 `i16`——量化由协议类型自带。 + +`PlantParams::gm3508()` 是**无摩擦无延迟**的理想模型（`chassisd --mock` 与历史闭环测试依赖它， +行为逐位不变）；`PlantParams::gm3508_bench()` 叠加静 / 库仑摩擦（400~600 raw 量级， +取自台架实测）与 2 拍延迟，是整定的默认被控对象。 + +单点模型整定出来的增益往往对增益 / 时间常数 / 延迟高度敏感，因此 `chassis-tune` 默认在 +`PlantEnsemble`（域随机化：增益 ±20%、时间常数 ±30%、延迟 0~3 拍、摩擦 ±50%）上做鲁棒优化， +而不是针对一条理想曲线做最优化。 + +## 自动整定（chassis-tune） + +两阶段搜索： + +1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数铺满四维空间； +2. **精修**：以成本最低的 top-k 为起点，在**坐标空间**（对数刻度，增益恒正）做多起点 + `argmin` 局部搜索（Nelder-Mead 默认，可选 PSO）。 + +成本 = 归一化加权（调节时间 / 超调 / IAE / 电流）；**硬约束不进权重**：过速、饱和占比过高、 +不收敛直接给罚值并记录判定——否则搜索会主动挑一个会飞车的解。 + +可复现性：每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，粗扫结果排序后再取 top-k， +因此同 seed 两次运行**逐位一致**（CI 的 `just tune-smoke` 依赖这一点）。 + +超参数（参数空间 / 场景 / 权重 / 验收阈值 / 搜索预算 / 被控对象 / 真机安全包络）不在命令行 +默认值里散着，而是版本化在 `config/tune.toml` 的具名预设中（见 +[`preset.rs`](../crates/chassis-tune/src/preset.rs)）。优先级为 +**内置默认 < 预设文件 < 显式 CLI**，逐字段覆盖；`current_limit` / `direction` / `gear_ratio` +不是预设字段，写进预设文件会直接报错（安全参数不允许被预设绕过）。 + +真机后端（`--backend bench`）与仿真后端实现同一个 `Evaluator` trait，区别只是"评估一组增益" +从浮点积分换成真机试验：打开链路 → 上电门控（反馈新鲜）→ 逐次热更新增益 +（`EscMotor::set_velocity_gains`，不重启总线）→ 逐拍重发设定值 + 安全包络 → 任何退出路径 +先 0 电流 → 试验间静默。搜索强制单线程，因为只有一条总线。 + +## 命令租约与链路深度（实车踩过的坑） + +1. **命令是租约**：`EscBus` 在 `command_timeout`（默认 50ms）内没收到新命令就强制 0 输出 + （防上层任务死亡后旧指令持续输出）。所以 `set_velocity` 不是"设一次就完事"，上层必须 + 逐拍重发；只在开头下发一次的台架脚本会在 50ms 后自动失电。 +2. **命令队列必须浅**：驱动按 1kHz 生产命令，而 slcan 一次 write 在 CDC ACM 上约 2ms， + 下行只有几百帧/秒。若 `CanLink` 命令方向缓冲很深（早期是 512），队列会被过时命令灌满， + ESC 执行的是**一秒多以前**算出的电流——速度环等于开了 1.4s 延迟，轮子必然跑飞。 + 现在 slcan 桥的 `CHANNEL_BOUND = 8`：`send().await` 立即回压，每一帧都按最新反馈现算。 + 反馈方向是采样流，来不及就丢（`try_send`），绝不阻塞泵线程。 +3. **桥的两个方向不能互相饿死**：发送要有上界（`TX_BURST`）并在一帧写不动时立刻让位， + 接收要批量排空（`RX_BURST`）。`serial` crate 的 `TTYPort` 读写共用同一个超时， + 写超时属正常现象，不能当致命错误（否则一次写超时就会把接收方向一起带停）。 + ## DJI 电调 CAN 协议要点（C620 手册） | 项目 | 值 | @@ -65,13 +134,29 @@ C620 内部**只闭电流环**：转速 / 位置环由主控侧 PID 实现（`mo | 层级 | 内容 | 平台 | | --- | --- | --- | -| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试 | 全平台 | -| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、守护进程全链路 | 全平台 | +| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试、slcan 帧形状过滤 | 全平台 | +| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、**命令租约到期与续租**、**增益热更新**、守护进程全链路 | 全平台 | +| 模型一致性 | 同步 harness 与 `EscBus` 异步闭环在同一组增益下的稳态 / effort 对齐（防止"对着错模型调参"） | 全平台 | +| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） | +| 超参数预设 | schema 往返、逐字段覆盖与优先级、未知键 / 越界 / 缺失报错、`config/tune.toml` 全部预设可解算、命名与后端一致、安全参数不可注入 | 全平台 | | vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） | +| 实车台架 | `chassis-spin`（`chassis-bench`）四轮定速旋转：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 目标板 + slcan/SocketCAN（人工） | | 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check | CI | ## 部署 `chassisd` 为单二进制守护进程：`--interface can0 --config /etc/chassisd/chassis.toml`。 -systemd 样例见 `deploy/chassisd.service`。板端接口就绪依赖： -USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`。 +systemd 样例见 `deploy/chassisd.service`。板端 CAN 两条路径： + +- **SocketCAN**：USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`； +- **slcan**：USB 适配器（CANable2 等）在 CDC ACM 下枚举为 `/dev/ttyACM0`，由 + [`dji-esc-slcan`](../crates/dji-esc-slcan) 在用户态处理（不需 `ip link`，也绕开了 + gs_usb 驱动）。台架工具 `chassis-spin`（[`chassis-bench`](../crates/chassis-bench)）与 + 自动整定的真机后端走这条路径。整定工具本身是宿主机 / 板端通用的纯 Rust 程序， + `current_limit` 等安全参数由 CLI 显式给出，不参与搜索。 + +## 相关文档 + +- [底盘开发文档](chassis.md)——接线 / 标定 / 上车流程 / 自动整定与超参数预设 / 故障排查 +- [整体架构规划](../ARCHITECTURE.md)——全系统分层与阶段计划 + diff --git a/docs/chassis.md b/docs/chassis.md new file mode 100644 index 0000000..59ff841 --- /dev/null +++ b/docs/chassis.md @@ -0,0 +1,748 @@ +# 底盘开发文档 + +**适用对象**：哨兵机器人麦轮底盘（4 × GM3508 + C620 电调） +**目标平台**：Allwinner T153（Cortex-A7，Linux，armv7）+ CANable2（slcan）或板载 SocketCAN +**控制频率**：1 kHz（2ms 是旧方案；本项目实际为 1ms 节拍） +**文档目的**：从零到上车的完整开发路径——接口约定、标定步骤、自动整定与超参数、安全与故障处理 + +--- + +## 目录 + +1. [范围与文档分工](#1-范围与文档分工) +2. [快速上手](#2-快速上手) +3. [硬件与接线](#3-硬件与接线) +4. [坐标、单位与符号约定](#4-坐标单位与符号约定) +5. [软件架构](#5-软件架构) +6. [关键机制（实车踩过的坑）](#6-关键机制实车踩过的坑) +7. [控制设计](#7-控制设计) +8. [配置参考（chassis.toml）](#8-配置参考chassistoml) +9. [标定与上车流程](#9-标定与上车流程) +10. [自动整定与超参数预设](#10-自动整定与超参数预设) +11. [台架工具](#11-台架工具) +12. [安全设计](#12-安全设计) +13. [测试与质量门禁](#13-测试与质量门禁) +14. [部署](#14-部署) +15. [故障排查](#15-故障排查) +16. [术语表](#16-术语表) + +--- + +## 1. 范围与文档分工 + +| 文档 | 回答的问题 | 读者 | +| --- | --- | --- | +| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 整套 RoboMaster CAN 控制系统的**全系统分层规划**（含云台、发射机构、delta10 IMU、T153 实时性、9 电机） | 架构决策者 | +| [`docs/architecture.md`](architecture.md) | 本仓库**已实现**部分的解耦点：`CanLink` seam、被控对象模型、自动整定两阶段搜索、测试策略 | 改代码的人 | +| **本文** | **底盘这一块**怎么开发、怎么标定、怎么整定、怎么上车、出事怎么办 | 底盘开发 / 调试 / 上车的人 | + +`ARCHITECTURE.md` 里的 `rm-*` crate 划分是**规划**；本仓库实际落地为下面这套 crate 命名 +（功能对应关系见第 5 节）。本文只覆盖底盘切片，云台 / 发射 / IMU 不在范围内。 + +--- + +## 2. 快速上手 + +### 2.1 无硬件（macOS / Linux / CI 均可） + +```sh +cargo test --workspace # 单测 + 闭环仿真 + 整定，全部无需硬件 +cargo run -p chassisd -- --mock # 进程内电机模型的全链路演示 +cargo run -p chassis-tune -- presets # 看有哪些整定超参数预设 +just tune-smoke # 确定性整定冒烟（CI 同款） +just lint # fmt + clippy（与 CI 同标准） +``` + +### 2.2 上车（详细步骤见第 9 节） + +```sh +# 1) 只验链路与反馈，绝不发非零电流（接线排查第一步） +cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run + +# 2) 四轮定性旋转（轮子必须架空或整车固定，急停可及） +cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --rpm 500 + +# 3) 先离线整定，再上台架复验（用版本化的超参数预设） +cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name sim_bench_model +cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name bench_noload \ + --backend bench --device /dev/ttyACM0 --i-know-the-wheel-spins + +# 4) 把结果写回配置并跑守护进程 +cargo run --release -p chassis-tune -- sweep --config config/chassis.toml.example --motor front_left \ + --preset config/tune.toml --preset-name sim_bench_model --write-config /tmp/chassis.tuned.toml +chassisd --config /tmp/chassis.tuned.toml --interface can0 +``` + +--- + +## 3. 硬件与接线 + +### 3.1 拓扑 + +``` +T153 (Linux) Gm3508 × 4 + C620 +├── USB CDC /dev/ttyACM0 ──► CANable2 (slcan 固件) ──► CAN 总线 ──► 4 个电调 +└── 板载 CAN 控制器 can0 ──────────────────────────► （二选一） +``` + +两条路径**互斥地二选一**，应用层无感知（都由 `CanLink` 抽象屏蔽）： + +| 路径 | 打开方式 | 适用 | +| --- | --- | --- | +| **slcan**（`dji-esc-slcan`） | 用户态直接读写 `/dev/ttyACM0`（Lawicel ASCII 协议） | 免 `ip link`、绕开 `gs_usb` 驱动；台架与初始联调首选 | +| **SocketCAN**（`dji-esc-socketcan`） | `ip link set can0 up type can bitrate 1000000 restart-ms 100` | 板载原生 CAN；生产部署 | + +### 3.2 设备与协议常量（C620 手册） + +| 项目 | 值 | +| --- | --- | +| 控制帧 ID | `0x200`（电调 1–4）、`0x1FF`（电调 5–8） | +| 控制帧格式 | 8 字节 = 4 × **i16 大端**电流命令，按电调 ID 顺序 | +| C620 电流标度 | ±16384 ↔ ±20 A | +| C610 电流标度 | ±10000 ↔ ±10 A（M2006 组合；接硬件前需核对） | +| 反馈帧 ID | `0x200 + 电调ID`，即 `0x201`–`0x208` | +| 反馈帧格式 | u16 角度(0..8191) + i16 转速 rpm + i16 转矩电流 + i8 温度℃，均大端 | +| 反馈频率 | 每电调 1 kHz | +| 减速比 | GM3508 = 3591 / 187 ≈ **19.203**（转子 : 输出轴） | + +C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-core::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。 + +### 3.3 电调 ID 与安装方向 + +默认约定（`config/chassis.toml.example`、`chassis-spin` 默认值一致）： + +| 轮位 | 轮序下标 | 电调 ID | 安装方向 | +| --- | --- | --- | --- | +| 前左 FL | 0 | 1 | `+1` | +| 前右 FR | 1 | 2 | `-1` | +| 后左 RL | 2 | 3 | `+1` | +| 后右 RR | 3 | 4 | `-1` | + +左右镜像安装（`direction` 相反）使四轮在"底盘前进"时物理同向。`direction = -1` 的电机 +由**驱动层**在写命令与读反馈两个方向上统一翻转符号，上层永远看到"正转 = 前进方向"。 + +### 3.4 电流限幅 + +| 场景 | 建议限幅（raw） | 说明 | +| --- | --- | --- | +| 空载台架（`chassis-spin` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 | +| 整车实车 | 6000 ~ 12000 | 视供电与机械而定；同时是 PID 抗饱和的钳位点 | + +> **限流不是"越大越安全"**：它同时是 PID 输出的钳位点。设得远大于实际所需， +> 积分会在堵转期间持续积累，一旦起动就冲过头。**必须贴近实际所需电流。** + +### 3.5 接线前置条件（缺一不可） + +- 电调供电与主控**共地**；CAN_H / CAN_L 不接反，两端 120Ω 终端电阻齐全。 +- 轮子**架空**或整车固定；急停开关在手可及处。 +- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`chassis-spin` 会打印每个电调的 + 转速 / 温度 / 反馈年龄）。 + +--- + +## 4. 坐标、单位与符号约定 + +### 4.1 机器人坐标系（ISO） + +- **x 前、y 左、ω 绕 z 逆时针为正**（`mecanum::Twist`）； +- 轮序固定为 **前左 FL、前右 FR、后左 RL、后右 RR**（常量 `mecanum::FL/FR/RL/RR`， + `chassis.toml` 的 `[[motor]]` 顺序、`--esc-ids` / `--directions` 的顺序都遵循它）； +- 辊子 45°、俯视 X 型布置（RoboMaster 标准布局）。 + +### 4.2 运动学（`mecanum`） + +设 `a = half_length + half_width`，四轮线速度： + +``` +v_FL = vx - vy - a·ω +v_FR = vx + vy + a·ω +v_RL = vx + vy - a·ω +v_RR = vx - vy + a·ω +``` + +正解是逆解的左伪逆（JᵀJ 对角，可解析求平均）。实现见 +[`crates/mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` / +`wheel_rpms` / `twist_from_wheel_rpms`。 + +### 4.3 单位与层级（最容易出错的地方） + +| 量 | 所在层 | 单位 | +| --- | --- | --- | +| `Twist.vx/vy` | 应用 / 控制 | m/s | +| `Twist.omega` | 应用 / 控制 | rad/s | +| 轮端转速 | `OutputShaft` 呈现 | rpm（**输出轴**） | +| 转子转速 | `EscBus` / `EscMotor` / `MotorState::rotor_speed_rpm` | rpm（**转子**） | +| 控制量 effort | 驱动 / PID | 协议 **raw**（ESC = 电流，C620 ±16384） | +| `kp/ki/kd/feedforward` | 速度环 | raw/rpm、raw/(rpm·s)、raw/(rpm/s)、raw/rpm | + +- **减速比换算只发生在一个地方**：`motor-core::OutputShaft` 包装（`转子 = 输出轴 × gear_ratio`）。 + `EscBus` / `EscMotor` 只认转子，绝不碰减速比；`EscConfig::gear_ratio` 仅作配置元数据。 +- PID 的接线顺序：`ChassisController` 算出**轮端** rpm → `OutputShaft` 换算成转子 rpm → + `EscMotor` 的速度环以转子 rpm 闭环。 + +--- + +## 5. 软件架构 + +### 5.1 Crate 分层与职责 + +**依赖只能向下**（上层可依赖下层，反之不行）： + +``` +第 4 层 应用 / 工具 chassisd chassis-tune chassis-bench + （含两个 bin） +第 3 层 领域 chassis-config mecanum dji-esc-sim + （schema / 运动学 / 被控对象 + 离线 harness） +第 2 层 协议 / 设备 dji-esc（C620 编解码 + EscBus 驱动 + 命令租约） +第 1 层 抽象 can-link（CanFrame + 通道对） motor-core（能力 trait + PID） +第 0 层 传输 dji-esc-slcan（串口） / dji-esc-socketcan（内核 CAN） +``` + +`dji-esc-slcan` / `dji-esc-socketcan` 只实现 `can-link` 的通道语义，**不含任何应用逻辑**； +它们被第 4 层的工具与守护进程选用，而不是被协议层依赖。 + +| Crate | 职责 | 平台 | +| --- | --- | --- | +| [`motor-core`](../crates/motor-core) | 能力 trait（`VelocityDrive` 等）、`MotorState`、`Pid`、`VelocityController`、`OutputShaft` | 纯逻辑，全平台 | +| [`can-link`](../crates/can-link) | `CanFrame` + `CanLink` 通道对（**无协议知识**） | 纯逻辑，全平台 | +| [`dji-esc`](../crates/dji-esc) | C620/C610 编解码、多圈累计、`EscBus` 异步驱动、命令租约 | 纯逻辑，全平台 | +| [`dji-esc-sim`](../crates/dji-esc-sim) | 被控对象模型、场景、**离线 harness**、异步假电调 | 纯逻辑，全平台 | +| [`dji-esc-slcan`](../crates/dji-esc-slcan) | slcan（Lawicel）串口 ↔ `CanLink` 桥接 | 全平台 | +| [`dji-esc-socketcan`](../crates/dji-esc-socketcan) | SocketCAN ↔ `CanLink` 桥接 | Linux | +| [`mecanum`](../crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 | +| [`chassis-config`](../crates/chassis-config) | `chassis.toml` schema：解析 / 校验 / 写回 | 全平台 | +| [`chassis-bench`](../crates/chassis-bench) | 台架：安全包络、采样、`chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） | +| [`chassis-tune`](../crates/chassis-tune) | 速度环 PID 自动整定 + **超参数预设** | 全平台 | +| [`chassisd`](../crates/chassisd) | 底盘守护进程：1 kHz 循环、安全逻辑、指令源 seam | 全平台（实车需 Linux） | + +### 5.2 依赖方向（硬约束） + +``` +chassisd ─┬─► chassis-config ─► motor-core, mecanum + ├─► dji-esc ──► can-link, motor-core + ├─► dji-esc-sim ─► dji-esc（协议类型）, can-link + └─► dji-esc-socketcan ─► can-link + +chassis-tune ─┬─► dji-esc-sim（离线 harness） + ├─► chassis-bench（安全包络 + 采样） + ├─► chassis-config（共用 schema） + └─► dji-esc ─► can-link, motor-core + +chassis-bench ─┬─► dji-esc ─► can-link, motor-core + ├─► dji-esc-sim（场景 / 轨迹类型） + ├─► dji-esc-slcan / dji-esc-socketcan + └─► chassis-config +``` + +**传输后端 crate 只做桥接，不含应用逻辑**：`chassis-spin` 曾住在 `dji-esc-slcan` 的 +example 里，现在统一收归 `chassis-bench`；整定逻辑住在 `chassis-tune`。要换总线介质 +（串口转 CAN、MCU 转发），只需**新增一个桥接 crate**，协议 / 控制 / 整定全部不动。 + +### 5.3 一个 1 ms 拍里发生了什么 + +``` +tick (1ms) + ├─ 读取 watch（有新值则更新，记录时间戳） + ├─ 指令超时判定（默认 200ms）→ 超时则 Twist::ZERO + ├─ 幅值限幅 Twist::clamped(max_vx, max_vy, max_omega)（按比例收缩，保方向） + ├─ 上电门控：四轮是否都收到过反馈？否则全部 disable 并告警 + ├─ 麦轮逆解 → 四轮轮端 rpm + ├─ OutputShaft 换算 → 转子 rpm + └─ set_velocity（续租命令）→ EscBus 在自身 1 kHz 节拍上组帧、发送 +``` + +同时 `EscBus` 侧持续：接收反馈帧 → 分发到各 `EscMotor` → 更新 `MotorState` +→ 检查反馈离线（默认 100 ms）→ 检查命令租约（默认 50 ms）。 + +--- + +## 6. 关键机制（实车踩过的坑） + +### 6.1 命令是**租约**，不是设定值 + +`EscMotor::set_velocity` / `set_effort_raw` / `set_position` **每次调用即续租**。超过 +`BusOptions::command_timeout`（默认 **50 ms**）没有新命令，驱动强制 0 电流。 + +- 这是"上层任务死亡后旧指令继续输出"的防线； +- 因此上层必须**逐拍重发**同一个设定值（`chassisd` 的 1 kHz 循环天然满足）； +- 只在开头下发一次的台架脚本会在 50 ms 后自动失电——**这是预期行为，不是 bug**。 + +### 6.2 命令队列必须浅 + +驱动按 1 kHz 生产命令，而 slcan 一次 write 在 CDC ACM 上约 2 ms（下行只有几百帧/秒）。 +若命令方向缓冲很深，队列会被过时命令灌满，ESC 执行的是**一秒多以前**算出的电流—— +速度环等于开了 1.4 s 延迟，轮子必然跑飞。 + +因此 slcan 桥的 `CHANNEL_BOUND = 8`：`send().await` 立即回压，每一帧都按最新反馈现算； +反馈方向是采样流，来不及就丢（`try_send`），**绝不阻塞泵线程**。 + +### 6.3 桥的两个方向不能互相饿死 + +- 发送有上界（`TX_BURST`），一帧写不动时立刻让位给接收； +- 接收批量排空（`RX_BURST`）； +- `serial` crate 的 `TTYPort` 读写**共用同一个超时**，写超时属正常现象，**不能当致命错误** + （否则一次写超时就会把接收方向一起带停）。 + +### 6.4 反馈离线 + +任一电机反馈超时（`feedback_timeout`，默认 **100 ms**）→ 该电机命令强制 0 电流并告警。 +顶层另有**上电门控**：任一电机从未收到过反馈前，整组不下发速度命令。 + +--- + +## 7. 控制设计 + +### 7.1 PID（`motor-core::Pid`） + +并联式 PID，显式时间步 `dt`（秒），特性： + +1. **输出限幅** `[out_min, out_max]`（速度环里取 `±current_limit`）； +2. **积分抗饱和**（clamping）：输出已饱和且误差继续同向时**暂停积分**； +3. **微分作用于测量值**（不是误差），避免设定值阶跃的微分冲击； +4. 可选**微分一阶低通**（`with_derivative_tau`），抑制编码器量化噪声； +5. `dt <= 0` 或非有限输入 → 返回上次输出且不更新内部状态（防御跳帧 / 首帧）。 + +### 7.2 速度环（`motor-core::VelocityController`） + +``` +effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rpm, ±limit ) +``` + +`feedforward` 单位 **raw / rpm**：稳态下所需控制量与转速近似成正比。 + +- **空载台架必须 ff = 0**：轮子架空时维持转速只需克服摩擦的极小电流，任何接近 + `1/K`（GM3508 空载 `K ≈ 0.57 rpm/raw` → `1/K ≈ 1.77`）的前馈都会把空载轮子直接推到最高转速。 +- **带载**才需要正前馈（抵消地面摩擦），典型 ≈ 1.77 raw/rpm，必须按实车整定。 +- 前馈可能把输出推出限幅，因此 `update` 在加完前馈后**再夹一次**。 + +### 7.3 增益的物理直觉（空载台架） + +| 现象 | 原因 | 方向 | +| --- | --- | --- | +| 高 `kp`（8~18）震荡 | 被控对象近似积分器（≈12 rpm/s per raw）+ 反馈延迟 | 降 `kp`、提 `ki` | +| `ki` 太小长期停在 300~400 rpm | 静摩擦需要积分缓慢"顶"到起动电流（stick-slip） | 提 `ki`（有 `current_limit` 钳位兜底） | +| 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 | +| 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 | + +### 7.4 被控对象模型（`dji-esc-sim`） + +逐拍离散（`dt` 秒，电调坐标系：正电流 → 正转速）： + +1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才作用； +2. **摩擦**：运动时 `net = u − sign(rpm)·coulomb − viscous·rpm`；静止且 `|u| > static` + 时 `net = u − sign(u)·coulomb`；静止且 `|u| ≤ static` 时 `net = 0`（粘住）； +3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，以时间常数 `tau` 一阶趋近； +4. **量化**：机械角按转速积分并对 8192 取模，角度截断为 `u16`、转速为 `i16` + （量化由协议类型自带）。 + +`PlantParams::gm3508()` 是**无摩擦无延迟**的理想模型；`gm3508_bench()` 叠加实测量级的 +静 / 库仑摩擦（400~600 raw）与 2 拍延迟，是整定的默认被控对象。 + +--- + +## 8. 配置参考（chassis.toml） + +样例：[`config/chassis.toml.example`](../config/chassis.toml.example)。 +schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程与整定工具共用同一份**—— +这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。 + +### 8.1 顶层 + +| 键 | 含义 | +| --- | --- | +| `[can] interface` | 底盘总线 SocketCAN 接口名（如 `can0`） | +| `[can.function] interface` | 预留的功能总线（云台 / 拨弹），必须与底盘总线**不同名** | +| `[chassis]` | 控制循环与安全超时、速度上限 | +| `[mecanum]` | `half_length` / `half_width` / `wheel_radius`（米） | +| `[[motor]]` × 4 | 顺序固定 FL / FR / RL / RR | + +### 8.2 `[chassis]`（缺省值） + +| 键 | 默认 | 含义 | +| --- | --- | --- | +| `control_period_ms` | 1 | 控制节拍（1 kHz） | +| `command_timeout_ms` | 200 | 上层 twist 超时 → 目标速度归零 | +| `feedback_timeout_ms` | 100 | 反馈离线 → 强制 0 电流 | +| `motor_command_timeout_ms` | 50 | 驱动层命令租约（必须 ≥ `control_period_ms`） | +| `max_vx` / `max_vy` / `max_omega` | 3.0 / 3.0 / 6.0 | 速度限幅（m/s、m/s、rad/s） | + +### 8.3 `[[motor]]` + +| 键 | 含义 | +| --- | --- | +| `name` | 轮名（`front_left` …），整定的 `--motor` 用它选目标 | +| `esc_id` | 电调 ID（1–8，即 CAN 拨码） | +| `model` | `C620` / `C610` | +| `direction` | 安装方向 `+1` / `-1` | +| `gear_ratio` | 减速比（转子 : 输出轴），GM3508 ≈ 19.203 | +| `current_limit` | effort 限幅（raw），同时是 PID 钳位点 | +| `kp` / `ki` / `kd` / `feedforward` | 速度环增益（整定的产物写回这里） | + +### 8.4 校验规则（`Config::validate`） + +- 电机数必须**恰好 4**；麦轮几何必须为正；四个超时必须 > 0； + `motor_command_timeout_ms ≥ control_period_ms`；速度上限 > 0；`gear_ratio > 0`； +- 底盘总线**只允许一种协议族**（四轮不能混用 C620 / C610）； +- 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）； +- `deny_unknown_fields`：**拼错的键直接报错**，不会被静默忽略。 + +--- + +## 9. 标定与上车流程 + +> 每一步都有明确的**通过判据**。不要跳步——跳过的步骤会在后面以"轮子跑飞"的形式还回来。 + +| # | 步骤 | 命令 | 通过判据 | +| --- | --- | --- | --- | +| 1 | **链路与反馈** | `chassis-spin --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID | +| 2 | **电调 ID 对应** | 同 1，逐个拔插 | 拔掉哪个电调，哪个 ID 的反馈消失 → ID 映射正确 | +| 3 | **方向** | `chassis-spin --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` | +| 4 | **减速比 / 轮径** | 手转一圈 + 读反馈角度 | 反馈角度 × 减速比 ≈ 轮子转过角度；不符则改 `gear_ratio` | +| 5 | **几何参数** | 卷尺量 `half_length` / `half_width` / `wheel_radius` | 填入 `chassis.toml`（影响运动学精度，不影响稳定性） | +| 6 | **限流** | 从 1500 raw 起，逐步加到"能可靠起动且不冲" | 越小越好；实车再按需上调 | +| 7 | **空载整定** | `chassis-tune sweep --preset-name bench_noload` | 出一组增益：超调 ≤ 25%、稳态误差 ≤ 8%、三次复验一致 | +| 8 | **带载复验** | 整车落地，`--preset-name bench_loaded` | 超调 ≤ 15%、稳态误差 ≤ 5%；空载参数直接上车通常**不行** | +| 9 | **双向验收** | `--preset-name field_updown` | 升速 / 降速 / 回零三个方向都达标，重复性 ≥ 4/5 | +| 10 | **固化** | `--write-config` + 人工评审 | 增益写入 `chassis.toml`，限流 / 方向 / 减速比未被改动 | + +**关于"仿真整定能不能直接上车"**：不能。仿真最优解只在被控对象模型内最优，模型里的 +摩擦 / 延迟是量级估计且不含测量噪声，因此可能偏激进。必须用同一场景在台架上复验 +（第 10.6 节的迁移证据）。 + +--- + +## 10. 自动整定与超参数预设 + +手动试凑 `kp/ki/kd/ff` 既慢又难复现。`chassis-tune` 把它变成**参数空间搜索 + 版本化超参数**。 + +### 10.1 搜索算法 + +两阶段： + +1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数铺满四维空间； +2. **精修**：以成本最低的 top-k 为起点，在**坐标空间**做多起点 `argmin` 局部搜索 + （Nelder-Mead 默认，可选 PSO）。 + +**坐标空间**是关键的数值技巧：`kp/ki/kd` 合法取值跨数量级且必须为正，每个轴映射到 +（对数轴取 `ln`，线性轴恒等）之后—— + +- 无约束优化器不会走出正增益区域； +- 步长在不同数量级上等价（±0.1 坐标步长 = ×1.1 比例步长）； +- `LogOrZero` 轴在**粗扫**阶段可精确取 0（`ff = 0` 是台架常态），在**精修**阶段退化为对数轴 + （零不可导，本地搜索无收益）。 + +### 10.2 成本函数与硬约束 + +成本 = 归一化加权（调节时间 / 超调 / IAE / 电流），各项已归一化到 O(1)，因此权重在不同 +场景 / 电机之间可解释。 + +**硬约束不进权重**：过速、饱和占比过高、不收敛 → 直接给罚值 `1e9` 并记录判定。 +否则搜索会主动挑一个"飞车但其它项很好看"的解。 + +| 判定 | 触发条件 | +| --- | --- | +| `rejected` | 增益含 NaN/inf，或场景退化 | +| `diverged` | 数值发散 | +| `overspeed` | 峰值转速 > 目标 × 3 + 500 | +| `saturated` | 饱和占比 > 40% | +| `no-settle` | 场景内未收敛 | + +### 10.3 鲁棒优化（默认） + +单点模型整定出的增益对增益 / 时间常数 / 延迟高度敏感，因此默认在 `PlantEnsemble` +（域随机化：增益 ±20%、时间常数 ±30%、延迟 0~3 拍、摩擦 ±50%，8 个成员）上评估， +成本按 `robust_weight` 在"均值"与"最坏成员"之间折衷（默认 0.5）。 + +### 10.4 超参数预设（`config/tune.toml`） + +**这是"实验条件"的版本管理**：搜什么区间、用哪个场景、什么算好、什么算达标、搜多细， +全部写在 [`config/tune.toml`](../config/tune.toml) 里，评审一次、长期复用、任何机器上可复现。 + +优先级（**逐字段**覆盖）： + +``` +内置默认 < 预设文件里的预设 < 显式命令行参数 +``` + +因此"用预设跑一次 + 临时改一个权重"是安全的，不会把其它超参数打回硬编码值。 + +```sh +chassis-tune presets # 列出全部预设（只读） +chassis-tune sweep --preset config/tune.toml # 用 default_preset +chassis-tune sweep --preset config/tune.toml --preset-name bench_loaded +chassis-tune sweep --preset-name bench_loaded # 缺省路径即 config/tune.toml +chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预设的单项 +``` + +**随仓库提供的预设**： + +| 预设名 | 后端 | 场景 | 用途 | +| --- | --- | --- | --- | +| `sim_bench_model`（默认） | sim | ramp-and-hold 1500 rpm | 台架量级模型全量搜索：评审、CI、代码回归的基准 | +| `sim_ideal` | sim | ramp-and-hold 1500 rpm | 理想模型秒级回归；**结果不可直接上车** | +| `bench_noload` | bench | ramp-and-hold 800 rpm | 空载台架：小样本局部搜索，**前馈固定为 0** | +| `bench_loaded` | bench | ramp-and-hold 1200 rpm | 带载（整车落地）：前馈区间上调至 0.5~2.5 | +| `field_updown` | bench | **up-down** 1200 rpm | 实战验收：升 / 降 / 回零双向，阈值收紧到 12% / 3% | + +**预设 schema**（`version = 1`，未知键报错，见 +[`crates/chassis-tune/src/preset.rs`](../crates/chassis-tune/src/preset.rs)）： + +```toml +version = 1 +default_preset = "sim_bench_model" + +[preset.bench_noload] +description = "空载台架：摩擦小、静差靠积分压制" +backend = "bench" + +[preset.bench_noload.space] +kp = { min = 0.2, max = 12.0, scale = "log" } # scale: linear | log | log0 +feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜索时恒定 + +[preset.bench_noload.cost] # settle / overshoot / iae / effort +[preset.bench_noload.acceptance] # max_overshoot_pct / max_ss_error_pct +[preset.bench_noload.search] # seed / samples / top_k / solver / max_iters / particles / max_evals / threads +[preset.bench_noload.sim] # plant / ensemble / robust_weight / gain_pct / tau_pct / delay_ticks_max / friction_pct +[preset.bench_noload.bench] # max_rpm / max_temp_c / trial_timeout_ms / between_trials_ms / + # startup_timeout_ms / continue_on_abort / validate_runs +``` + +每个小节都可缺省，只写"与默认不同"的项；刻度（`scale`）与区间可以分别覆盖 +（`--kp-scale linear` 不改区间）。 + +> **安全边界**：`current_limit`、`direction`、`gear_ratio`、电调 ID、串口设备名 +> **不是**预设字段——写进预设文件会直接报错。它们只来自 `chassis.toml` 或显式 CLI， +> 否则预设文件就成了绕过限流的口子。 + +**新增一个预设**：在 `config/tune.toml` 里加一节 → 跑 +`chassis-tune presets` 确认能解算 → 跑一次小样本 +`chassis-tune sweep --preset config/tune.toml --preset-name <新名> --samples 16` 确认可用。 +测试会强制：每个预设都有 `description`；`bench_*` / `field_*` 必须 `backend = "bench"` +（防止"以为在台架验过、其实是离线跑的"）。 + +### 10.5 产物与判读（`--out-dir`） + +| 文件 | 内容 | +| --- | --- | +| `trials.csv` | 全部候选 + 判定（可复现性 diff 的基准） | +| `best.toml` | 可直接粘贴的增益片段 | +| `best-trace.csv` | 最优解的轨迹 | +| `report.md` | 人类可读报告：参数空间、权重、验收结论、**边界提示** | +| `summary.json` | 机器可读摘要 | +| `hil-validation.csv` | 仅真机后端：同一组增益的重复性复验 | + +判读要点： + +1. **最优解贴到参数空间边界**时报告会明确提示——那是"约束在决定结果"，应放宽 + `--kp` / `--ki` 再搜，而不是把边界值当最优解交付； +2. **达标数 / 评估数**太低（如 9/198）说明大部分候选被硬约束拦下，通常意味着限流太小 + 或参数空间太宽； +3. 真机 `hil-validation.csv` 的**离散度**比均值更重要：三次结果差很多 = 参数贴着噪声，不该上车。 + +### 10.6 真机后端与迁移证据 + +`--backend bench` 与仿真实现同一个 `Evaluator` trait，区别只是"评估一组增益"从浮点积分 +换成真机试验：打开链路 → 上电门控 → 逐次**热更新**增益（不重启总线）→ 逐拍重发设定值 + +安全包络 → 任何退出路径先 0 电流 → 试验间静默。搜索强制单线程（只有一条总线）。 + +``` +仿真整定 → 台架复验（同场景、同阈值）→ 重复性达标 → 写回 chassis.toml → 整车验收 +``` + +**可复现性**：同一 `seed` + 同一参数空间 + 同一后端 → **逐位一致**。CI 的 +`just tune-smoke` 依赖这一点（同一命令跑两次，`diff trials.csv` 必须为空）。 + +--- + +## 11. 台架工具 + +### 11.1 `chassis-spin`（`chassis-bench`，slcan，全平台） + +四轮同时以固定**转子**转速旋转。关键参数： + +| 参数 | 默认 | 说明 | +| --- | --- | --- | +| `--device` | `/dev/ttyACM0` | slcan 串口设备 | +| `--serial-baud` | 2000000 | CDC ACM 一般忽略；CH340 类需与固件一致 | +| `--serial-timeout-ms` | 50 | 读写共用 | +| `--can-speed` | `1m` | `1m` / `500k` | +| `--esc-ids` | `1,2,3,4` | 顺序 = FL、FR、RL、RR | +| `--directions` | `1,-1,1,-1` | 各轮安装方向 | +| `--rpm` | 500 | 目标**转子**转速（轮端 = 转子 ÷ 减速比） | +| `--gear-ratio` | 19.203 | 减速比 | +| `--current-limit` | 1500 | raw；同时是 PID 钳位点 | +| `--ramp-ms` | 3000 | 缓升时间（让积分在起动前平滑建立，避免"先憋住、一动就飞"） | +| `--hold-secs` | 10 | 目标转速保持时长 | +| `--startup-timeout-ms` | 3000 | 等待全部电调反馈的超时 | +| `--dry-run` | — | 只验证链路与反馈，**不发任何非零电流** | +| `--allow-partial` | — | 允许缺失部分电调（默认要求四个全在线） | +| `--kp/--ki/--kd/--feedforward` | 2 / 10 / 0 / 0 | 空载台架默认值（见 7.3） | +| `--max-rpm` | 目标 × 2 + 200 | 过速保护上限 | + +安全与使用要点： + +- **轮子必须架空或整车固定**；ctrl-c 随时安全停机（先 0 电流再关总线）； +- 过速保护：任一电调超过 `--max-rpm` 持续 60 ms 即立即停机； +- 上电门控：四个电调都上报新鲜反馈前绝不下发速度； +- 空载的合适整定与带载**完全不同**，空载**不要**加前馈。 + +### 11.2 `socketcan-spin`（`chassis-bench`，仅 Linux） + +单电机链路验证，走 SocketCAN。用于板载原生 CAN 的接线排查： + +```sh +ip link set can0 up type can bitrate 1000000 restart-ms 100 +cargo run --release -p chassis-bench --bin socketcan-spin -- --interface can0 --dry-run +``` + +--- + +## 12. 安全设计 + +按"故障点 → 防线"排列，每一层都独立生效： + +| # | 故障 | 防线 | 默认阈值 | +| --- | --- | --- | --- | +| 1 | 上层任务死亡，旧指令继续输出 | **命令租约**：超时未刷新 → 0 输出 | 50 ms | +| 2 | 电机 / CAN 掉线 | **反馈离线**：超时 → 该电机 0 电流 + 告警 | 100 ms | +| 3 | 上电瞬间反馈未到就发速度 | **上电门控**：全部见到反馈前全体 `disable` | — | +| 4 | 上层不再发指令 | **指令超时**：twist 归零 | 200 ms | +| 5 | 轮子飞车 | 台架 **过速保护**（超过上限持续 60 ms 停机） | 目标 × 2 + 200 rpm | +| 6 | 堵转烧电机 | **限流**（同时是 PID 钳位点，抗饱和） | 台架 1500 / 实车 6000~12000 raw | +| 7 | 电调过热 | 台架**温度保护** | 70 ℃（空载预设 60 ℃） | +| 8 | 整定 / 台架脚本失控 | 试验**超时** + 立即 0 电流 + 试验间静默 | 6 s / 500 ms | +| 9 | 误触真机模式 | `--i-know-the-wheel-spins` **强制显式确认**（预设也无法绕过） | — | +| 10 | 命令队列积压导致延迟 | slcan 桥 `CHANNEL_BOUND = 8` + `send().await` 回压 | — | +| 11 | 进程被杀 | systemd `Restart=always`（板端） | 1 s | + +**出事时的正确操作顺序**：① 急停 / 断电 → ② ctrl-c 让进程走优雅停机（先 0 电流）→ +③ 记下 `--dry-run` 与 `hil-validation.csv` 的现象 → ④ 再分析。 + +--- + +## 13. 测试与质量门禁 + +### 13.1 分层测试 + +| 层级 | 内容 | 平台 | +| --- | --- | --- | +| 单元 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试、slcan 帧形状过滤 | 全平台 | +| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、命令租约到期与续租、增益热更新、守护进程全链路 | 全平台 | +| 模型一致性 | 同步 harness 与 `EscBus` 异步闭环在同一组增益下的稳态 / effort 对齐（防止"对着错模型调参"） | 全平台 | +| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） | +| **预设** | schema 往返、逐字段覆盖、优先级（CLI > 预设 > 内置）、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 | +| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） | +| 实车台架 | `chassis-spin` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） | +| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check；armv7 静态构建 `chassisd` | CI | + +### 13.2 每个阶段完成后的门禁（`just lint` + 以下） + +```sh +cargo build --workspace # 无警告 +cargo test --workspace # 全绿 +cargo clippy --workspace --all-targets -- -D warnings +cargo fmt --all -- --check +RUSTDOCFLAGS="-D warnings" cargo doc --workspace --no-deps +cargo check -p chassis-tune --features plot --all-targets # 可选 feature 防腐烂 +just tune-smoke # 同 seed 逐位一致 + 达标 +``` + +新增代码要求：有文档注释；关键路径有测试覆盖；**跨 crate 的常量（如限流、减速比）只有一个来源**。 + +--- + +## 14. 部署 + +### 14.1 交叉编译（musl 静态，无需交叉工具链） + +```sh +just build-armv7 # cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf +just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2） +``` + +目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。二进制静态链接，不依赖板端 libc。 + +### 14.2 板端拉起 CAN + +```sh +# SocketCAN（板载原生控制器） +ip link set can0 type can bitrate 1000000 restart-ms 100 +ip link set can0 up +ip -d link show can0 + +# slcan（USB 适配器）：不需要 ip link，应用层直接打开串口 +chmod 666 /dev/ttyACM0 +``` + +### 14.3 systemd + +[`deploy/chassisd.service`](../deploy/chassisd.service)： + +```ini +ExecStart=/usr/local/bin/chassisd --interface can0 --config /etc/chassisd/chassis.toml +Restart=always +RestartSec=1 +# 实时性（按需打开）： +# CPUSchedulingPolicy=fifo +# CPUSchedulingPriority=20 +# LimitRTPRIO=95 +``` + +> 电调未上电 / CAN 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off +> （`ip -d link show can0` 可见），`chassisd` 侧表现为四电机反馈离线、强制零电流—— +> **这正是安全设计的预期行为**，不是故障。 +> +> 调度策略：`ARCHITECTURE.md` 规划了 `SCHED_FIFO` + `mlockall`（对应 `rm-app::realtime`）， +> 本仓库目前**未实现**实时优先级提升，当前只依赖 1 kHz tokio 定时器 + +> `MissedTickBehavior::Skip`。板端抖动实测与是否需要加固见第 15 节的抖动条目。 + +--- + +## 15. 故障排查 + +| 症状 | 可能原因 | 处理 | +| --- | --- | --- | +| `/dev/ttyACM0` 打不开 | 权限 / 设备名不符 / 固件非 slcan | `ls -l /dev/ttyACM*`；`chmod 666`；确认适配器是 Lawicel 固件（不是 gs_usb） | +| 四个电调都无反馈 | CAN_H/L 接反、无终端电阻、电调未上电、波特率不符 | 万用表量差分电阻（约 60Ω）；`--dry-run` 复查；确认 `--can-speed 1m` | +| 只有部分电调有反馈 | 电调 ID 冲突 / 拨码错 / 该段线断 | 逐个拔插定位（流程第 2 步） | +| 上电后轮子纹丝不动 | 正常：上电门控未满足 | 等反馈同时打印"waiting for first feedback"；查反馈丢失原因 | +| 台架脚本 50 ms 后失电 | 命令租约到期（只下发了一次） | 每拍重发命令；这不是 bug | +| 轮子"先憋住、一动就飞" | 前馈过大 / 限流远大于实际所需 / 缓升太短 | ff → 0；限流贴近实际；`--ramp-ms` 加大 | +| 速度环高频震荡 | `kp` 太大（近似积分器 + 延迟） | 降 `kp`、提 `ki`；参考第 7.3 节 | +| 稳态长期差 100+ rpm | 静摩擦未被积分吃掉 | 提 `ki`；确认 `current_limit` 大于静摩擦（400~600 raw） | +| 整定报告说"贴边界" | 约束在决定结果 | 放宽对应轴区间再搜，别交付边界值 | +| 仿真整定的参数上车就飞 | 模型不含摩擦 / 噪声，仿真解偏激进 | 用 `bench_loaded` / `field_updown` 复验；以台架结果为准 | +| `trials.csv` 两次不一致 | 改了参数空间 / 预设 / 后端 | 检查 `超参数来源` 打印行；同 seed 必须同空间同后端 | +| 守护进程起来就退出 | 配置校验失败 / `--config` 缺失 | 看 stderr；`Config::validate` 的报错会指出具体键 | +| 抖动大（>500 µs） | 标准 Linux 非硬实时 | 先降 `chassis` 控制频率或合并任务；再考虑调度优先级 / 异构核（`ARCHITECTURE.md` 决策点 D4） | + +--- + +## 16. 术语表 + +| 术语 | 含义 | +| --- | --- | +| SLCAN | Serial CAN，通过串口传输 CAN 帧的 ASCII（Lawicel）协议 | +| SocketCAN | Linux 内核 CAN 子系统 | +| raw | 协议原始控制量单位（C620 电流 ±16384 ↔ ±20 A） | +| 命令租约 | `set_*` 每次调用续租，超时未刷新即强制 0 输出 | +| 上电门控 | 未见全部电机反馈前不下发速度命令 | +| 转子 / 轮端 | 电机轴 / 减速器输出轴；换算只在 `OutputShaft` 发生 | +| 域随机化 | 在一组扰动参数（增益 / 时间常数 / 延迟 / 摩擦）上做鲁棒优化 | +| 代价空间 | 对数（或恒等）映射后的参数坐标，使正增益约束天然满足 | +| 超参数预设 | 版本化的搜索条件：空间 / 场景 / 权重 / 阈值 / 预算 / 被控对象 / 安全包络 | +| HIL | Hardware-in-the-loop，此处指 `--backend bench` 的真机逐次试验 | + +--- + +## 相关文档 + +- [整体架构规划 `ARCHITECTURE.md`](../ARCHITECTURE.md) +- [已实现架构与解耦点 `docs/architecture.md`](architecture.md) +- [配置样例 `config/chassis.toml.example`](../config/chassis.toml.example) +- [整定超参数预设 `config/tune.toml`](../config/tune.toml) +- [项目 README](../README.md) + +| 版本 | 日期 | 变更 | +| --- | --- | --- | +| 1.0 | 2026-09-23 | 首版：底盘接口约定、标定流程、自动整定与超参数预设、安全与故障处理 | diff --git a/justfile b/justfile index 2ac9c60..ceab6be 100644 --- a/justfile +++ b/justfile @@ -37,12 +37,69 @@ build-armv7: cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf # 推送到板子并试运行（默认 mock 模式） -deploy board="root@169.254.63.4" mode="--mock": +deploy board="root@192.168.2.2" mode="--mock": just build-armv7 ssh {{board}} "mkdir -p /tmp/chassisd" scp -q target/armv7-unknown-linux-musleabihf/release/chassisd {{board}}:/tmp/chassisd/chassisd ssh {{board}} "/tmp/chassisd/chassisd {{mode}}" -# 板端：拉起 CAN 接口（1Mbps，bus-off 后 100ms 自动恢复） -can-up board="root@169.254.63.4" iface="can0": +# 板端：拉起 SocketCAN 接口（1Mbps，bus-off 后 100ms 自动恢复） +# 注意：slcan 适配器（CANable 系列）走串口，不走 SocketCAN，台架用下面的 bench-* 配方。 +can-up board="root@192.168.2.2" iface="can0": ssh {{board}} "ip link set {{iface}} down; ip link set {{iface}} up type can bitrate 1000000 restart-ms 100 && ip -br link show {{iface}}" + +# 交叉编译台架工具（armv7，musl 静态，无需交叉工具链） +build-bench: + rustup target add armv7-unknown-linux-musleabihf + cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf + +# 台架：四轮同时 500 rpm（转子）旋转 —— 编译 → 推送 → 板端运行 +# 轮子必须架空或整车固定；ctrl-c 安全停机。 +bench-spin board="root@192.168.2.2" device="/dev/ttyACM0" rpm="500" opts="": + just build-bench + scp -q target/armv7-unknown-linux-musleabihf/release/chassis-spin {{board}}:/tmp/chassis-spin + ssh {{board}} "/tmp/chassis-spin --device {{device}} --rpm {{rpm}} {{opts}}" + +# 台架：只验证 slcan 链路与四轮反馈（不发非零电流） +bench-check board="root@192.168.2.2" device="/dev/ttyACM0": + just build-bench + scp -q target/armv7-unknown-linux-musleabihf/release/chassis-spin {{board}}:/tmp/chassis-spin + ssh {{board}} "/tmp/chassis-spin --device {{device}} --dry-run" + +# 自动整定：列出超参数预设（只读；预设文件 config/tune.toml） +tune-presets preset="config/tune.toml": + cargo run -p chassis-tune -- presets --preset {{preset}} + +# 自动整定：用预设跑（超参数进版本管理；CLI 只覆盖本次要变的项） +# 例：just tune-preset bench_loaded "--samples 16" +tune-preset name="sim_bench_model" opts="" preset="config/tune.toml": + cargo run --release -p chassis-tune -- sweep --preset {{preset}} --preset-name {{name}} {{opts}} + +# 自动整定：仿真搜索（不碰硬件；同 seed 结果逐位一致） +tune-sim samples="512" seed="20260101": + cargo run --release -p chassis-tune -- sweep --samples {{samples}} --seed {{seed}} --out-dir target/tune + +# 自动整定：冒烟（确定性、秒级；CI 与提交前自检用） +tune-smoke samples="64" seed="7": + cargo run --release -p chassis-tune -- sweep --samples {{samples}} --top-k 3 --max-iters 40 --ensemble 2 --seed {{seed}} --out-dir target/tune-smoke + +# 自动整定：复算一组给定增益（退出码 0 通过 / 1 未通过 / 2 参数错） +tune-validate gains="2,10,0,0": + cargo run --release -p chassis-tune -- validate --gains {{gains}} + +# 自动整定：带 SVG 图输出（需要 --features plot） +tune-plot samples="128": + cargo run --release -p chassis-tune --features plot -- sweep --samples {{samples}} --out-dir target/tune-plot + +# 自动整定：把结果写回一份配置（需 --config 指向现有 chassis.toml） +tune-write config="config/chassis.toml.example" motor="front_left" out="target/tune/chassis.tuned.toml": + cargo run --release -p chassis-tune -- sweep --config {{config}} --motor {{motor}} --write-config {{out}} --samples 256 --out-dir target/tune + +# 自动整定：**真机**逐次试验（轮子会转！轮子架空或整车固定，急停可及） +# 例：just tune-bench board="root@192.168.2.2" device="/dev/ttyACM0" esc_id="1" samples="24" +# 预设提供场景 / 参数空间 / 权重 / 阈值 / 安全包络；CLI 提供接线与样本数（CLI 优先）。 +tune-bench board="root@192.168.2.2" device="/dev/ttyACM0" esc_id="1" samples="24" preset="bench_noload" opts="": + cargo build --release -p chassis-tune + scp -q target/release/chassis-tune {{board}}:/tmp/chassis-tune + scp -q config/tune.toml {{board}}:/tmp/tune.toml + ssh {{board}} "/tmp/chassis-tune sweep --backend bench --preset /tmp/tune.toml --preset-name {{preset}} --device {{device}} --esc-id {{esc_id}} --samples {{samples}} --top-k 3 --i-know-the-wheel-spins {{opts}}"