diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index cae40c3..91e8a68 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,7 +10,6 @@ env: jobs: lint-and-test: - name: fmt / clippy / test / doc runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 @@ -18,58 +17,47 @@ jobs: with: components: rustfmt, clippy - uses: Swatinem/rust-cache@v2 - - name: rustfmt - run: cargo fmt --all -- --check - - name: clippy - run: cargo clippy --workspace --all-targets -- -D warnings - - name: test - run: cargo test --workspace - - name: doc - env: - RUSTDOCFLAGS: -D warnings - run: cargo doc --workspace --no-deps - # 整定工具的可选 feature 不在默认 clippy/test 里，单独编译一次防止腐烂。 - - name: chassis-tune plot feature + - run: cargo fmt --all -- --check + - run: cargo clippy --workspace --all-targets -- -D warnings + - run: cargo test --workspace --all-targets + - name: enforce flat crate source + run: test -z "$(find crates/*/src -mindepth 2 -type d -print)" + - name: forbid unsafe and no_std markers + run: "! rg '\\bunsafe\\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml'" + - name: optional plot feature run: cargo check -p chassis-tune --features plot --all-targets - # 自动整定的确定性冒烟：同 seed 必须逐位一致，且最优解必须通过验收阈值。 - - name: tune smoke (deterministic, no hardware) + - name: deterministic tuning smoke 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 + 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) runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 - uses: dtolnay/rust-toolchain@stable - uses: Swatinem/rust-cache@v2 - - name: set up vcan0 - run: | + - run: | sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set up vcan0 - - name: run vcan tests - run: cargo test -p dji-esc-socketcan -- --ignored --test-threads 1 + - run: cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1 - cross-aarch64: - name: cross check aarch64 + cross-check: runs-on: ubuntu-latest + strategy: + matrix: + target: [aarch64-unknown-linux-gnu, armv7-unknown-linux-musleabihf] steps: - uses: actions/checkout@v4 - uses: dtolnay/rust-toolchain@stable with: - targets: aarch64-unknown-linux-gnu + targets: ${{ matrix.target }} - uses: Swatinem/rust-cache@v2 - - run: cargo check --workspace --target aarch64-unknown-linux-gnu + - run: cargo check --workspace --target ${{ matrix.target }} build-armv7: - name: build armv7 static (board target) runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 @@ -77,21 +65,4 @@ jobs: with: 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) - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v4 - - uses: dtolnay/rust-toolchain@stable - - uses: Swatinem/rust-cache@v2 - - uses: taiki-e/install-action@cargo-llvm-cov - - run: cargo llvm-cov --workspace --lcov --output-path lcov.info - - uses: actions/upload-artifact@v4 - with: - name: coverage-lcov - path: lcov.info + - run: cargo build --release -p chassisd -p spin -p chassis-tune --target armv7-unknown-linux-musleabihf diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md index 118a8f8..da09b38 100644 --- a/ARCHITECTURE.md +++ b/ARCHITECTURE.md @@ -1,1172 +1,157 @@ -# RoboMaster CAN 控制系统架构文档 +# RoboMaster 底盘职责型架构 -**版本**: 1.0 -**目标平台**: Allwinner T153 (Cortex-A7 @ 1.6GHz, Linux) -**控制频率**: 500Hz -**文档目的**: 指导执行 Agent 完成项目拆分、结构整理与模块设计 +**版本**: 2.0 +**平台**: Allwinner T153 / Linux +**底盘控制频率**: 1 kHz +**实现范围**: 四轮麦轮底盘；云台与发射机构尚未实现 ---- +本文是架构的唯一规范。`docs/chassis.md` 只记录接线、标定、部署与排障。 -## 目录 +## 1. 架构原则 -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-附录) +- **一个 crate 一件事**：协议、传输、设备运行时、算法、仿真、工具和应用入口分别成 crate。 +- **依赖必须无环**：允许依赖同层或更低抽象，但禁止反向控制依赖和隐藏的完整链路打包。 +- **std，禁止 unsafe**：workspace 使用 `unsafe_code = "forbid"`；允许使用提供安全 API 的第三方依赖。 +- **实时调度交给 systemd**：进程不调用 libc FFI，不实现 `SCHED_FIFO` 或 `mlockall`。 +- **crate 内保持扁平**：只允许 Cargo 惯例目录 `src`、`tests`；`src` 内不建立模块子目录。 +- **安全边界独立于控制律**：总线负责反馈新鲜度、命令租约和零电流；控制器通过策略接口组合。 ---- +## 2. Workspace -## 1. 系统概述 +```text +基础 + can CAN 帧、CanBus、CanLink、MockBus + imu IMU 数据契约 -### 1.1 目标 +协议 + motor-protocol C620/C610、GM6020 编解码、多圈与 ID + delta10-protocol DETA10 帧布局与解码 -构建一个运行在 T153 Linux 平台上的 RoboMaster 机器人控制系统，实现： +驱动 + socketcan Linux SocketCAN ↔ CanLink + slcan Lawicel 串口 CAN ↔ CanLink + delta10 DETA10 IMU 驱动 -- 对 **9 个 CAN 电机**（5 个原生 CAN + 4 个 SLCAN）的 500Hz 闭环控制 -- 对 **delta10 IMU** 的姿态数据采集（通过 CAN 连接） -- 对机架不同部分的**完整功能抽象**（云台、发射机构、底盘） -- 支持**仿真环境下的自动 PID 整定**，并将参数迁移到实机 -- 保证每一层的**可维护性、可验证性、可移植性** +设备与运行时 + motor Motor 能力 trait、状态、命令、减速比、注册表、EffortPolicy + esc-bus ESC 帧泵、反馈状态、租约、离线保护、effort 输出 -### 1.2 关键约束 +控制 + pid 通用 PID + motor-control 速度/位置/串级/前馈与 MotorPolicy + mecanum 麦轮正逆运动学 -| 约束 | 值 | 影响 | -|------|-----|------| -| 控制频率 | 500Hz (2ms) | 任务预算分配、调度策略 | -| 平台 | T153 Cortex-A7 Linux | 非硬实时，需 SCHED_FIFO | -| CAN 接口 | 2 原生 + 1 SLCAN | 三种不同的访问路径 | -| IMU 接口 | CAN | 与电机共享 can0 总线 | -| 仿真目标 | 纯算法验证 | 不需要全系统数字孪生 | +实验与仿真 + motor-model DC/GM3508 物理模型 + experiment Scenario、Trace、Window、StepMetrics + sim-harness 同步闭环、总线损伤、域随机化、假电调 + tuning Relay/ZN、成本、增益空间、搜索、预设 ---- - -## 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 │ │ │ │ │ -└──────────────────┘ └──────────┘ └───────────────────────┘ +应用 + chassis-config chassis.toml schema 与校验 + bench 真机链路门面、安全包络、试验采样 + chassisd 底盘守护进程二进制 + spin chassis / motor 台架二进制 + chassis-tune 自动整定二进制与报告/绘图适配 ``` -### 2.2 设备清单 +共 21 个 crate。crate 名不使用 `rm-` 等无领域含义的统一前缀。 -| 设备 | 数量 | 总线 | 协议 | 控制帧 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 | +## 3. 允许依赖 -**注意**：GM6020 的 ID 分配需在初始化时固定，避免与 M3508 冲突。建议 GM6020 使用 0x205-0x206（对应 ID 1-2），M3508 摩擦轮使用 0x201-0x202（对应 ID 1-2），预留 ID 3-4。 +主要边如下： -### 2.3 总线负载估算 +```text +motor-protocol -> can +delta10-protocol -> can, imu +socketcan -> can +slcan -> can +delta10 -> can, imu, delta10-protocol -| 总线 | 设备数 | 帧/秒（500Hz） | 1Mbps 负载 | -|------|--------|----------------|-----------| -| can0 | 4 电机 + delta10 | ~5000 | ~12% | -| can1 | 1 电机 | ~1000 | ~2% | -| SLCAN | 4 电机 | ~4000 | USB CDC 带宽充裕 | +motor -> can, motor-protocol +esc-bus -> can, motor, motor-protocol ---- +pid -> (无内部依赖) +motor-control -> pid, motor +mecanum -> (无内部依赖) -## 3. 架构原则 +motor-model -> motor-protocol +sim-harness -> can, motor, motor-control, motor-model, motor-protocol, experiment +tuning -> pid, motor-control, motor-model, sim-harness, experiment -### 3.1 分层原则 +chassis-config -> esc-bus, motor-control, motor-protocol, mecanum +bench -> can, esc-bus, experiment, motor, motor-control, socketcan, slcan -系统分为 **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） │ -└─────────────────────────────────────────┘ +chassisd -> chassis-config, esc-bus, mecanum, motor-control, sim-harness, socketcan +spin -> bench, esc-bus, motor-control, socketcan, slcan +chassis-tune -> bench, chassis-config, experiment, sim-harness, tuning ``` -**依赖方向**：L7 → L6 → L5 → L4 → L3 → L2 → L1。不允许反向依赖，不允许跳层依赖（如 L7 直接使用 L2）。 +禁止边： -### 3.2 Rust 特性使用原则 +- `esc-bus -> motor-control` / `pid`：`esc-bus` 不包含控制律。 +- `bench -> tuning`：真机试验只产生 `TrialOutcome`；成本和搜索适配在 `chassis-tune`。 +- `sim-harness -> tuning`：仿真只产生模型与轨迹；搜索器不反向渗入仿真。 +- 协议或驱动依赖设备、控制、仿真、整定或应用 crate。 -| 特性 | 使用场景 | 示例 | -|------|---------|------| -| `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` | +`motor::EffortPolicy` 是控制与总线的唯一组合点。`motor-control::MotorPolicy` +实现该 trait，应用负责把它传给 `esc_bus::EscBus::spawn`。 -### 3.3 单一职责原则 +## 4. 运行模型 -- **一个 crate 一个职责**：`rm-protocol` 只做编解码，不含 IO -- **一个模块一个概念**：`rm-devices/motor/gm6020.rs` 只处理 GM6020 -- **一个函数一个动作**：`send_current` 只负责发送，不负责校验以外的逻辑 +- 底盘速度环和 ESC 命令节拍为 **1 kHz**。 +- `esc-bus` 每拍读取命令租约和反馈新鲜度，再调用 `EffortPolicy`。 +- 反馈超时或命令租约到期时强制零输出；策略状态会被复位。 +- 上层必须逐拍重发目标值；一次性命令会在默认 50 ms 后失效。 +- `chassisd` 将麦轮逆解得到的轮端速度经 `OutputShaft` 换算到转子速度。 -### 3.4 可移植性原则 +## 5. std 与实时策略 -- **硬件无关性**：`rm-control` 和 `rm-protocol` 不含任何平台相关代码 -- **trait 边界**：所有硬件访问通过 `rm-hal` 的 trait -- **仿真对等**：`SimPlant` 和 `RealPlant` 实现同一 trait，可无缝切换 +- Rust 代码只使用 std 与安全第三方 API，workspace 设置 `unsafe_code = "forbid"`。 +- 不依赖 `libc`，不包含 FFI、`no_std` 分支或 `realtime` 模块。 +- `deploy/chassisd.service` 通过 `CPUSchedulingPolicy=fifo`、 + `CPUSchedulingPriority=80`、`LimitRTPRIO=95` 设置调度策略。 +- `LimitMEMLOCK=infinity` 只提高锁定额度；std 程序不调用 `mlockall`，本文不承诺主动锁页。 +- 不支持的平台由 `socketcan` 返回明确错误；核心算法与仿真可跨平台测试。 ---- +## 6. 目录规则 -## 4. Crate 拆分 +每个 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 # 整定指南 +```text +Cargo.toml +src/lib.rs # 库 +src/main.rs # 二进制 +tests/*.rs # 可选集成测试 ``` -### 4.2 Workspace Cargo.toml +不允许 `src//` 子目录。单元测试使用源码文件内的 `#[cfg(test)] mod tests`。 -```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", -] +## 7. 当前实现边界 -[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"] } +- C620/C610 电流协议、GM6020 电压协议骨架、多圈跟踪与 ID 校验。 +- SocketCAN、SLCAN、DETA10 驱动骨架、MockBus。 +- 四轮 ESC 总线、命令租约、反馈离线保护、外部控制策略。 +- PID、速度/位置/串级/前馈、麦轮运动学。 +- 电机模型、总线损伤、同步/异步闭环、参数搜索、预设与报告。 +- `chassisd --mock`、`spin chassis|motor`、`chassis-tune`。 -# 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"] } +- 云台与发射机构任务和控制律。 +- DETA10 真实载荷布局（决策点 D1，原手册链接失效）。 +- GM6020 实车闭环。 +- E907 异构核（决策点 D4）。 +- 进程内 `mlockall`。 -# 实时 -libc = "0.2" +## 8. 验收 -# 序列化 -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）时暂停并请求人工确认。 +- `cargo fmt --all -- --check` +- `cargo clippy --workspace --all-targets -- -D warnings` +- `cargo test --workspace --all-targets` +- Linux vcan：`cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1` +- `find crates/*/src -mindepth 2 -type d` 无输出。 +- Rust 源码不得包含 `unsafe`、`libc` 或 `no_std`。 +- `cargo tree` 确认禁止边不存在、依赖图无环。 diff --git a/Cargo.lock b/Cargo.lock index 56cbbea..6577a44 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -113,6 +113,23 @@ version = "1.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" +[[package]] +name = "bench" +version = "0.1.0" +dependencies = [ + "anyhow", + "can", + "esc-bus", + "experiment", + "motor", + "motor-control", + "slcan 0.1.0", + "socketcan 0.1.0", + "thiserror", + "tokio", + "tracing", +] + [[package]] name = "bit-set" version = "0.8.0" @@ -153,7 +170,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] -name = "can-link" +name = "can" version = "0.1.0" dependencies = [ "tokio", @@ -181,30 +198,15 @@ 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", + "approx", + "esc-bus", "mecanum", - "motor-core", + "motor-control", + "motor-protocol", "serde", "thiserror", "toml", @@ -218,25 +220,32 @@ dependencies = [ "approx", "argmin", "argmin-math", - "can-link", - "chassis-bench", + "bench", + "can", "chassis-config", "clap", "csv", - "dji-esc", - "dji-esc-sim", + "esc-bus", + "experiment", "indicatif", - "motor-core", + "motor", + "motor-control", + "motor-model", + "motor-protocol", "plotters", "rand", "rayon", "serde", "serde_json", + "sim-harness", + "slcan 0.1.0", + "socketcan 0.1.0", "thiserror", "tokio", "toml", "tracing", "tracing-subscriber", + "tuning", ] [[package]] @@ -244,15 +253,15 @@ name = "chassisd" version = "0.1.0" dependencies = [ "approx", + "can", "chassis-config", "clap", - "dji-esc", - "dji-esc-sim", - "dji-esc-socketcan", + "esc-bus", "mecanum", - "motor-core", - "proptest", - "thiserror", + "motor", + "motor-control", + "sim-harness", + "socketcan 0.1.0", "tokio", "tracing", "tracing-subscriber", @@ -449,6 +458,27 @@ dependencies = [ "syn 2.0.119", ] +[[package]] +name = "delta10" +version = "0.1.0" +dependencies = [ + "can", + "delta10-protocol", + "imu", + "thiserror", + "tokio", +] + +[[package]] +name = "delta10-protocol" +version = "0.1.0" +dependencies = [ + "approx", + "can", + "imu", + "thiserror", +] + [[package]] name = "derive_builder" version = "0.20.2" @@ -501,53 +531,6 @@ dependencies = [ "windows-sys", ] -[[package]] -name = "dji-esc" -version = "0.1.0" -dependencies = [ - "can-link", - "motor-core", - "thiserror", - "tokio", - "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" -dependencies = [ - "can-link", - "dji-esc", - "motor-core", - "socketcan", - "tokio", - "tracing", -] - [[package]] name = "dlib" version = "0.5.3" @@ -606,6 +589,30 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "esc-bus" +version = "0.1.0" +dependencies = [ + "approx", + "can", + "experiment", + "motor", + "motor-control", + "motor-model", + "motor-protocol", + "sim-harness", + "thiserror", + "tokio", + "tracing", +] + +[[package]] +name = "experiment" +version = "0.1.0" +dependencies = [ + "approx", +] + [[package]] name = "fastrand" version = "2.5.0" @@ -839,6 +846,10 @@ version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" +[[package]] +name = "imu" +version = "0.1.0" + [[package]] name = "indexmap" version = "2.14.2" @@ -990,8 +1001,41 @@ dependencies = [ ] [[package]] -name = "motor-core" +name = "motor" version = "0.1.0" +dependencies = [ + "approx", + "can", + "motor-protocol", + "thiserror", +] + +[[package]] +name = "motor-control" +version = "0.1.0" +dependencies = [ + "approx", + "motor", + "pid", +] + +[[package]] +name = "motor-model" +version = "0.1.0" +dependencies = [ + "approx", + "motor-protocol", +] + +[[package]] +name = "motor-protocol" +version = "0.1.0" +dependencies = [ + "approx", + "can", + "proptest", + "thiserror", +] [[package]] name = "nb" @@ -1143,6 +1187,13 @@ dependencies = [ "rustc_version", ] +[[package]] +name = "pid" +version = "0.1.0" +dependencies = [ + "approx", +] + [[package]] name = "pin-project-lite" version = "0.2.17" @@ -1546,12 +1597,41 @@ dependencies = [ "libc", ] +[[package]] +name = "sim-harness" +version = "0.1.0" +dependencies = [ + "approx", + "can", + "experiment", + "motor", + "motor-control", + "motor-model", + "motor-protocol", + "rand", + "thiserror", + "tokio", + "tracing", +] + [[package]] name = "slab" version = "0.4.12" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" +[[package]] +name = "slcan" +version = "0.1.0" +dependencies = [ + "can", + "serial", + "slcan 0.1.3", + "thiserror", + "tokio", + "tracing", +] + [[package]] name = "slcan" version = "0.1.3" @@ -1577,6 +1657,21 @@ dependencies = [ "windows-sys", ] +[[package]] +name = "socketcan" +version = "0.1.0" +dependencies = [ + "can", + "esc-bus", + "motor", + "motor-control", + "motor-protocol", + "socketcan 4.0.0", + "thiserror", + "tokio", + "tracing", +] + [[package]] name = "socketcan" version = "4.0.0" @@ -1598,6 +1693,24 @@ dependencies = [ "tokio", ] +[[package]] +name = "spin" +version = "0.1.0" +dependencies = [ + "bench", + "clap", + "esc-bus", + "experiment", + "mecanum", + "motor", + "motor-control", + "slcan 0.1.0", + "socketcan 0.1.0", + "tokio", + "tracing", + "tracing-subscriber", +] + [[package]] name = "strsim" version = "0.11.1" @@ -1809,6 +1922,29 @@ version = "0.20.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4" +[[package]] +name = "tuning" +version = "0.1.0" +dependencies = [ + "approx", + "argmin", + "argmin-math", + "experiment", + "indicatif", + "motor-control", + "motor-model", + "motor-protocol", + "pid", + "proptest", + "rand", + "rayon", + "serde", + "sim-harness", + "thiserror", + "toml", + "tracing", +] + [[package]] name = "unarray" version = "0.1.4" diff --git a/Cargo.toml b/Cargo.toml index f16f72e..0e17117 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,17 +1,27 @@ [workspace] resolver = "3" members = [ - "crates/motor-core", - "crates/can-link", - "crates/dji-esc", - "crates/dji-esc-sim", - "crates/dji-esc-socketcan", - "crates/dji-esc-slcan", + "crates/can", + "crates/imu", + "crates/motor-protocol", + "crates/delta10-protocol", + "crates/socketcan", + "crates/slcan", + "crates/delta10", + "crates/motor", + "crates/esc-bus", + "crates/pid", + "crates/motor-control", "crates/mecanum", + "crates/motor-model", + "crates/experiment", + "crates/sim-harness", + "crates/tuning", "crates/chassis-config", - "crates/chassis-bench", - "crates/chassis-tune", + "crates/bench", "crates/chassisd", + "crates/spin", + "crates/chassis-tune", ] [workspace.package] @@ -27,20 +37,23 @@ strip = true [workspace.dependencies] # 异步运行时 tokio = { version = "1", features = ["rt-multi-thread", "macros", "sync", "time", "signal"] } -# CAN 总线（仅 Linux，由 dji-esc-socketcan 按 target 引入） -socketcan = { version = "4", features = ["tokio"] } +# CAN 总线（仅 Linux，由 socketcan 按 target 引入） +socketcan-rs = { package = "socketcan", version = "4", features = ["tokio"] } +# SLCAN 串口适配器（CANable 等，用户态 Lawicel 协议） +slcan-proto = { package = "slcan", version = "0.1.3" } +serial = "0.4" # 错误定义与结构化日志 thiserror = "2" tracing = "0.1" tracing-subscriber = { version = "0.3", features = ["env-filter"] } -# 配置与命令行（chassisd） +# 配置与命令行 serde = { version = "1", features = ["derive"] } toml = "0.9" clap = { version = "4", features = ["derive"] } # 测试 proptest = "1" approx = "0.5" -# 自动整定工具（chassis-tune） +# 自动整定工具 # # `rand` 对齐 argmin 依赖的 0.9，避免依赖树里出现两个 rand 大版本； # `argmin-math` 必须显式开启 `vec`（argmin 自身只启 `primitives`）， @@ -56,16 +69,28 @@ 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" } + +# ---- 职责型内部 crate ---- +can = { path = "crates/can" } +imu = { path = "crates/imu" } +motor-protocol = { path = "crates/motor-protocol" } +delta10-protocol = { path = "crates/delta10-protocol" } +socketcan = { path = "crates/socketcan" } +slcan = { path = "crates/slcan" } +delta10 = { path = "crates/delta10" } +motor = { path = "crates/motor" } +esc-bus = { path = "crates/esc-bus" } +pid = { path = "crates/pid" } +motor-control = { path = "crates/motor-control" } mecanum = { path = "crates/mecanum" } +motor-model = { path = "crates/motor-model" } +experiment = { path = "crates/experiment" } +sim-harness = { path = "crates/sim-harness" } +tuning = { path = "crates/tuning" } chassis-config = { path = "crates/chassis-config" } -chassis-bench = { path = "crates/chassis-bench" } +bench = { path = "crates/bench" } +chassisd = { path = "crates/chassisd" } +spin = { path = "crates/spin" } chassis-tune = { path = "crates/chassis-tune" } [workspace.lints.rust] diff --git a/README.md b/README.md index de543e1..01ba60c 100644 --- a/README.md +++ b/README.md @@ -1,171 +1,61 @@ # sentry-chassis -机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），CAN 接入（SocketCAN 或串口 slcan），tokio 异步架构。 +RoboMaster 哨兵机器人麦轮底盘：Linux ARM 主控、4 × GM3508/C620、SocketCAN 或 SLCAN、tokio 异步运行时。 -## 架构一览 +架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 21 个职责单一 crate 组成，不使用 `rm-` 前缀，crate 内源码保持扁平。 -| crate | 职责 | 平台 | -| --- | --- | --- | -| [`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`、`dji-esc-sim` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道 -（`CanLink`），因此协议、运动学、控制与整定逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层 -（`dji-esc-socketcan` / `dji-esc-slcan`，以及组合它们的 `chassis-bench` / `chassisd`）。 +- `can` / `imu`：基础接口。 +- `motor-protocol` / `delta10-protocol`：纯协议编解码。 +- `socketcan` / `slcan` / `delta10`：硬件驱动。 +- `motor` / `esc-bus`：设备契约与安全总线运行时。 +- `pid` / `motor-control` / `mecanum`：纯控制算法。 +- `motor-model` / `experiment` / `sim-harness` / `tuning`：仿真与自动整定。 +- `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`：配置、台架和应用入口。 -> 运输后端 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 循环天然满足）。 +`esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为 +`motor::EffortPolicy` 传入总线，从而保持依赖无环。 ## 开发 -需要 Rust stable（仓库通过 `rust-toolchain.toml` 锁定）。 - ```sh -cargo test --workspace # 单测 + 闭环仿真测试（无需硬件） -cargo run -p chassisd -- --mock # 无硬件全链路演示 -just tune-smoke # 自动整定冒烟（确定性，无需硬件，CI 同款） -just lint # fmt + clippy（与 CI 同标准） +cargo test --workspace --all-targets +cargo run -p chassisd -- --mock +cargo run -p spin -- chassis --device /dev/ttyACM0 --dry-run +cargo run -p chassis-tune -- presets --preset config/tune.toml +just lint +just tune-smoke ``` -### Linux 虚拟 CAN（vcan0）集成测试 +底盘控制频率为 1 kHz。命令是 50 ms 租约，上层必须逐拍重发目标值。 + +## 台架 ```sh -just vcan-test # 或见 justfile 内的 modprobe / ip link 命令 +# 四轮 500 rpm 转子，slcan +cargo run --release -p spin -- chassis --device /dev/ttyACM0 --rpm 500 + +# 单电机 SocketCAN 验证 +cargo run --release -p spin -- motor can0 1 1000 10 ``` -## 台架：四轮 500 rpm 旋转测试（slcan） +轮子必须架空或整车固定，急停可及。工具默认限流、限时并支持 ctrl-c 安全停机。 -实车台架验证工具 `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 + --preset config/tune.toml --preset-name sim_bench_model \ + --samples 512 --out-dir target/tune ``` -随仓库提供的预设（`config/tune.toml`）： +仿真搜索不接触硬件。真机后端会转动轮子，必须显式传入 +`--i-know-the-wheel-spins`。详细接线、标定和排障见 +[`docs/chassis.md`](docs/chassis.md)。 -| 预设 | 后端 | 用途 | -| --- | --- | --- | -| `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 试运行（默认 root@192.168.2.2） -# 或手动： -cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf -scp target/armv7-unknown-linux-musleabihf/release/chassisd root@192.168.2.2:/tmp/ -``` - -板端 CAN 有两条互斥路径： - -- **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 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off（可用 -> `ip -d link show can0` 查看），chassisd 侧表现为四电机反馈离线、强制零电流—— -> 这正是安全设计的预期行为。 - -## 文档 - -- [**底盘开发文档**](docs/chassis.md)——硬件接线、坐标与单位约定、控制与安全设计、配置参考、 - **标定与上车流程**、自动整定与超参数预设、台架工具、故障排查（开发 / 调试 / 上车先看这份） -- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、被控对象模型、参数空间搜索、测试策略 -- [整体架构规划](ARCHITECTURE.md)——全系统（含云台 / 发射 / IMU / 实时性）的 crate 划分与阶段计划 +仓库不使用 unsafe 或 libc FFI。生产调度策略由 +[`deploy/chassisd.service`](deploy/chassisd.service) 的 systemd 指令设置。 +std 程序不调用 `mlockall`。 diff --git a/crates/bench/Cargo.toml b/crates/bench/Cargo.toml new file mode 100644 index 0000000..98cce3d --- /dev/null +++ b/crates/bench/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "bench" +description = "Hardware bench links, safety envelope, and trial runner" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +motor = { workspace = true } +esc-bus = { workspace = true } +experiment = { workspace = true } +motor-control = { workspace = true } +socketcan = { workspace = true } +slcan = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } +anyhow = { workspace = true } + +[dev-dependencies] +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/bench/src/lib.rs b/crates/bench/src/lib.rs new file mode 100644 index 0000000..fcafe8a --- /dev/null +++ b/crates/bench/src/lib.rs @@ -0,0 +1,947 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod link { + //! 链路打开：把两种运输后端收敛成一个门面。 + //! + //! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`CanLink`]"， + //! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处，上层就不必 + //! 同时依赖两个运输 crate。 + + use std::io::Result; + use std::time::Duration; + + use can::CanLink; + + /// slcan 波特率档位（对应协议 `Sn` 命令）。 + pub use slcan::CanSpeed; + /// slcan 打开参数。 + pub use slcan::SlcanOptions; + + /// 打开 slcan 串口设备（CANable 等），桥接为链路端点。 + /// + /// # Errors + /// + /// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。 + pub fn open_slcan(device: &str, options: SlcanOptions) -> Result { + slcan::open_link_with(device, options) + } + + /// 打开 SocketCAN 接口（Linux）。 + /// + /// # Errors + /// + /// 接口不存在 / 未 up / 权限不足时的 `io::Error`；非 Linux 平台恒为错误。 + #[cfg(target_os = "linux")] + pub fn open_socketcan(interface: &str) -> Result { + 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)); + } + } +} + +pub mod safety { + //! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。 + //! + //! 台架工具（`chassis-spin`）与自动整定的真机后端（`chassis-tune --backend bench`） + //! 共用同一套判定，避免安全逻辑在两个地方各写一遍、然后慢慢漂移。 + //! + //! 所有阈值都是 `Option`：只开需要的那个。`chassis-spin` 只开过速保护 + //! （保持历史行为），自动整定则开启全部——因为搜索会主动试错，必须由 + //! 包络兜住"试错到失控"。 + + use std::time::Duration; + + use motor::MotorFeedback; + // 用 tokio 的时钟：`start_paused` 下虚拟时间可控，安全包络（宽限期 / 超时） + // 才能被确定性地测试；生产构建里它就是 `std::time::Instant`。 + use thiserror::Error; + use tokio::time::Instant; + + /// 触发停机的原因。 + #[derive(Debug, Clone, PartialEq, Error)] + pub enum Abort { + /// 转速超过上限并持续超过宽限期。 + #[error("overspeed: {peak_rpm:.0} rpm > {limit_rpm:.0} rpm for over {grace_ms}ms")] + Overspeed { + /// 观测到的峰值转速（rpm）。 + peak_rpm: f32, + /// 配置的上限（rpm）。 + limit_rpm: f32, + /// 宽限期（毫秒）。 + grace_ms: u64, + }, + /// 温度超过上限。 + #[error("overtemperature: {temperature_c}℃ > {limit_c}℃")] + OverTemperature { + /// 观测温度（℃）。 + temperature_c: i8, + /// 上限（℃）。 + limit_c: i8, + }, + /// 反馈超出新鲜度阈值（掉线）。 + #[error("esc {esc_id} feedback lost: age {age_ms:?}ms > {timeout_ms}ms")] + FeedbackLost { + /// 电调 ID。 + esc_id: u8, + /// 实测反馈年龄（毫秒）。 + age_ms: Option, + /// 阈值（毫秒）。 + 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::{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:?}"); + } + } +} + +pub mod sampler { + //! 真机试验：把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。 + //! + //! 与 `sim-harness` 的离线 harness 是**同一套场景词汇**：整定工具先用离线 + //! harness 搜索，再用本模块在台架上复核，两条路径的指标口径因此一致。 + //! + //! # 安全契约 + //! + //! - 每个采样拍都重发设定值——**命令是租约**，不重发会在 50ms 后失电； + //! - 每个采样拍都过一遍 [`SafetyMonitor`]； + //! - 无论正常结束、触发包络还是被取消，退出前一律 `disable()`（0 电流）。 + + use std::future::Future; + use std::time::Duration; + + use experiment::{Scenario, Trace}; + use motor::{Disable, MotorFeedback, VelocityDrive}; + use tokio::time::{Instant, MissedTickBehavior, interval}; + + use crate::safety::{Abort, SafetyEnvelope, SafetyMonitor}; + + /// 一次真机试验的结果。 + #[derive(Debug, Clone)] + pub struct TrialOutcome { + /// 采集到的轨迹（触发中止时只含中止前的采样）。 + pub trace: Trace, + /// 触发安全包络 / 被取消的原因；`None` 表示按场景跑完。 + pub abort: Option, + } + + 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::{Command, MockMotor, MotorState}; + + fn ready_motor(id: u8) -> MockMotor { + let motor = MockMotor::new(id); + motor.inject_state(MotorState { feedback_count: 1, ..MotorState::default() }); + motor + } + + #[tokio::test(start_paused = true)] + async fn trial_follows_scenario_and_disables_at_the_end() { + let motor = ready_motor(1); + let scenario = Scenario::ramp_and_hold(1000.0, 0.1, 0.2); + let outcome = run_step_trial( + &motor, + &scenario, + &SafetyEnvelope::new(), + 12_000, + Duration::from_millis(5), + std::future::pending::<()>(), + ) + .await; + + assert!(outcome.completed(), "{:?}", outcome.abort); + assert!(outcome.trace.is_consistent()); + assert!(outcome.trace.len() > 250, "len = {}", outcome.trace.len()); + // 斜坡中点的设定值应约为一半。 + let mid = outcome.trace.window(0.049, 0.052).setpoint_rpm()[0]; + assert!((mid - 500.0).abs() < 30.0, "mid setpoint = {mid}"); + // 收尾必须归零。 + assert_eq!(motor.last_command(), Command::Disabled); + } + + #[tokio::test(start_paused = true)] + async fn overspeed_aborts_and_zeroes_output() { + let motor = ready_motor(1); + // 反馈恒为 5000 rpm，远超 3000 rpm 上限。 + motor.inject_state(MotorState { + rotor_speed_rpm: 5000.0, + feedback_count: 1, + ..MotorState::default() + }); + let envelope = SafetyEnvelope::overspeed_only(3000.0, Duration::from_millis(20)); + let scenario = Scenario::step(1000.0, 2.0); + let outcome = run_step_trial( + &motor, + &scenario, + &envelope, + 12_000, + Duration::from_millis(5), + std::future::pending::<()>(), + ) + .await; + + assert!(matches!(outcome.abort, Some(Abort::Overspeed { .. })), "{:?}", outcome.abort); + assert_eq!(motor.last_command(), Command::Disabled); + // 中止得足够早，不该跑满场景。 + assert!(outcome.trace.len() < 200, "len = {}", outcome.trace.len()); + } + + #[tokio::test(start_paused = true)] + async fn stalled_feedback_aborts() { + let motor = MockMotor::new(4); + motor.inject_state_with_age( + MotorState { feedback_count: 1, ..MotorState::default() }, + Duration::from_millis(500), + ); + let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100)); + let scenario = Scenario::step(500.0, 1.0); + let outcome = run_step_trial( + &motor, + &scenario, + &envelope, + 12_000, + Duration::from_millis(5), + std::future::pending::<()>(), + ) + .await; + + assert!( + matches!(outcome.abort, Some(Abort::FeedbackLost { esc_id: 4, .. })), + "{:?}", + outcome.abort + ); + assert_eq!(motor.last_command(), Command::Disabled); + } + + #[tokio::test(start_paused = true)] + async fn cancellation_stops_and_zeroes_output() { + let motor = ready_motor(2); + let scenario = Scenario::step(800.0, 5.0); + let outcome = run_step_trial( + &motor, + &scenario, + &SafetyEnvelope::new(), + 12_000, + Duration::from_millis(5), + async { tokio::time::sleep(Duration::from_millis(50)).await }, + ) + .await; + + assert_eq!(outcome.abort, Some(Abort::Cancelled)); + assert_eq!(motor.last_command(), Command::Disabled); + assert!(!outcome.trace.is_empty()); + assert!(outcome.trace.len() < 200, "取消后不得继续采样"); + } + } +} + +pub mod hil { + //! 真机（HIL）评估后端：在台架上逐次试验一组增益。 + //! + //! 与 [`SimEvaluator`](crate::SimEvaluator) 实现同一个 [`Evaluator`]， + //! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动， + //! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。 + //! + //! # 安全设计 + //! + //! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）； + //! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c， + //! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。 + //! + //! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池 + //! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。 + + use std::sync::Mutex; + use std::time::Duration; + + use crate::{SafetyEnvelope, run_step_trial}; + use anyhow::Context as _; + use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor}; + use experiment::Scenario; + use motor::{Disable as _, MotorFeedback}; + use motor_control::{MotorPolicy, VelocityGains}; + use thiserror::Error; + + use crate::sampler::TrialOutcome; + + /// 真机后端错误。 + #[derive(Debug, Error)] + pub enum BenchError { + /// 无法打开总线。 + #[error("cannot open CAN link: {0}")] + Link(String), + /// 电调总线启动失败。 + #[error("cannot start ESC bus: {0}")] + Bus(String), + /// 启动超时：目标电调没有反馈。 + #[error( + "esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)" + )] + NoFeedback { + /// 目标电调 ID。 + esc_id: u8, + /// 等待时长（毫秒）。 + waited_ms: u64, + }, + /// 搜索期间因安全事件中止且未要求继续。 + #[error("aborted by safety envelope: {reason}")] + Aborted { + /// 触发原因。 + reason: String, + }, + } + + /// 真机评估配置。 + #[derive(Debug, Clone)] + pub struct BenchConfig { + /// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。 + pub esc: EscConfig, + /// 总线运行参数。 + pub bus: BusOptions, + /// 评估场景。 + pub scenario: Scenario, + /// 安全包络。 + pub envelope: SafetyEnvelope, + /// 试验之间的静默期（等轮子停稳）。 + pub between_trials: Duration, + /// 命令租约续期节拍。 + pub lease_period: Duration, + /// 启动时等待反馈的超时。 + pub startup_timeout: Duration, + } + + impl BenchConfig { + /// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。 + #[must_use] + pub fn new(esc: EscConfig, scenario: Scenario, envelope: SafetyEnvelope) -> Self { + Self { + esc, + bus: BusOptions::default(), + scenario, + envelope, + between_trials: Duration::from_millis(500), + lease_period: Duration::from_millis(5), + startup_timeout: Duration::from_secs(5), + } + } + } + + /// 真机评估器（持有总线与运行时）。 + pub struct BenchEvaluator { + session: Mutex, + scenario: Scenario, + envelope: SafetyEnvelope, + effort_limit_raw: i16, + between_trials: Duration, + lease_period: Duration, + } + + struct Session { + runtime: tokio::runtime::Runtime, + bus: Option>, + motor: EscMotor, + } + + impl BenchEvaluator { + /// 打开链路、启动总线并等待目标电调反馈。 + /// + /// # Errors + /// + /// 链路 / 总线启动失败，或启动超时（电调无反馈）。 + pub fn open(link: can::CanLink, config: BenchConfig) -> Result { + let esc_id = config.esc.esc_id; + let effort_limit_raw = config.esc.current_limit; + let runtime = tokio::runtime::Builder::new_multi_thread() + .worker_threads(2) + .enable_all() + .build() + .map_err(|e| BenchError::Bus(e.to_string()))?; + + let mut policy = MotorPolicy::new(); + policy.configure(config.esc.esc_id, config.esc.current_limit, VelocityGains::ZERO); + let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone(), policy) + .map_err(|e| BenchError::Bus(e.to_string()))?; + let motor = bus + .motor(esc_id) + .ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))? + .clone(); + + let startup_timeout = config.startup_timeout; + let ready = runtime.block_on(async { + let deadline = tokio::time::Instant::now() + startup_timeout; + loop { + if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) { + return true; + } + if tokio::time::Instant::now() >= deadline { + return false; + } + tokio::time::sleep(Duration::from_millis(20)).await; + } + }); + let evaluator = Self { + session: Mutex::new(Session { runtime, bus: Some(bus), motor }), + scenario: config.scenario, + envelope: config.envelope, + effort_limit_raw, + between_trials: config.between_trials, + lease_period: config.lease_period, + }; + if !ready { + // 启动失败也必须走完整停机路径：先 0 电流，再关总线。 + evaluator.shutdown(); + return Err(BenchError::NoFeedback { + esc_id, + waited_ms: startup_timeout.as_millis() as u64, + }); + } + Ok(evaluator) + } + + /// Scenario executed by each trial. + #[must_use] + pub fn scenario(&self) -> Scenario { + self.scenario + } + + /// Stop outputs and close the bus. + pub fn shutdown(&self) { + let mut session = + self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + let Some(bus) = session.bus.take() else { + return; + }; + for motor in bus.motors() { + motor.disable(); + } + session.runtime.block_on(async { + tokio::time::sleep(Duration::from_millis(50)).await; + bus.shutdown().await; + }); + } + + /// Run one physical trial and return its raw trace and abort reason. + pub fn run_trial(&self, gains: &VelocityGains) -> TrialOutcome { + let mut session = + self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner); + let Session { runtime, motor, bus, .. } = &mut *session; + let esc_id = motor.id(); + if let Some(bus) = bus.as_ref() { + bus.with_policy(|policy| policy.set_velocity_gains(esc_id, *gains)); + } + let scenario = self.scenario; + let envelope = self.envelope.clone(); + let limit = self.effort_limit_raw; + let lease = self.lease_period; + let between = self.between_trials; + + runtime.block_on(async { + let trial = run_step_trial( + motor, + &scenario, + &envelope, + limit, + lease, + std::future::pending(), + ) + .await; + tokio::time::sleep(between).await; + trial + }) + } + } + + /// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。 + /// + /// # Errors + /// + /// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。 + pub fn open_link( + device: &str, + serial_baud: u32, + can_speed: crate::CanSpeed, + interface: Option<&str>, + ) -> Result { + let result = match interface { + Some(interface) => crate::open_socketcan(interface), + None => crate::open_slcan(device, crate::default_slcan_options(serial_baud, can_speed)), + }; + result + .with_context(|| match interface { + Some(interface) => { + format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）") + } + None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"), + }) + .map_err(|e| BenchError::Link(e.to_string())) + } + + #[cfg(test)] + mod tests { + use super::*; + + fn config() -> BenchConfig { + let mut esc = EscConfig::gm3508("bench", 1, 1.0); + esc.current_limit = 1500; + let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15); + let envelope = SafetyEnvelope::new() + .with_overspeed(1200.0, Duration::from_millis(60)) + .with_temperature_max(70) + .with_feedback_timeout(Duration::from_millis(200)) + .with_trial_timeout(Duration::from_secs(2)); + BenchConfig::new(esc, scenario, envelope) + } + + #[test] + fn config_defaults_are_safe() { + let config = config(); + assert_eq!(config.bus.period, Duration::from_millis(1)); + assert_eq!(config.bus.command_timeout, Duration::from_millis(50)); + assert!(config.between_trials >= Duration::from_millis(200)); + assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约"); + } + + #[test] + fn link_open_failures_are_errors() { + // 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。 + let result = + open_link("/definitely/not/a/device", 2_000_000, crate::CanSpeed::M1, None); + assert!(result.is_err(), "不存在的串口设备必须报错"); + let missing_interface = + open_link("ignored", 2_000_000, crate::CanSpeed::M1, Some("can-not-real")); + assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错"); + } + } +} + +pub use hil::*; +pub use link::*; +pub use safety::*; +pub use sampler::*; diff --git a/crates/can-link/src/lib.rs b/crates/can-link/src/lib.rs deleted file mode 100644 index d354078..0000000 --- a/crates/can-link/src/lib.rs +++ /dev/null @@ -1,75 +0,0 @@ -//! CAN 传输抽象：帧与链路通道。 -//! -//! 本 crate **不含任何协议知识**（无 DJI ID 常量）：只定义经典 CAN 数据帧 -//! 与一对 tokio 通道构成的链路端点。协议族（`dji-esc` / 未来 `dji-gm6020`） -//! 与总线后端（SocketCAN 桥、仿真器）共享此类型，互不依赖。 -#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] - -use tokio::sync::mpsc; - -/// 经典 CAN 数据帧（数据长度固定 8 字节）。 -#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] -pub struct CanFrame { - /// 11 位标准 ID。 - pub id: u32, - /// 数据域。 - pub data: [u8; 8], -} - -impl CanFrame { - /// 构造一帧。 - #[must_use] - pub const fn new(id: u32, data: [u8; 8]) -> Self { - Self { id, data } - } -} - -/// CAN 链路端点。 -/// -/// - `outgoing_tx`：本端发往总线的帧； -/// - `incoming_rx`：本端从总线收到的帧。 -/// -/// 一条链路由两个端点构成（见 [`CanLink::pair`]）。持有本端、把对端交给 -/// 对侧（SocketCAN 桥、仿真器）即可通信。 -#[derive(Debug)] -pub struct CanLink { - /// 发往总线。 - pub outgoing_tx: mpsc::Sender, - /// 来自总线。 - pub incoming_rx: mpsc::Receiver, -} - -impl CanLink { - /// 由已有通道组装端点。 - #[must_use] - pub const fn new( - outgoing_tx: mpsc::Sender, - incoming_rx: mpsc::Receiver, - ) -> Self { - Self { outgoing_tx, incoming_rx } - } - - /// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。 - /// - /// `bound` 为每方向缓冲帧数（建议 ≥ 64，1kHz 收发下留足余量）。 - #[must_use] - pub fn pair(bound: usize) -> (Self, Self) { - let (tx_a_to_b, rx_at_b) = mpsc::channel(bound); - let (tx_b_to_a, rx_at_a) = mpsc::channel(bound); - (Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b)) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[tokio::test] - async fn pair_loops_both_directions() { - let (mut a, mut b) = CanLink::pair(8); - a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); - b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); - assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200); - assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201); - } -} diff --git a/crates/can/Cargo.toml b/crates/can/Cargo.toml new file mode 100644 index 0000000..39338fd --- /dev/null +++ b/crates/can/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "can" +description = "CAN frame, bus interface, channel link, and test bus" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[features] +mock = [] + +[dependencies] +tokio = { workspace = true } + +[dev-dependencies] +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/can/src/lib.rs b/crates/can/src/lib.rs new file mode 100644 index 0000000..84c140e --- /dev/null +++ b/crates/can/src/lib.rs @@ -0,0 +1,463 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod frame { + //! CAN 抽象：经典数据帧与**总线 trait**。 + //! + //! [`CanFrame`] 是全系统唯一的帧表示（协议层、驱动层、仿真层、设备层共享）， + //! 它**不含任何协议知识**（没有 DJI 帧 ID 常量）。字段与 ARCHITECTURE.md + //! §5.1 一致：`id` / `data` / `dlc` / `is_extended`。 + //! + //! [`CanBus`] 是 ARCHITECTURE.md §5.1 定义的硬件访问边界；`driver crates` 为 + //! 真实总线实现它，`can::mock` 为假电调实现它， + //! [`CanLink`](crate::link::CanLink) 是它的通道式实现。 + + /// 经典 CAN 数据帧数据域长度（字节）。 + pub const CAN_DATA_LEN: usize = 8; + + /// 经典 CAN 数据帧。 + /// + /// 当前所有设备（C620/C610、GM6020、DETA10）都用「11 位标准 ID + 8 字节」， + /// 但字段保留完整形态：将来接扩展帧或短帧时不必改动所有构造点。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] + pub struct CanFrame { + /// 标准 11 位 / 扩展 29 位 ID。 + pub id: u32, + /// 数据域（未使用的字节为 0）。 + pub data: [u8; CAN_DATA_LEN], + /// 数据长度码（0..=8）。 + pub dlc: u8, + /// 是否扩展帧（29 位 ID）。 + pub is_extended: bool, + } + + impl CanFrame { + /// 构造一帧标准帧：8 字节数据、`dlc = 8`。 + /// + /// 这是仓库内**唯一**的常规构造方式；`dlc` / 扩展帧只在特殊场景用 + /// [`CanFrame::with_dlc`] / [`CanFrame::extended`] 调整。 + #[must_use] + pub const fn new(id: u32, data: [u8; CAN_DATA_LEN]) -> Self { + Self { id, data, dlc: CAN_DATA_LEN as u8, is_extended: false } + } + + /// 换一个数据长度码（链式）。 + #[must_use] + pub const fn with_dlc(mut self, dlc: u8) -> Self { + self.dlc = if dlc > CAN_DATA_LEN as u8 { CAN_DATA_LEN as u8 } else { dlc }; + self + } + + /// 标记为扩展帧（链式）。 + #[must_use] + pub const fn extended(mut self) -> Self { + self.is_extended = true; + self + } + + /// 长度码是否覆盖完整数据域。 + #[must_use] + pub const fn is_complete(&self) -> bool { + self.dlc == CAN_DATA_LEN as u8 + } + + /// ID 是否在 11 位标准帧范围内。 + #[must_use] + pub const fn is_standard_id(&self) -> bool { + self.id <= 0x7FF + } + } + + /// CAN 总线访问错误。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum CanError { + /// 链路已关闭（对端释放）。 + Closed, + /// ID 与帧类型不匹配（如标准帧却给了 29 位 ID）。 + BadId(u32), + /// 数据长度码非法（> 8）。 + BadDlc(u8), + /// 底层收发失败（驱动自行补充上下文）。 + Io, + } + + impl core::fmt::Display for CanError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::Closed => write!(f, "CAN 链路已关闭"), + Self::BadId(id) => write!(f, "CAN ID {id:#x} 与帧类型不匹配"), + Self::BadDlc(dlc) => write!(f, "CAN 数据长度码 {dlc} 非法（应 ≤ 8）"), + Self::Io => write!(f, "CAN 收发失败"), + } + } + } + + impl core::error::Error for CanError {} + + /// 硬件无关的 CAN 总线边界（ARCHITECTURE.md §5.1）。 + /// + /// 只描述"发一帧 / 收一帧 / 非阻塞收一帧"，**不规定**底层是 SocketCAN、 + /// 串口 slcan、内存通道还是假电调。`async fn in trait` 是本仓库对 + /// ARCHITECTURE.md §3.2 的直接采纳（Rust 1.75+ 稳定）。 + /// + /// # 关于非阻塞 + /// + /// 控制循环严格要求"读反馈不阻塞"（§6.3 第 2–5 步必须非阻塞）。 + /// 通道式实现天然支持 [`CanBus::try_recv`]；对不支持的系统调用， + /// 默认实现返回 `Ok(None)` 而不是报错，让上层按"本轮没新帧"处理。 + pub trait CanBus { + /// 底层错误类型。 + type Error: core::error::Error + Send + Sync + 'static; + + /// 发送一帧。 + /// + /// # Errors + /// + /// 链路关闭、帧类型非法或底层写失败时返回错误。 + fn send( + &mut self, + frame: &CanFrame, + ) -> impl core::future::Future>; + + /// 接收一帧（阻塞直到有帧或链路关闭）。 + /// + /// # Errors + /// + /// 链路关闭或底层读失败时返回错误。 + fn recv(&mut self) -> impl core::future::Future>; + + /// 非阻塞接收：本轮没有新帧时返回 `Ok(None)`。 + /// + /// # Errors + /// + /// 仅当链路**确定**已关闭 / 损坏时返回错误；"暂时没数据"不是错误。 + fn try_recv(&mut self) -> Result, Self::Error> { + Ok(None) + } + } + + /// 帧是否符合"标准 ID + 完整 8 字节"的常规约定。 + #[must_use] + pub const fn is_regular_standard_frame(frame: &CanFrame) -> bool { + frame.is_standard_id() && !frame.is_extended && frame.is_complete() + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn new_defaults_to_a_complete_standard_frame() { + let frame = CanFrame::new(0x200, [1, 2, 3, 4, 5, 6, 7, 8]); + assert_eq!(frame.id, 0x200); + assert_eq!(frame.data, [1, 2, 3, 4, 5, 6, 7, 8]); + assert_eq!(frame.dlc, 8); + assert!(!frame.is_extended); + assert!(frame.is_complete()); + assert!(is_regular_standard_frame(&frame)); + } + + #[test] + fn builders_are_chainable_and_clamp_dlc() { + let short = CanFrame::new(0x201, [0; 8]).with_dlc(4); + assert_eq!(short.dlc, 4); + assert!(!short.is_complete()); + assert!(!is_regular_standard_frame(&short)); + + let over = CanFrame::new(0x201, [0; 8]).with_dlc(9); + assert_eq!(over.dlc, 8, "dlc 必须被夹到数据域长度"); + + let extended = CanFrame::new(0x1_2345, [0; 8]).extended(); + assert!(extended.is_extended); + assert!(!is_regular_standard_frame(&extended)); + } + + #[test] + fn standard_id_boundary() { + assert!(CanFrame::new(0x7FF, [0; 8]).is_standard_id()); + assert!(!CanFrame::new(0x800, [0; 8]).is_standard_id()); + } + + #[test] + fn errors_render_in_chinese() { + let text = CanError::BadId(0x1234).to_string(); + assert!(text.contains("0x1234"), "{text}"); + assert!(CanError::BadDlc(9).to_string().contains('9')); + assert!(!CanError::Closed.to_string().is_empty()); + } + } +} + +pub use frame::*; + +pub mod link { + //! 通道式总线端点：[`CanLink`] 与它对 [`CanBus`] 的实现。 + //! + //! 一对 tokio 通道构成一条链路：本端发出的帧由对端收到，反之亦然。 + //! 这是本仓库"**帧传输的唯一接缝**"——协议层与设备层只面向通道工作， + //! 因此同一套协议 / 控制 / 整定代码可以对接真实总线（`driver crates` 的 + //! SocketCAN / slcan 桥）或纯内存假电调（`can::mock`）， + //! 无需任何条件编译。 + + use tokio::sync::mpsc; + + use crate::frame::{CanBus, CanError, CanFrame}; + + /// CAN 链路的**一端**。 + /// + /// - `outgoing_tx`：本端发往总线的帧； + /// - `incoming_rx`：本端从总线收到的帧。 + #[derive(Debug)] + pub struct CanLink { + /// 发往总线。 + pub outgoing_tx: mpsc::Sender, + /// 来自总线。 + pub incoming_rx: mpsc::Receiver, + } + + impl CanLink { + /// 由已有通道组装端点。 + #[must_use] + pub const fn new( + outgoing_tx: mpsc::Sender, + incoming_rx: mpsc::Receiver, + ) -> Self { + Self { outgoing_tx, incoming_rx } + } + + /// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。 + /// + /// `bound` 为每方向缓冲帧数。命令方向必须**浅**（1kHz 生产、总线消费 + /// 不过来时靠 `send().await` 回压，见 `slcan` 的说明）； + /// 反馈方向是采样流，可以深一些。 + #[must_use] + pub fn pair(bound: usize) -> (Self, Self) { + let (tx_a_to_b, rx_at_b) = mpsc::channel(bound); + let (tx_b_to_a, rx_at_a) = mpsc::channel(bound); + (Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b)) + } + } + + impl CanBus for CanLink { + type Error = CanError; + + async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> { + if frame.dlc > 8 { + return Err(CanError::BadDlc(frame.dlc)); + } + self.outgoing_tx.send(*frame).await.map_err(|_| CanError::Closed) + } + + async fn recv(&mut self) -> Result { + self.incoming_rx.recv().await.ok_or(CanError::Closed) + } + + fn try_recv(&mut self) -> Result, Self::Error> { + match self.incoming_rx.try_recv() { + Ok(frame) => Ok(Some(frame)), + Err(mpsc::error::TryRecvError::Empty) => Ok(None), + Err(mpsc::error::TryRecvError::Disconnected) => Err(CanError::Closed), + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[tokio::test] + async fn pair_loops_both_directions() { + let (mut a, mut b) = CanLink::pair(8); + a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); + b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); + assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200); + assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201); + } + + #[tokio::test] + async fn can_bus_impl_sends_receives_and_polls() { + let (mut a, mut b) = CanLink::pair(8); + assert_eq!(a.try_recv().unwrap(), None, "空通道不是错误"); + a.send(&CanFrame::new(0x1FF, [7; 8])).await.unwrap(); + assert_eq!(b.try_recv().unwrap().map(|f| f.id), Some(0x1FF)); + assert_eq!(b.try_recv().unwrap(), None, "取走之后就没有了"); + } + + #[tokio::test] + async fn recv_reports_closed_link() { + let (mut a, b) = CanLink::pair(8); + drop(b); + assert_eq!(a.recv().await.unwrap_err(), CanError::Closed); + assert_eq!(a.try_recv().unwrap_err(), CanError::Closed); + } + + #[tokio::test] + async fn send_rejects_illegal_dlc() { + let (mut a, _b) = CanLink::pair(8); + let bad = CanFrame::new(0x200, [0; 8]).with_dlc(8).with_dlc(8); + assert!(a.send(&bad).await.is_ok()); + // `with_dlc` 本身会夹住越界值，因此非法 dlc 只能由手工构造的帧产生。 + let mut manual = CanFrame::new(0x200, [0; 8]); + manual.dlc = 9; + assert_eq!(a.send(&manual).await.unwrap_err(), CanError::BadDlc(9)); + } + } +} + +pub use link::*; + +pub mod mock { + //! 驱动级测试替身：[`MockBus`]（`mock` feature）。 + //! + //! **这里没有电机模型**——那是 `sim-harness` 的事（`sim-harness::rig` 的异步假电调带 + //! 摩擦 / 延迟物理模型）。本模块只做驱动层该做的事： + //! + //! - 记录所有**发出去**的帧（断言"上层到底写了什么"）； + //! - 按需**注入**要收到的帧（模拟总线上的反馈）； + //! - 可以设置成"链路已断开"，用于测试离线 / 安全路径。 + //! + //! 有了它，协议层与设备层的测试不必启动 tokio 任务，也不必构造真实总线。 + + use std::collections::VecDeque; + + use crate::frame::{CanBus, CanError, CanFrame}; + + /// 记录发送、可注入接收的假总线。 + #[derive(Debug, Default)] + pub struct MockBus { + /// 上层通过 [`CanBus::send`] 发出的帧（按时间顺序）。 + sent: Vec, + /// 等待被上层收到的帧。 + pending: VecDeque, + /// 置位后所有操作都返回 [`CanError::Closed`]。 + closed: bool, + } + + impl MockBus { + /// 空总线。 + #[must_use] + pub fn new() -> Self { + Self::default() + } + + /// 注入一帧"来自总线"的数据。 + pub fn inject(&mut self, frame: CanFrame) { + self.pending.push_back(frame); + } + + /// 模拟链路断开。 + pub fn close(&mut self) { + self.closed = true; + } + + /// 已发出的帧。 + #[must_use] + pub fn sent(&self) -> &[CanFrame] { + &self.sent + } + + /// 最近发出的一帧。 + #[must_use] + pub fn last_sent(&self) -> Option<&CanFrame> { + self.sent.last() + } + + /// 清空发送记录。 + pub fn clear_sent(&mut self) { + self.sent.clear(); + } + + /// 还剩多少帧没被取走。 + #[must_use] + pub fn pending_len(&self) -> usize { + self.pending.len() + } + } + + impl CanBus for MockBus { + type Error = CanError; + + // 这里用 `std::future::ready` 而不是 `async fn`：内存总线没有真正的等待， + // 返回一个已完成的 future 更诚实（也避免"async 函数里没有 await"的告警）。 + fn send( + &mut self, + frame: &CanFrame, + ) -> impl std::future::Future> { + let result = if self.closed { + Err(CanError::Closed) + } else if frame.dlc > 8 { + Err(CanError::BadDlc(frame.dlc)) + } else if !frame.is_standard_id() && !frame.is_extended { + Err(CanError::BadId(frame.id)) + } else { + self.sent.push(*frame); + Ok(()) + }; + std::future::ready(result) + } + + fn recv(&mut self) -> impl std::future::Future> { + let result = if self.closed { + Err(CanError::Closed) + } else { + self.pending.pop_front().ok_or(CanError::Closed) + }; + std::future::ready(result) + } + + fn try_recv(&mut self) -> Result, Self::Error> { + if self.closed { + return Err(CanError::Closed); + } + Ok(self.pending.pop_front()) + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[tokio::test] + async fn records_sent_frames_in_order() { + let mut bus = MockBus::new(); + bus.send(&CanFrame::new(0x200, [1; 8])).await.unwrap(); + bus.send(&CanFrame::new(0x200, [2; 8])).await.unwrap(); + assert_eq!(bus.sent().len(), 2); + assert_eq!(bus.last_sent().unwrap().data[0], 2); + bus.clear_sent(); + assert!(bus.sent().is_empty()); + } + + #[tokio::test] + async fn injected_frames_come_back_once() { + let mut bus = MockBus::new(); + bus.inject(CanFrame::new(0x201, [9; 8])); + assert_eq!(bus.pending_len(), 1); + let frame = bus.recv().await.unwrap(); + assert_eq!(frame.id, 0x201); + assert_eq!(bus.try_recv().unwrap(), None, "取走之后就没有了"); + } + + #[tokio::test] + async fn closed_bus_reports_errors_on_every_path() { + let mut bus = MockBus::new(); + bus.close(); + assert_eq!( + bus.send(&CanFrame::new(0x200, [0; 8])).await.unwrap_err(), + CanError::Closed + ); + assert_eq!(bus.recv().await.unwrap_err(), CanError::Closed); + assert_eq!(bus.try_recv().unwrap_err(), CanError::Closed); + } + + #[tokio::test] + async fn illegal_dlc_is_rejected_before_recording() { + let mut bus = MockBus::new(); + let mut bad = CanFrame::new(0x200, [0; 8]); + bad.dlc = 9; + assert_eq!(bus.send(&bad).await.unwrap_err(), CanError::BadDlc(9)); + assert!(bus.sent().is_empty(), "非法帧不得进入记录"); + } + } +} + +pub use mock::*; diff --git a/crates/chassis-bench/Cargo.toml b/crates/chassis-bench/Cargo.toml deleted file mode 100644 index 05a2efd..0000000 --- a/crates/chassis-bench/Cargo.toml +++ /dev/null @@ -1,32 +0,0 @@ -[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 deleted file mode 100644 index cfbe48d..0000000 --- a/crates/chassis-bench/src/bin/chassis-spin.rs +++ /dev/null @@ -1,554 +0,0 @@ -//! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 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/chassis-bench/src/bin/socketcan-spin.rs b/crates/chassis-bench/src/bin/socketcan-spin.rs deleted file mode 100644 index 965e1fc..0000000 --- a/crates/chassis-bench/src/bin/socketcan-spin.rs +++ /dev/null @@ -1,121 +0,0 @@ -//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。 -//! -//! ```sh -//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs] -//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒 -//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10 -//! ``` -//! -//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道 -//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码 -//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。 -//! -//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。 - -#![allow(clippy::unwrap_used, clippy::expect_used)] - -#[cfg(target_os = "linux")] -use std::time::Duration; - -#[cfg(target_os = "linux")] -use dji_esc::{BusOptions, EscBus, EscConfig}; -#[cfg(target_os = "linux")] -use dji_esc_socketcan::open_link; -#[cfg(target_os = "linux")] -use motor_core::Motor; -#[cfg(target_os = "linux")] -use tokio::time::{MissedTickBehavior, interval}; - -#[cfg(target_os = "linux")] -#[tokio::main] -async fn main() { - let mut args = std::env::args().skip(1); - let interface = args.next().unwrap_or_else(|| "can0".into()); - let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1); - let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0); - let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0); - - println!("=== 单电机抽象验证 ==="); - println!( - "接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s" - ); - - let link = open_link(&interface).unwrap_or_else(|e| { - eprintln!( - "打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）" - ); - std::process::exit(1); - }); - let config = EscConfig::gm3508("spin_test", esc_id, 1.0); - println!( - "电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}", - config.model, config.gear_ratio, config.current_limit - ); - - let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法"); - let motor = bus.motor(esc_id).expect("刚配置的电机必然存在"); - - spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await; - - bus.shutdown().await; - println!("已退出"); -} - -/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。 -/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。 -/// -/// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发 -/// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值 -/// （真实底盘的控制循环每 1ms 重发一次）。 -#[cfg(target_os = "linux")] -async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) { - const LEASE_PERIOD: Duration = Duration::from_millis(10); - const REPORT_PERIOD: Duration = Duration::from_millis(100); - - motor.set_velocity(target_rpm); - println!("已下发目标转速，开始观测（100ms 周期）……\n"); - - let mut lease = interval(LEASE_PERIOD); - lease.set_missed_tick_behavior(MissedTickBehavior::Skip); - let mut report = interval(REPORT_PERIOD); - report.set_missed_tick_behavior(MissedTickBehavior::Skip); - let deadline = tokio::time::Instant::now() + duration; - - loop { - tokio::select! { - _ = tokio::signal::ctrl_c() => { - println!("\n收到 ctrl-c"); - break; - } - _ = lease.tick() => motor.set_velocity(target_rpm), // 续租 - _ = report.tick() => { - let st = motor.state(); - if st.has_feedback() { - println!( - "转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧", - st.rotor_speed_rpm, - st.effort_raw, - st.temperature_c, - st.rotor_revolutions, - st.feedback_count - ); - } else { - println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线"); - } - } - } - if tokio::time::Instant::now() >= deadline { - break; - } - } - - motor.disable(); - // 让 0 电流帧至少发出一拍再关总线。 - tokio::time::sleep(Duration::from_millis(50)).await; - println!("已停机（0 电流）"); -} - -#[cfg(not(target_os = "linux"))] -fn main() { - eprintln!("spin 仅支持 Linux（需要 SocketCAN）"); -} diff --git a/crates/chassis-bench/src/lib.rs b/crates/chassis-bench/src/lib.rs deleted file mode 100644 index 45fdff4..0000000 --- a/crates/chassis-bench/src/lib.rs +++ /dev/null @@ -1,27 +0,0 @@ -//! 台架工具库：激励场景、安全包络与真机试验采样。 -//! -//! 本 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 deleted file mode 100644 index a511f9b..0000000 --- a/crates/chassis-bench/src/link.rs +++ /dev/null @@ -1,87 +0,0 @@ -//! 链路打开：把两种运输后端收敛成一个门面。 -//! -//! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`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 deleted file mode 100644 index 798e761..0000000 --- a/crates/chassis-bench/src/safety.rs +++ /dev/null @@ -1,322 +0,0 @@ -//! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。 -//! -//! 台架工具（`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 deleted file mode 100644 index a3f510d..0000000 --- a/crates/chassis-bench/src/sampler.rs +++ /dev/null @@ -1,239 +0,0 @@ -//! 真机试验：把一条 [`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 index db4607e..43c1162 100644 --- a/crates/chassis-config/Cargo.toml +++ b/crates/chassis-config/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "chassis-config" -description = "chassis.toml schema: parsing, semantic validation, and mapping to driver configuration" +description = "chassis.toml schema and validation" version.workspace = true edition.workspace = true rust-version.workspace = true @@ -10,9 +10,13 @@ license.workspace = true workspace = true [dependencies] -dji-esc = { workspace = true } +motor-control = { workspace = true } mecanum = { workspace = true } -motor-core = { workspace = true } +esc-bus = { workspace = true } +motor-protocol = { workspace = true } serde = { workspace = true } toml = { workspace = true } thiserror = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/chassis-config/src/lib.rs b/crates/chassis-config/src/lib.rs index e761620..0f49b63 100644 --- a/crates/chassis-config/src/lib.rs +++ b/crates/chassis-config/src/lib.rs @@ -10,9 +10,10 @@ use std::time::Duration; -use dji_esc::{BusOptions, EscConfig, EscModel}; +use esc_bus::{BusOptions, EscConfig}; use mecanum::Mecanum; -use motor_core::VelocityGains; +use motor_control::VelocityGains; +use motor_protocol::EscModel; use serde::{Deserialize, Serialize}; use thiserror::Error; @@ -411,10 +412,6 @@ impl Config { gear_ratio: m.gear_ratio, direction: m.direction, current_limit: m.current_limit, - kp: m.kp, - ki: m.ki, - kd: m.kd, - feedforward: m.feedforward, }) .collect() } diff --git a/crates/chassis-tune/Cargo.toml b/crates/chassis-tune/Cargo.toml index f55d33e..3132835 100644 --- a/crates/chassis-tune/Cargo.toml +++ b/crates/chassis-tune/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "chassis-tune" -description = "Automated PID gain search for the chassis velocity loop: offline sweep/refine plus bench (hardware-in-the-loop) verification" +description = "Automated chassis velocity-loop tuning tool" version.workspace = true edition.workspace = true rust-version.workspace = true @@ -10,32 +10,37 @@ license.workspace = true workspace = true [features] -default = [] -# 可选：用 plotters 输出 best.svg（默认关闭，避免 CI 依赖系统字体与更长编译）。 -plot = ["dep:plotters"] +plot = ['dep:plotters'] [dependencies] -can-link = { workspace = true } -chassis-bench = { workspace = true } +bench = { workspace = true } +can = { workspace = true } chassis-config = { workspace = true } +experiment = { workspace = true } +motor = { workspace = true } +esc-bus = { workspace = true } +motor-protocol = { workspace = true } +motor-control = { workspace = true } +motor-model = { workspace = true } +sim-harness = { workspace = true } +tuning = { workspace = true } +socketcan = { workspace = true } +slcan = { workspace = true } tokio = { workspace = true } -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 } +clap = { workspace = true } +serde = { workspace = true } +serde_json = { workspace = true } +toml = { workspace = true } +csv = { workspace = true } +indicatif = { workspace = true } +rayon = { workspace = true } +rand = { workspace = true } +argmin = { workspace = true } +argmin-math = { workspace = true } +anyhow = { workspace = true } plotters = { workspace = true, optional = true } [dev-dependencies] diff --git a/crates/chassis-tune/src/backend.rs b/crates/chassis-tune/src/backend.rs deleted file mode 100644 index aee0673..0000000 --- a/crates/chassis-tune/src/backend.rs +++ /dev/null @@ -1,173 +0,0 @@ -//! 评估后端：把"一组增益有多好"这件事抽象出来。 -//! -//! - [`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 deleted file mode 100644 index 326b895..0000000 --- a/crates/chassis-tune/src/bench.rs +++ /dev/null @@ -1,383 +0,0 @@ -//! 真机（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 deleted file mode 100644 index f7f56bb..0000000 --- a/crates/chassis-tune/src/cost.rs +++ /dev/null @@ -1,302 +0,0 @@ -//! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。 -//! -//! 两个原则： -//! -//! 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 deleted file mode 100644 index 7ef3099..0000000 --- a/crates/chassis-tune/src/gains.rs +++ /dev/null @@ -1,572 +0,0 @@ -//! 增益参数空间：轴定义、采样与**坐标空间**映射。 -//! -//! # 为什么要有"坐标空间" -//! -//! 增益的合法取值跨越数量级（`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 deleted file mode 100644 index 31511cb..0000000 --- a/crates/chassis-tune/src/lib.rs +++ /dev/null @@ -1,60 +0,0 @@ -//! 底盘速度环 **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 index 2fd6718..55170b9 100644 --- a/crates/chassis-tune/src/main.rs +++ b/crates/chassis-tune/src/main.rs @@ -1,55 +1,945 @@ -//! `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))] +#![allow(clippy::unwrap_used, clippy::expect_used)] + +use std::sync::Mutex; + +pub mod report { + //! 报告与产物写出：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 experiment::{Scenario, Trace}; + use serde::Serialize; + use sim_harness::LoopLimits; + + use experiment::StepMetrics; + use tuning::gains::GainSpace; + use tuning::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::tune::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, + tuning::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: tuning::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, "| 场景 | `{}` |", tuning::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::SimEvaluator; + use motor_model::PlantParams; + use sim_harness::{LoopLimits, PlantEnsemble}; + use tuning::Evaluator; + use tuning::cost::{Objective, Weights}; + use tuning::gains::GainSpace; + use tuning::search::{SearchConfig, sweep}; + + 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 = + sim_harness::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)); + } + } +} + +#[cfg(feature = "plot")] +pub mod plot { + //! 可选（`--features plot`）：把最优步响应画成 SVG。 + //! + //! 为什么是 SVG 而不是 PNG：矢量图在任何缩放级别都能看清起动段的细节， + //! 且不需要位图编码器（依赖更小）。默认关闭该 feature——绘图不是整定的 + //! 必要条件，CSV 已经给了全部数据。 + + use std::path::Path; + + use experiment::{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 experiment::SetpointProfile; + use motor_control::VelocityGains; + use motor_model::PlantParams; + use sim_harness::{LoopLimits, simulate}; + + #[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); + } + } +} + +// `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。 use std::path::{Path, PathBuf}; use std::process::ExitCode; use std::time::Duration; +use self::report::{ReportContext, write_reports}; use anyhow::{Context as _, Result, bail}; -use chassis_bench::{CanSpeed, SafetyEnvelope}; +use bench::CanSpeed; +use bench::hil::{BenchConfig, BenchEvaluator as BenchRunner, open_link}; +use bench::safety::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 esc_bus::EscConfig; +use experiment::SetpointProfile; use indicatif::ProgressBar; +use motor_control::VelocityGains; +use motor_protocol::EscModel; use serde::Serialize; +use sim_harness::{LoopLimits, PlantEnsemble}; +use tuning::{ + AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH, + Evaluator, Objective, PlantSpec, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, ScaleSpec, + ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict, parse_axis, + sweep, +}; /// 子命令。 #[derive(Debug, Subcommand)] @@ -66,7 +956,7 @@ enum Command { /// /// 超参数（参数空间、场景、权重、阈值、搜索预算、被控对象、安全包络）都可以缺省： /// 缺省时按 **内置默认 → 预设文件 → 显式 CLI** 解算，见模块文档与 -/// [`chassis_tune::preset`]。这里的 `Option` 正是"用户是否显式给了"的载体。 +/// `tuning::preset`。这里的 `Option` 正是"用户是否显式给了"的载体。 #[derive(Debug, Clone, ClapArgs)] struct CommonArgs { /// 评估后端：`sim`（离线）或 `bench`（真机台架） @@ -656,7 +1546,7 @@ fn range(text: Option<&str>, name: &'static str) -> Result> { text.map(|text| { // 先用线性刻度只解析区间；对数轴"下界必须为正"的检查留给最终解算， // 这样只给区间、刻度继承预设时也能得到准确的报错字段。 - let axis = parse_axis(name, text, chassis_tune::Scale::Linear)?; + let axis = parse_axis(name, text, tuning::Scale::Linear)?; Ok(RangeSpec::new(axis.min, axis.max)) }) .transpose() @@ -701,9 +1591,163 @@ fn apply_jitter(preset: &mut Preset, cli: &CommonArgs) -> Result<()> { } /// 后端选择：把两种评估器收进一个枚举，报告与后处理因此不必 `Box`。 +/// Test-only offline evaluator for search regression tests. +struct SimEvaluator { + ensemble: sim_harness::PlantEnsemble, + scenario: experiment::Scenario, + limits: sim_harness::LoopLimits, + objective: tuning::cost::Objective, + robust_weight: f64, +} + +impl SimEvaluator { + /// Build an offline evaluator. + #[must_use] + pub fn new( + ensemble: sim_harness::PlantEnsemble, + scenario: experiment::Scenario, + limits: sim_harness::LoopLimits, + objective: tuning::cost::Objective, + robust_weight: f64, + ) -> Self { + Self { ensemble, scenario, limits, objective, robust_weight } + } +} + +impl Evaluator for SimEvaluator { + fn evaluate(&self, gains: &VelocityGains) -> Score { + let mut costs = Vec::with_capacity(self.ensemble.len()); + let mut nominal = None; + let mut worst_verdict = tuning::cost::Verdict::Ok; + for (index, plant) in self.ensemble.iter().enumerate() { + let score = match sim_harness::simulate(*gains, plant, &self.scenario, self.limits) { + Ok(trace) => self.objective.score(&trace), + Err(error) => { + tuning::cost::Score::penalized(tuning::cost::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().max(1) as f64; + let mean = costs.iter().sum::() / count; + let worst = costs.iter().copied().fold(f64::NEG_INFINITY, f64::max); + let cost_value = (1.0 - self.robust_weight) * mean + self.robust_weight * worst; + let nominal = nominal + .unwrap_or_else(|| tuning::cost::Score::penalized(tuning::cost::Verdict::Rejected)); + let verdict = if nominal.verdict.is_ok() { worst_verdict } else { nominal.verdict }; + tuning::cost::Score { cost: cost_value, verdict, metrics: nominal.metrics } + } + + fn describe(&self) -> String { + format!("sim(ensemble={}, robust_weight={:.2})", self.ensemble.len(), self.robust_weight) + } +} + +/// Tuning adapter around the hardware-only bench runner. +struct TuningBench { + runner: BenchRunner, + objective: Objective, + continue_on_abort: bool, + best: Mutex>, + aborts: Mutex>, + describe: String, +} + +impl TuningBench { + fn open( + link: can::CanLink, + config: BenchConfig, + objective: Objective, + continue_on_abort: bool, + ) -> Result { + let describe = format!( + "bench(esc={}, name={}, limit={} raw, target={} rpm, trial={:.1}s, gap={}ms)", + config.esc.esc_id, + config.esc.name, + config.esc.current_limit, + config.scenario.target_rpm, + config.scenario.total_s(), + config.between_trials.as_millis() + ); + let runner = BenchRunner::open(link, config).map_err(|e| anyhow::anyhow!("{e}"))?; + Ok(Self { + runner, + objective, + continue_on_abort, + best: Mutex::new(None), + aborts: Mutex::new(Vec::new()), + describe, + }) + } + + fn best_trace(&self) -> Option { + self.best + .lock() + .unwrap_or_else(std::sync::PoisonError::into_inner) + .as_ref() + .map(|(_, trace)| trace.clone()) + } + + fn aborts(&self) -> Vec { + self.aborts.lock().unwrap_or_else(std::sync::PoisonError::into_inner).clone() + } +} + +impl Evaluator for TuningBench { + fn evaluate(&self, gains: &VelocityGains) -> Score { + let outcome = self.runner.run_trial(gains); + let score = if let Some(reason) = outcome.abort { + self.aborts + .lock() + .unwrap_or_else(std::sync::PoisonError::into_inner) + .push(format!("{}: {reason}", self.describe)); + Score::penalized(verdict_for(&reason)) + } else { + match experiment::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)); + } + if !score.verdict.is_ok() && !self.continue_on_abort { + tracing::warn!(verdict = score.verdict.label(), "bench trial did not pass"); + } + score + } + + fn describe(&self) -> String { + self.describe.clone() + } +} + +impl TuningBench { + fn scenario(&self) -> experiment::Scenario { + self.runner.scenario() + } +} + +/// Map a safety abort to a tuning verdict. +fn verdict_for(abort: &bench::Abort) -> Verdict { + match abort { + bench::Abort::Overspeed { .. } | bench::Abort::OverTemperature { .. } => Verdict::Overspeed, + bench::Abort::FeedbackLost { .. } + | bench::Abort::Timeout { .. } + | bench::Abort::Cancelled => Verdict::Rejected, + } +} + enum Backend { Sim(Box), - Bench(Box), + Bench(Box), } impl Evaluator for Backend { @@ -723,7 +1767,7 @@ impl Evaluator for Backend { } impl Backend { - fn bench(&self) -> Option<&BenchEvaluator> { + fn bench(&self) -> Option<&TuningBench> { match self { Self::Bench(evaluator) => Some(evaluator), Self::Sim(_) => None, @@ -933,7 +1977,7 @@ fn sweep_cmd(settings: &Settings) -> Result { println!("=== chassis-tune sweep ==="); println!("超参数来源：{}", settings.source); println!("参数空间：{}", settings.tuning.space.describe()); - println!("场景：{}", chassis_tune::search::scenario_summary(&settings.tuning.scenario)); + println!("场景：{}", tuning::search::scenario_summary(&settings.tuning.scenario)); println!( "轮子：{}｜电调 {}｜方向 {:+.0}｜减速比 {:.3}｜限流 {} raw", hardware.name, @@ -973,7 +2017,7 @@ fn sweep_cmd(settings: &Settings) -> Result { // 最优轨迹：仿真后端重放名义模型；真机后端用搜索期间记录的最好一次。 let trace = match &evaluator { - Backend::Sim(_) => dji_esc_sim::simulate( + Backend::Sim(_) => sim_harness::simulate( ranked.best.gains, &settings.tuning.plant, &settings.tuning.scenario, @@ -1049,7 +2093,7 @@ fn compare_with_baseline(evaluator: &Backend, hardware: &Hardware, ranked: &Rank } /// 打印真机搜索期间触发的安全事件（最多 5 条）。 -fn print_aborts(evaluator: &BenchEvaluator) { +fn print_aborts(evaluator: &TuningBench) { let aborts = evaluator.aborts(); if aborts.is_empty() { return; @@ -1105,7 +2149,7 @@ struct HilRow { /// 真机重复性复验：同一组增益跑 `runs` 次，写出 `hil-validation.csv`。 fn hil_validate( - evaluator: &BenchEvaluator, + evaluator: &TuningBench, gains: VelocityGains, runs: usize, out_dir: &Path, @@ -1262,7 +2306,7 @@ fn validate_cmd(settings: &Settings, gains_text: &str) -> Result { if passes { "✓ 通过" } else { "✗ 未通过" } ); } - None => println!("没有可用指标（{}）", chassis_tune::search::verdict_hint(score.verdict)), + None => println!("没有可用指标（{}）", tuning::search::verdict_hint(score.verdict)), } Ok(exit_code(passes)) } @@ -1293,7 +2337,7 @@ fn open_bench( hardware: &Hardware, limits: LoopLimits, objective: Objective, -) -> Result { +) -> Result { let bench = &settings.bench; let envelope_spec = settings.envelope; if !bench.i_know_the_wheel_spins { @@ -1324,12 +2368,12 @@ fn open_bench( .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); + let mut config = BenchConfig::new(esc, settings.tuning.scenario, envelope); 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}")) + TuningBench::open(link, config, objective, envelope_spec.continue_on_abort) + .map_err(|e| anyhow::anyhow!("{e}")) } #[cfg(test)] @@ -1640,7 +2684,7 @@ samples = 99 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_eq!(settings.tuning.space.kp.scale, tuning::Scale::Linear); assert!((settings.tuning.space.kp.min - 0.5).abs() < 1e-12); assert!((settings.tuning.space.kp.max - 30.0).abs() < 1e-12); @@ -1648,7 +2692,7 @@ samples = 99 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); + assert_eq!(settings.tuning.space.kp.scale, tuning::Scale::Log); } #[test] @@ -1708,7 +2752,7 @@ samples = 99 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); + assert_eq!(space.ki.scale, tuning::Scale::Linear); } #[test] diff --git a/crates/chassis-tune/src/metrics.rs b/crates/chassis-tune/src/metrics.rs deleted file mode 100644 index f77b326..0000000 --- a/crates/chassis-tune/src/metrics.rs +++ /dev/null @@ -1,265 +0,0 @@ -//! 步响应指标：把一条 [`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 deleted file mode 100644 index e78a2bf..0000000 --- a/crates/chassis-tune/src/plot.rs +++ /dev/null @@ -1,129 +0,0 @@ -//! 可选（`--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 deleted file mode 100644 index 73f6104..0000000 --- a/crates/chassis-tune/src/preset.rs +++ /dev/null @@ -1,1305 +0,0 @@ -//! 整定**超参数**预设：把"搜什么、怎么打分、什么算达标"从 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 deleted file mode 100644 index f99f12b..0000000 --- a/crates/chassis-tune/src/report.rs +++ /dev/null @@ -1,713 +0,0 @@ -//! 报告与产物写出：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 deleted file mode 100644 index 14feeaf..0000000 --- a/crates/chassis-tune/src/search.rs +++ /dev/null @@ -1,628 +0,0 @@ -//! 两阶段参数空间搜索：种子化分层粗扫 → `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 3389aae..7806fb4 100644 --- a/crates/chassisd/Cargo.toml +++ b/crates/chassisd/Cargo.toml @@ -1,28 +1,28 @@ [package] name = "chassisd" +description = "Mecanum chassis daemon" version.workspace = true edition.workspace = true rust-version.workspace = true license.workspace = true -description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety supervision, and mock mode" [lints] 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 } +can = { workspace = true } +motor = { workspace = true } +esc-bus = { workspace = true } +motor-control = { workspace = true } mecanum = { workspace = true } +chassis-config = { workspace = true } +sim-harness = { workspace = true } +socketcan = { workspace = true } tokio = { workspace = true } -thiserror = { workspace = true } +clap = { workspace = true } tracing = { workspace = true } tracing-subscriber = { workspace = true } -clap = { workspace = true } [dev-dependencies] -proptest = { workspace = true } -approx = { workspace = true } tokio = { workspace = true, features = ["test-util"] } +approx = { workspace = true } diff --git a/crates/chassisd/src/controller.rs b/crates/chassisd/src/controller.rs deleted file mode 100644 index 709caa7..0000000 --- a/crates/chassisd/src/controller.rs +++ /dev/null @@ -1,197 +0,0 @@ -//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。 - -use std::time::Duration; - -use mecanum::{Mecanum, Twist}; -use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive}; -use tokio::sync::watch; -use tokio::task::JoinHandle; -use tokio::time::{Instant, MissedTickBehavior, interval}; -use tracing::{info, warn}; - -/// 底盘控制器。 -/// -/// 职责（1kHz 节拍）： -/// 1. 读取最新 twist 指令，**指令超时则归零**； -/// 2. 速度限幅（保持方向按比例收缩）； -/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用； -/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。 -/// -/// 电机反馈离线安全在驱动层完成（强制 0 电流）。 -pub struct ChassisController { - kinematics: Mecanum, - /// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。 - wheels: [W; 4], - /// (max_vx, max_vy, max_omega)。 - limits: (f32, f32, f32), - command_timeout: Duration, -} - -impl ChassisController -where - W: VelocityDrive + MotorFeedback + Disable + 'static, -{ - /// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。 - #[must_use] - pub fn new( - kinematics: Mecanum, - wheels: [W; 4], - limits: (f32, f32, f32), - command_timeout: Duration, - ) -> Self { - Self { kinematics, wheels, limits, command_timeout } - } - - /// 启动控制循环任务。 - pub fn spawn(self, commands: watch::Receiver, period: Duration) -> JoinHandle<()> { - tokio::spawn(self.run(commands, period)) - } - - async fn run(self, mut commands: watch::Receiver, period: Duration) { - let mut ticker = interval(period); - ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); - let mut latest = Twist::ZERO; - let mut last_command_at: Option = None; - let mut ticks: u64 = 0; - - loop { - ticker.tick().await; - let now = Instant::now(); - - if commands.has_changed().unwrap_or(false) { - latest = *commands.borrow_and_update(); - last_command_at = Some(now); - } - let timed_out = match last_command_at { - Some(t) => now.duration_since(t) > self.command_timeout, - // 尚未收到任何指令：保持静止。 - None => true, - }; - let twist = if timed_out { - Twist::ZERO - } else { - latest.clamped(self.limits.0, self.limits.1, self.limits.2) - }; - - let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback()); - if all_seen_feedback { - let wheel_rpms = self.kinematics.wheel_rpms(&twist); - for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) { - motor.set_velocity(wheel_rpm); - } - } else { - for motor in &self.wheels { - motor.disable(); - } - if ticks.is_multiple_of(1000) { - warn!("waiting for first feedback from all motors"); - } - } - - ticks += 1; - if ticks.is_multiple_of(1000) { - self.log_status(); - } - } - } - - fn log_status(&self) { - let (rotor_rpm, effort_raw): (Vec, Vec) = self - .wheels - .iter() - .map(|m| { - let st = m.state(); - (st.rotor_speed_rpm, i32::from(st.effort_raw)) - }) - .unzip(); - info!(?rotor_rpm, ?effort_raw, "chassis status"); - } -} - -/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。 -#[must_use] -pub fn wrap_wheels( - motors: [T; N], - gear_ratios: [f32; N], -) -> [OutputShaft; N] { - let mut out = Vec::with_capacity(N); - let mut gears = gear_ratios.into_iter(); - for m in motors { - let g = gears.next().unwrap_or(1.0); - out.push(OutputShaft::new(m, g)); - } - out.try_into().unwrap_or_else(|_| unreachable!()) -} - -#[cfg(test)] -mod tests { - use super::*; - use motor_core::{Command, MockMotor, MotorState}; - - const GEAR: f32 = 19.203; - - fn mock_wheel(id: u8) -> MockMotor { - let m = MockMotor::new(id); - m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() }); - m - } - - /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。 - #[tokio::test(start_paused = true)] - async fn controller_drives_wheels_through_capability_traits() { - let kinematics = Mecanum::new(0.2, 0.25, 0.0762); - let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)]; - let ctrl = ChassisController::new( - kinematics, - wrap_wheels(mocks.clone(), [GEAR; 4]), - (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)); - - let twist = Twist { vx: 1.0, ..Twist::ZERO }; - tx.send_replace(twist); - tokio::time::sleep(Duration::from_millis(5)).await; - - let expected = kinematics.wheel_rpms(&twist); - for (i, mock) in mocks.iter().enumerate() { - match mock.last_command() { - Command::Velocity(rotor_rpm) => { - let want = expected[i] * GEAR; - assert!( - (rotor_rpm - want).abs() < 1e-2, - "wheel {i}: rotor_rpm {rotor_rpm} != {want}" - ); - } - ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"), - } - } - - handle.abort(); - } - - /// 反馈未到齐时必须全部停用，不得下发速度。 - #[tokio::test(start_paused = true)] - async fn disables_wheels_until_all_feedback_seen() { - let kinematics = Mecanum::new(0.2, 0.25, 0.0762); - let ready = mock_wheel(1); - 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 (tx, rx) = watch::channel(Twist::ZERO); - let handle = ctrl.spawn(rx, Duration::from_millis(1)); - - tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); - tokio::time::sleep(Duration::from_millis(5)).await; - - assert_eq!(ready.last_command(), Command::Disabled); - assert_eq!(missing.last_command(), Command::Disabled); - - handle.abort(); - } -} diff --git a/crates/chassisd/src/lib.rs b/crates/chassisd/src/lib.rs deleted file mode 100644 index a2ade32..0000000 --- a/crates/chassisd/src/lib.rs +++ /dev/null @@ -1,20 +0,0 @@ -//! `chassisd` 库部分：底盘控制循环与指令源抽象。 -//! -//! 配置 schema 已独立为 [`chassis_config`]（守护进程与整定工具共用一份 -//! 定义），这里以 `config` 别名重新导出，保持 `chassisd::config::Config` -//! 这一历史路径可用。 -//! -//! 二进制入口见 `src/main.rs`。 -#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] - -pub use chassis_config as config; - -pub mod controller; -pub mod source; - -pub use chassis_config::{ - CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig, - ProtocolFamily, -}; -pub use controller::{ChassisController, wrap_wheels}; -pub use source::{ChannelSource, TwistSource, ZeroSource, pump_source}; diff --git a/crates/chassisd/src/main.rs b/crates/chassisd/src/main.rs index 3175118..b9f1273 100644 --- a/crates/chassisd/src/main.rs +++ b/crates/chassisd/src/main.rs @@ -1,5 +1,771 @@ -//! `chassisd`：麦轮底盘守护进程（4 × GM3508 / C620，SocketCAN）。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] +pub mod messages { + //! 任务间通信消息（ARCHITECTURE.md §5.7 / §6.2）。 + //! + //! 通信原语的选择与文档一致： + //! + //! | 方向 | 原语 | 理由 | + //! | --- | --- | --- | + //! | 遥控 / 上层 → 任务 | [`watch`](tokio::sync::watch) | 只关心**最新**指令，过期指令必须被丢弃 | + //! | 任务 → 诊断 | [`mpsc`](tokio::sync::mpsc) | 有背压，允许排队但不会覆盖告警 | + //! + //! [`RemoteCommand`] 是"上层想怎么动"的唯一表示：它**只描述意图**，不做限幅、 + //! 不做运动学、不碰电机。限幅在 `crate::tasks::chassis` 里按配置执行， + //! 这样"意图"与"安全策略"不会混在一个类型里。 + + use mecanum::Twist; + + /// 来自遥控器 / 自主导航的底盘指令。 + #[derive(Debug, Clone, Copy, PartialEq, Default)] + pub struct RemoteCommand { + /// 底盘平面速度（m/s、m/s、rad/s；见 [`mecanum::Twist`]）。 + pub twist: Twist, + /// 是否允许运动（安全开关 / 失控保护）。 + /// + /// `false` 时接收方必须视同 [`Twist::ZERO`]——这是"上层说停就停"的硬语义， + /// 不依赖 twist 里恰好是 0。 + pub enabled: bool, + } + + impl RemoteCommand { + /// 停车指令（允许运动但速度为 0）。 + pub const STOP: Self = Self { twist: Twist::ZERO, enabled: true }; + + /// 失能指令（不允许运动）。 + pub const DISABLED: Self = Self { twist: Twist::ZERO, enabled: false }; + + /// 构造一条使能指令。 + #[must_use] + pub const fn new(twist: Twist) -> Self { + Self { twist, enabled: true } + } + + /// 实际应当执行的速度：未使能时恒为零。 + #[must_use] + pub fn effective_twist(&self) -> Twist { + if self.enabled { self.twist } else { Twist::ZERO } + } + + /// 是否要求停车（未使能，或速度全零）。 + #[must_use] + pub fn is_stop(&self) -> bool { + !self.enabled + || (self.twist.vx == 0.0 && self.twist.vy == 0.0 && self.twist.omega == 0.0) + } + } + + impl From for RemoteCommand { + fn from(twist: Twist) -> Self { + Self::new(twist) + } + } + + /// 任务 → 诊断通道的事件（§6.2 的 `mpsc` 方向）。 + #[derive(Debug, Clone, PartialEq)] + pub enum Diagnostic { + /// 某电机的反馈离线。 + MotorOffline { + /// 电调 ID。 + esc_id: u8, + /// 轮名。 + name: String, + }, + /// 上层指令超时，目标速度已归零。 + CommandTimeout { + /// 静默时长（毫秒）。 + silent_ms: u64, + }, + /// 控制周期抖动超预算（§7.2）。 + Jitter { + /// 本次抖动（微秒）。 + jitter_us: u64, + /// 预算（微秒）。 + budget_us: u64, + }, + /// 上电门控尚未满足（还没收到全部电机的反馈）。 + AwaitingFeedback { + /// 已见到反馈的电机数。 + seen: usize, + /// 需要的电机数。 + expected: usize, + }, + } + + impl Diagnostic { + /// 是否属于"必须立即处理"的级别（用于决定日志等级 / 是否告警）。 + #[must_use] + pub const fn is_alert(&self) -> bool { + matches!(self, Self::MotorOffline { .. }) + } + + /// 一行描述。 + #[must_use] + pub fn describe(&self) -> String { + match self { + Self::MotorOffline { esc_id, name } => { + format!("电机 {name}（电调 {esc_id}）反馈离线") + } + Self::CommandTimeout { silent_ms } => { + format!("上层指令静默 {silent_ms} ms，目标速度已归零") + } + Self::Jitter { jitter_us, budget_us } => { + format!("控制周期抖动 {jitter_us} µs 超过预算 {budget_us} µs") + } + Self::AwaitingFeedback { seen, expected } => { + format!("等待电机反馈：{seen}/{expected}") + } + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn disabled_command_is_always_a_stop() { + let command = RemoteCommand::new(Twist { vx: 3.0, vy: 0.0, omega: 1.0 }); + assert!(!command.is_stop()); + assert!((command.effective_twist().vx - 3.0).abs() < 1e-6); + + let disabled = RemoteCommand { enabled: false, ..command }; + assert!(disabled.is_stop()); + assert_eq!(disabled.effective_twist(), Twist::ZERO, "失能必须归零速度"); + } + + #[test] + fn constants_and_conversions_behave() { + assert!(RemoteCommand::STOP.is_stop()); + assert!(RemoteCommand::DISABLED.is_stop()); + let from_twist: RemoteCommand = Twist { vx: 1.0, ..Twist::ZERO }.into(); + assert!((from_twist.twist.vx - 1.0).abs() < 1e-6); + assert_eq!(RemoteCommand::default().twist, Twist::ZERO); + // 失能可以用"构造出来的运行期值"表达（常量上的断言会被常量折叠）。 + let disabled = RemoteCommand { enabled: false, ..RemoteCommand::new(Twist::ZERO) }; + assert!(!disabled.enabled); + } + + #[test] + fn diagnostics_render_and_classify() { + let offline = Diagnostic::MotorOffline { esc_id: 3, name: "rear_left".into() }; + assert!(offline.is_alert(), "掉线必须升级为告警"); + let text = offline.describe(); + assert!(text.contains("rear_left") && text.contains('3'), "{text}"); + + assert!(!Diagnostic::Jitter { jitter_us: 700, budget_us: 200 }.is_alert()); + assert!(!Diagnostic::CommandTimeout { silent_ms: 200 }.is_alert()); + assert!(!Diagnostic::AwaitingFeedback { seen: 2, expected: 4 }.is_alert()); + assert!( + Diagnostic::AwaitingFeedback { seen: 2, expected: 4 }.describe().contains("2/4") + ); + } + } +} + +pub use messages::*; + +pub mod source { + //! 上层速度指令源（twist source）。 + //! + //! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中 + //! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现 + //! [`TwistSource`] 即可。 + + use std::future::Future; + + use mecanum::Twist; + use tokio::sync::{mpsc, watch}; + + /// 速度指令源。 + pub trait TwistSource: Send { + /// 取下一条指令；`None` 表示源关闭。 + fn next_twist(&mut self) -> impl Future> + Send; + } + + /// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。 + pub struct ZeroSource; + + impl TwistSource for ZeroSource { + async fn next_twist(&mut self) -> Option { + std::future::pending().await + } + } + + /// 通道指令源（测试 / 简单进程内注入用）。 + pub struct ChannelSource { + rx: mpsc::Receiver, + } + + impl ChannelSource { + /// 由 mpsc 接收端构造。 + #[must_use] + pub const fn new(rx: mpsc::Receiver) -> Self { + Self { rx } + } + + /// 创建一对（发送端, 源）。 + #[must_use] + pub fn pair(bound: usize) -> (mpsc::Sender, Self) { + let (tx, rx) = mpsc::channel(bound); + (tx, Self { rx }) + } + } + + impl TwistSource for ChannelSource { + async fn next_twist(&mut self) -> Option { + self.rx.recv().await + } + } + + /// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。 + pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender) { + while let Some(twist) = source.next_twist().await { + if tx.send(twist).is_err() { + break; + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[tokio::test(start_paused = true)] + async fn channel_source_pumps_into_watch() { + let (tx, source) = ChannelSource::pair(8); + let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO); + let pump = tokio::spawn(pump_source(source, watch_tx)); + + tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap(); + tokio::time::sleep(std::time::Duration::from_millis(1)).await; + assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 }); + + // 源关闭 → 泵任务退出。 + drop(tx); + pump.await.unwrap(); + assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值 + } + } +} + +pub use source::*; + +pub mod telemetry { + //! 诊断输出：控制周期抖动统计（ARCHITECTURE.md §7.2）。 + //! + //! §7.2 的目标是**抖动 < 200 µs**（标准 Linux 的典型值），并在超过阈值时 + //! 触发降级策略（降频 / 合并任务 / 卸载到异构核）。要判断是否需要降级， + //! 先得有可复现的抖动数据，而不是"感觉有点卡"。 + //! + //! 本模块提供一个固定窗口的抖动记录器：记录 `(期望唤醒时刻, 实际时刻)` 的偏差， + //! 给出均值 / 最大值 / P99 与"超预算次数"。 + + use std::collections::VecDeque; + use std::time::{Duration, Instant}; + + /// §7.2 的默认抖动预算（微秒）。 + pub const DEFAULT_JITTER_BUDGET_US: u64 = 200; + + /// 抖动统计快照。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct JitterStats { + /// 样本数。 + pub count: usize, + /// 平均抖动（微秒）。 + pub mean_us: f64, + /// 最大抖动（微秒）。 + pub max_us: u64, + /// 第 99 百分位抖动（微秒）。 + pub p99_us: u64, + /// 超过预算的样本数。 + pub over_budget: usize, + /// 预算（微秒）。 + pub budget_us: u64, + } + + impl JitterStats { + /// 是否所有样本都在预算内。 + #[must_use] + pub const fn within_budget(&self) -> bool { + self.over_budget == 0 + } + + /// 超预算比例（0..=1）。 + #[must_use] + pub fn over_budget_ratio(&self) -> f64 { + if self.count == 0 { 0.0 } else { self.over_budget as f64 / self.count as f64 } + } + + /// 一行描述（日志用）。 + #[must_use] + pub fn describe(&self) -> String { + format!( + "抖动 n={} 均值 {:.0}µs 最大 {}µs P99 {}µs 超预算 {}/{}（预算 {}µs）", + self.count, + self.mean_us, + self.max_us, + self.p99_us, + self.over_budget, + self.count, + self.budget_us + ) + } + } + + /// 固定窗口抖动记录器。 + /// + /// 窗口是**有界**的：控制循环 1 kHz 跑一整天的样本量是 8.6e7， + /// 无界 `Vec` 会把内存吃光——这也是 ARCHITECTURE.md §7.1 强调 `mlockall` + /// 的原因之一（避免统计本身引入页错误）。 + #[derive(Debug, Clone)] + pub struct JitterRecorder { + window: VecDeque, + capacity: usize, + budget_us: u64, + total: u64, + } + + impl JitterRecorder { + /// 构造（`capacity` 至少 1）。 + #[must_use] + pub fn new(capacity: usize, budget_us: u64) -> Self { + let capacity = capacity.max(1); + Self { window: VecDeque::with_capacity(capacity), capacity, budget_us, total: 0 } + } + + /// 用 §7.2 的默认预算构造。 + #[must_use] + pub fn with_default_budget(capacity: usize) -> Self { + Self::new(capacity, DEFAULT_JITTER_BUDGET_US) + } + + /// 记录一次唤醒偏差：`actual` 晚于 `expected` 的时长即抖动。 + /// + /// 早于期望时刻（调度器提前唤醒）记为 0：负抖动在控制回路里没有实际危害， + /// 混进统计只会掩盖真正的问题。 + pub fn record(&mut self, expected: Instant, actual: Instant) -> u64 { + let jitter_us = actual.saturating_duration_since(expected).as_micros() as u64; + self.record_us(jitter_us) + } + + /// 直接记录一个抖动值（微秒）。 + pub fn record_us(&mut self, jitter_us: u64) -> u64 { + if self.window.len() == self.capacity { + self.window.pop_front(); + } + self.window.push_back(jitter_us); + self.total += 1; + // 超预算计数**不在这里维护**：窗口是环形的，出窗样本必须一并扣除， + // 否则计数会单调增长。`stats()` 每次从窗口重算，样本量 ≤ capacity。 + jitter_us + } + + /// 窗口内的样本数。 + #[must_use] + pub fn len(&self) -> usize { + self.window.len() + } + + /// 窗口是否为空。 + #[must_use] + pub fn is_empty(&self) -> bool { + self.window.is_empty() + } + + /// 累计记录次数（含已出窗）。 + #[must_use] + pub const fn total(&self) -> u64 { + self.total + } + + /// 清空窗口与计数。 + pub fn reset(&mut self) { + self.window.clear(); + self.total = 0; + } + + /// 统计快照。 + #[must_use] + pub fn stats(&self) -> JitterStats { + if self.window.is_empty() { + return JitterStats { + count: 0, + mean_us: 0.0, + max_us: 0, + p99_us: 0, + over_budget: 0, + budget_us: self.budget_us, + }; + } + let mut sorted: Vec = self.window.iter().copied().collect(); + sorted.sort_unstable(); + let count = sorted.len(); + let sum: u64 = sorted.iter().sum(); + // 最近秩法取 P99：`ceil(0.99·n) - 1`，n=1 时即唯一元素。 + let index = ((count * 99).div_ceil(100)).saturating_sub(1).min(count - 1); + JitterStats { + count, + mean_us: sum as f64 / count as f64, + max_us: *sorted.last().unwrap_or(&0), + p99_us: sorted[index], + over_budget: sorted.iter().filter(|value| **value > self.budget_us).count(), + budget_us: self.budget_us, + } + } + } + + /// 一次控制拍的时间基准：用固定起点 + 周期推算期望唤醒时刻（§7.2）。 + #[derive(Debug, Clone, Copy)] + pub struct TickClock { + start: Instant, + period: Duration, + tick: u64, + } + + impl TickClock { + /// 构造。 + #[must_use] + pub fn new(start: Instant, period: Duration) -> Self { + Self { start, period, tick: 0 } + } + + /// 当前拍的期望唤醒时刻。 + #[must_use] + pub fn expected(&self) -> Instant { + self.start + self.period * u32::try_from(self.tick).unwrap_or(u32::MAX) + } + + /// 当前拍号。 + #[must_use] + pub const fn tick(&self) -> u64 { + self.tick + } + + /// 前进一拍并给出上一拍的期望时刻。 + pub fn advance(&mut self) -> Instant { + let expected = self.expected(); + self.tick += 1; + expected + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn empty_stats_are_zeroed() { + let recorder = JitterRecorder::with_default_budget(8); + let stats = recorder.stats(); + assert_eq!(stats.count, 0); + assert!((stats.mean_us).abs() < 1e-6); + assert!(stats.within_budget()); + assert!(recorder.is_empty()); + assert_eq!(stats.budget_us, DEFAULT_JITTER_BUDGET_US); + } + + #[test] + fn stats_compute_mean_max_and_p99() { + let mut recorder = JitterRecorder::new(100, 200); + for value in 0..100u64 { + recorder.record_us(value); + } + let stats = recorder.stats(); + assert_eq!(stats.count, 100); + assert!((stats.mean_us - 49.5).abs() < 1e-9, "{}", stats.mean_us); + assert_eq!(stats.max_us, 99); + assert_eq!(stats.p99_us, 98, "最近秩法 P99 应为第 99 个样本"); + assert!(stats.within_budget()); + assert_eq!(recorder.total(), 100); + assert!(stats.describe().contains("P99")); + } + + #[test] + fn over_budget_samples_are_counted_and_ratio_is_reported() { + let mut recorder = JitterRecorder::new(10, 200); + for _ in 0..8 { + recorder.record_us(50); + } + for _ in 0..2 { + recorder.record_us(500); + } + let stats = recorder.stats(); + assert_eq!(stats.over_budget, 2); + assert!(!stats.within_budget()); + assert!((stats.over_budget_ratio() - 0.2).abs() < 1e-9); + assert_eq!(stats.max_us, 500); + } + + #[test] + fn window_is_bounded_and_expires_old_samples() { + let mut recorder = JitterRecorder::new(4, 200); + for _ in 0..10 { + recorder.record_us(999); + } + assert_eq!(recorder.len(), 4, "窗口必须有界（1kHz 长跑不能无界增长）"); + assert_eq!(recorder.total(), 10); + // 出窗后统计只看窗口内样本。 + assert_eq!(recorder.stats().count, 4); + } + + #[test] + fn recording_from_instants_ignores_early_wakeups() { + let mut recorder = JitterRecorder::new(4, 200); + let expected = Instant::now(); + assert_eq!(recorder.record(expected, expected), 0); + // 提前唤醒：用 checked_sub 而不是裸减（裸减在基准附近可能下溢 panic）。 + let early = expected.checked_sub(Duration::from_micros(50)).unwrap(); + assert_eq!(recorder.record(expected, early), 0); + let late = expected + Duration::from_micros(700); + assert_eq!(recorder.record(expected, late), 700); + assert_eq!(recorder.stats().over_budget, 1); + } + + #[test] + fn reset_clears_everything() { + let mut recorder = JitterRecorder::new(4, 200); + recorder.record_us(1000); + recorder.reset(); + assert!(recorder.is_empty()); + assert_eq!(recorder.total(), 0); + assert!(recorder.stats().within_budget()); + } + + #[test] + fn tick_clock_advances_by_the_period() { + let start = Instant::now(); + let mut clock = TickClock::new(start, Duration::from_millis(1)); + assert_eq!(clock.tick(), 0); + assert_eq!(clock.advance(), start); + assert_eq!(clock.tick(), 1); + assert_eq!(clock.advance(), start + Duration::from_millis(1)); + assert_eq!(clock.advance(), start + Duration::from_millis(2)); + assert_eq!(clock.tick(), 3); + } + + #[test] + fn zero_capacity_is_clamped_to_one() { + let mut recorder = JitterRecorder::new(0, 100); + recorder.record_us(1); + recorder.record_us(2); + assert_eq!(recorder.len(), 1); + assert_eq!(recorder.stats().max_us, 2); + } + } +} + +pub use telemetry::*; + +pub mod controller { + //! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。 + + use std::time::Duration; + + use mecanum::{Mecanum, Twist}; + use motor::{Disable, MotorFeedback, OutputShaft, VelocityDrive}; + use tokio::sync::watch; + use tokio::task::JoinHandle; + use tokio::time::{Instant, MissedTickBehavior, interval}; + use tracing::{info, warn}; + + /// 底盘控制器。 + /// + /// 职责（1kHz 节拍）： + /// 1. 读取最新 twist 指令，**指令超时则归零**； + /// 2. 速度限幅（保持方向按比例收缩）； + /// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用； + /// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。 + /// + /// 电机反馈离线安全在驱动层完成（强制 0 电流）。 + pub struct ChassisController { + kinematics: Mecanum, + /// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。 + wheels: [W; 4], + /// (max_vx, max_vy, max_omega)。 + limits: (f32, f32, f32), + command_timeout: Duration, + } + + impl ChassisController + where + W: VelocityDrive + MotorFeedback + Disable + 'static, + { + /// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。 + #[must_use] + pub fn new( + kinematics: Mecanum, + wheels: [W; 4], + limits: (f32, f32, f32), + command_timeout: Duration, + ) -> Self { + Self { kinematics, wheels, limits, command_timeout } + } + + /// 启动控制循环任务。 + pub fn spawn(self, commands: watch::Receiver, period: Duration) -> JoinHandle<()> { + tokio::spawn(self.run(commands, period)) + } + + async fn run(self, mut commands: watch::Receiver, period: Duration) { + let mut ticker = interval(period); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + let mut latest = Twist::ZERO; + let mut last_command_at: Option = None; + let mut ticks: u64 = 0; + + loop { + ticker.tick().await; + let now = Instant::now(); + + if commands.has_changed().unwrap_or(false) { + latest = *commands.borrow_and_update(); + last_command_at = Some(now); + } + let timed_out = match last_command_at { + Some(t) => now.duration_since(t) > self.command_timeout, + // 尚未收到任何指令：保持静止。 + None => true, + }; + let twist = if timed_out { + Twist::ZERO + } else { + latest.clamped(self.limits.0, self.limits.1, self.limits.2) + }; + + let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback()); + if all_seen_feedback { + let wheel_rpms = self.kinematics.wheel_rpms(&twist); + for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) { + motor.set_velocity(wheel_rpm); + } + } else { + for motor in &self.wheels { + motor.disable(); + } + if ticks.is_multiple_of(1000) { + warn!("waiting for first feedback from all motors"); + } + } + + ticks += 1; + if ticks.is_multiple_of(1000) { + self.log_status(); + } + } + } + + fn log_status(&self) { + let (rotor_rpm, effort_raw): (Vec, Vec) = self + .wheels + .iter() + .map(|m| { + let st = m.state(); + (st.rotor_speed_rpm, i32::from(st.effort_raw)) + }) + .unzip(); + info!(?rotor_rpm, ?effort_raw, "chassis status"); + } + } + + /// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。 + #[must_use] + pub fn wrap_wheels( + motors: [T; N], + gear_ratios: [f32; N], + ) -> [OutputShaft; N] { + let mut out = Vec::with_capacity(N); + let mut gears = gear_ratios.into_iter(); + for m in motors { + let g = gears.next().unwrap_or(1.0); + out.push(OutputShaft::new(m, g)); + } + out.try_into().unwrap_or_else(|_| unreachable!()) + } + + #[cfg(test)] + mod tests { + use super::*; + use motor::{Command, MockMotor, MotorState}; + + const GEAR: f32 = 19.203; + + fn mock_wheel(id: u8) -> MockMotor { + let m = MockMotor::new(id); + m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() }); + m + } + + /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。 + #[tokio::test(start_paused = true)] + async fn controller_drives_wheels_through_capability_traits() { + let kinematics = Mecanum::new(0.2, 0.25, 0.0762); + let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)]; + let ctrl = ChassisController::new( + kinematics, + wrap_wheels(mocks.clone(), [GEAR; 4]), + (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)); + + let twist = Twist { vx: 1.0, ..Twist::ZERO }; + tx.send_replace(twist); + tokio::time::sleep(Duration::from_millis(5)).await; + + let expected = kinematics.wheel_rpms(&twist); + for (i, mock) in mocks.iter().enumerate() { + match mock.last_command() { + Command::Velocity(rotor_rpm) => { + let want = expected[i] * GEAR; + assert!( + (rotor_rpm - want).abs() < 1e-2, + "wheel {i}: rotor_rpm {rotor_rpm} != {want}" + ); + } + ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"), + } + } + + handle.abort(); + } + + /// 反馈未到齐时必须全部停用，不得下发速度。 + #[tokio::test(start_paused = true)] + async fn disables_wheels_until_all_feedback_seen() { + let kinematics = Mecanum::new(0.2, 0.25, 0.0762); + let ready = mock_wheel(1); + 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 (tx, rx) = watch::channel(Twist::ZERO); + let handle = ctrl.spawn(rx, Duration::from_millis(1)); + + tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + tokio::time::sleep(Duration::from_millis(5)).await; + + assert_eq!(ready.last_command(), Command::Disabled); + assert_eq!(missing.last_command(), Command::Disabled); + + handle.abort(); + } + } +} + +pub use controller::*; + +// `chassisd`：麦轮底盘守护进程（4 × GM3508 / C620，SocketCAN）。 use std::path::PathBuf; use std::process::ExitCode; use std::time::Duration; @@ -10,9 +776,12 @@ use tokio::sync::watch; use tracing::{error, info}; use tracing_subscriber::EnvFilter; -use chassisd::config::Config; -use chassisd::controller::{ChassisController, wrap_wheels}; -use motor_core::Disable; +use crate::{ChassisController, wrap_wheels}; +use can::CanLink; +use chassis_config::Config; +use esc_bus::{EscBus, EscMotor}; +use motor::Disable; +use motor_control::MotorPolicy; /// 命令行参数。 #[derive(Debug, Parser)] @@ -29,6 +798,10 @@ struct Args { /// 无硬件仿真模式：进程内电机模型，全链路可离线运行。 #[arg(long)] mock: bool, + + /// 只装配配置、链路、总线和控制任务，短跑后安全退出。 + #[arg(long)] + dry_run: bool, } fn main() -> ExitCode { @@ -54,7 +827,7 @@ fn main() -> ExitCode { return ExitCode::FAILURE; } }; - match runtime.block_on(run(config, &interface, args.mock)) { + match runtime.block_on(run(config, &interface, args.mock, args.dry_run)) { Ok(()) => ExitCode::SUCCESS, Err(e) => { error!("{e}"); @@ -80,14 +853,15 @@ async fn run( config: Config, interface: &str, mock: bool, + dry_run: bool, ) -> Result<(), Box> { let bus_options = config.bus_options(); // 1) CAN 链路：仿真或真实 SocketCAN。 let link = if mock { - let (bus_side, plant_side) = dji_esc::CanLink::pair(512); + let (bus_side, plant_side) = CanLink::pair(512); let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); - dji_esc_sim::spawn_plant(plant_side, &esc_ids, bus_options.period); + sim_harness::spawn_plant(plant_side, &esc_ids, bus_options.period); info!(?esc_ids, "mock 模式：进程内电机仿真"); bus_side } else { @@ -96,8 +870,12 @@ async fn run( // 2) 电调总线。 let period = bus_options.period; - let bus = dji_esc::EscBus::spawn(link, config.to_esc_configs(), bus_options)?; - let names: Vec<&str> = bus.motors().iter().map(dji_esc::EscMotor::name).collect(); + let mut policy = MotorPolicy::new(); + for motor in &config.motors { + policy.configure(motor.esc_id, motor.current_limit, motor.velocity_gains()); + } + let bus = EscBus::spawn(link, config.to_esc_configs(), bus_options, policy)?; + let names: Vec<&str> = bus.motors().iter().map(EscMotor::name).collect(); info!( interface, mock, @@ -107,10 +885,10 @@ async fn run( ); // 3) 控制循环（当前无指令源：watch 保持 ZERO，安全静止； - // 遥控 / 自主导航后续经 chassisd::source 接入）。 + // 遥控 / 自主导航后续经 chassis_config::source 接入）。 let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega); - let motors: [dji_esc::EscMotor; 4] = + let motors: [EscMotor; 4] = bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); let gears: [f32; 4] = config .motors @@ -128,6 +906,17 @@ async fn run( .spawn(twist_rx, period); let _twist_tx = twist_tx; // 保留发送端供后续指令源使用 + if dry_run { + tokio::time::sleep(Duration::from_millis(20)).await; + for motor in bus.motors() { + motor.disable(); + } + tokio::time::sleep(Duration::from_millis(5)).await; + bus.shutdown().await; + info!("chassisd dry-run 完成"); + return Ok(()); + } + // 4) 优雅退出：ctrl-c 或控制任务意外退出。 tokio::select! { _ = tokio::signal::ctrl_c() => info!("收到 ctrl-c，正在停车…"), @@ -148,10 +937,10 @@ async fn run( } /// 打开真实总线（仅 Linux；其他平台给出明确错误）。 -fn open_real_link(interface: &str) -> Result> { +fn open_real_link(interface: &str) -> Result> { #[cfg(target_os = "linux")] { - Ok(dji_esc_socketcan::open_link(interface)?) + Ok(socketcan::open_link(interface)?) } #[cfg(not(target_os = "linux"))] { @@ -159,3 +948,181 @@ fn open_real_link(interface: &str) -> Result (EscBusHandle, watch::Sender) { + let (bus_side, plant_side) = CanLink::pair(512); + let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); + spawn_plant(plant_side, &esc_ids, config.bus_options().period); + + let mut policy = MotorPolicy::new(); + for motor in &config.motors { + policy.configure(motor.esc_id, motor.current_limit, motor.velocity_gains()); + } + let bus = + EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options(), policy).unwrap(); + + let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); + let motors: [EscMotor; 4] = + bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let gears: [f32; 4] = config + .motors + .iter() + .map(|m| m.gear_ratio) + .collect::>() + .try_into() + .unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let controller = ChassisController::new( + config.kinematics(), + wrap_wheels(motors, gears), + (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega), + Duration::from_millis(command_timeout_ms), + ); + controller.spawn(twist_rx, config.bus_options().period); + (bus, twist_tx) + } + + async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool { + let mut waited = 0u64; + while waited < total_ms { + if cond() { + return true; + } + sleep(Duration::from_millis(5)).await; + waited += 5; + } + cond() + } + + fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] { + bus.motors() + .iter() + .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio) + .collect::>() + .try_into() + .unwrap() + } + + /// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。 + const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762); + + #[tokio::test(start_paused = true)] + async fn forward_command_drives_all_wheels_equally() { + let config = Config::mock_default(); + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + let target = RPM_PER_MPS; + let ok = wait_until(3000, || { + wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!(ok, "未收敛到 {target} rpm: {rpm:?}"); + + // 停车：指令归零后各轮减速到停。 + twist_tx.send_replace(Twist::ZERO); + let stopped = + wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; + let rpm = wheel_rpms(&bus); + assert!(stopped, "未停车: {rpm:?}"); + bus.shutdown().await; + } + + #[tokio::test(start_paused = true)] + async fn rotation_command_respects_wheel_signs() { + let config = Config::mock_default(); + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO }); + // a·ω = (0.2 + 0.25) m/s 的轮线速度。 + let target = 0.45 * RPM_PER_MPS; + let ok = wait_until(3000, || { + let rpm = wheel_rpms(&bus); + // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。 + [ + (rpm[0] + target).abs(), + (rpm[1] - target).abs(), + (rpm[2] + target).abs(), + (rpm[3] - target).abs(), + ] + .iter() + .all(|e| *e < 0.05 * target) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}"); + bus.shutdown().await; + } + + #[tokio::test(start_paused = true)] + async fn command_timeout_stops_chassis() { + let config = Config::mock_default(); + // 指令超时使用配置原值（200ms）。 + let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms); + + // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。 + twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + let moving = + wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)) + .await; + assert!(moving, "指令后底盘应当运动"); + + sleep(Duration::from_millis(400)).await; // 超过 200ms 超时 + let stopped = + wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; + let rpm = wheel_rpms(&bus); + assert!(stopped, "指令超时后未停车: {rpm:?}"); + bus.shutdown().await; + } + + #[tokio::test(start_paused = true)] + async fn speed_limits_scale_motion() { + let mut config = Config::mock_default(); + config.chassis.max_vx = 1.0; // 限速 1 m/s + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(Twist { + vx: 3.0, // 超限指令 + vy: 2.0, + omega: 0.0, + }); + // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy： + // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。 + let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS; + let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS; + let tol = 0.08 * diag_fast; + let ok = wait_until(3000, || { + let rpm = wheel_rpms(&bus); + [ + (rpm[0] - diag_slow).abs(), + (rpm[1] - diag_fast).abs(), + (rpm[2] - diag_fast).abs(), + (rpm[3] - diag_slow).abs(), + ] + .iter() + .all(|e| *e < tol) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!( + ok, + "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}" + ); + bus.shutdown().await; + } +} diff --git a/crates/chassisd/src/source.rs b/crates/chassisd/src/source.rs deleted file mode 100644 index e53da84..0000000 --- a/crates/chassisd/src/source.rs +++ /dev/null @@ -1,81 +0,0 @@ -//! 上层速度指令源（twist source）。 -//! -//! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中 -//! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现 -//! [`TwistSource`] 即可。 - -use std::future::Future; - -use mecanum::Twist; -use tokio::sync::{mpsc, watch}; - -/// 速度指令源。 -pub trait TwistSource: Send { - /// 取下一条指令；`None` 表示源关闭。 - fn next_twist(&mut self) -> impl Future> + Send; -} - -/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。 -pub struct ZeroSource; - -impl TwistSource for ZeroSource { - async fn next_twist(&mut self) -> Option { - std::future::pending().await - } -} - -/// 通道指令源（测试 / 简单进程内注入用）。 -pub struct ChannelSource { - rx: mpsc::Receiver, -} - -impl ChannelSource { - /// 由 mpsc 接收端构造。 - #[must_use] - pub const fn new(rx: mpsc::Receiver) -> Self { - Self { rx } - } - - /// 创建一对（发送端, 源）。 - #[must_use] - pub fn pair(bound: usize) -> (mpsc::Sender, Self) { - let (tx, rx) = mpsc::channel(bound); - (tx, Self { rx }) - } -} - -impl TwistSource for ChannelSource { - async fn next_twist(&mut self) -> Option { - self.rx.recv().await - } -} - -/// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。 -pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender) { - while let Some(twist) = source.next_twist().await { - if tx.send(twist).is_err() { - break; - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[tokio::test(start_paused = true)] - async fn channel_source_pumps_into_watch() { - let (tx, source) = ChannelSource::pair(8); - let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO); - let pump = tokio::spawn(pump_source(source, watch_tx)); - - tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap(); - tokio::time::sleep(std::time::Duration::from_millis(1)).await; - assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 }); - - // 源关闭 → 泵任务退出。 - drop(tx); - pump.await.unwrap(); - assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值 - } -} diff --git a/crates/chassisd/tests/daemon.rs b/crates/chassisd/tests/daemon.rs deleted file mode 100644 index c8bdf65..0000000 --- a/crates/chassisd/tests/daemon.rs +++ /dev/null @@ -1,170 +0,0 @@ -//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机 -//! → 控制循环 → 麦轮运动学），全平台可跑。 - -#![allow(clippy::unwrap_used, clippy::expect_used)] - -use std::time::Duration; - -use chassisd::config::Config; -use chassisd::controller::{ChassisController, wrap_wheels}; -use dji_esc::{CanLink, EscBus, EscBusHandle}; -use dji_esc_sim::spawn_plant; -use mecanum::Twist; -use motor_core::MotorFeedback; -use tokio::sync::watch; -use tokio::time::sleep; - -/// 搭建与 `chassisd --mock` 完全同构的软件栈。 -/// -/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的 -/// 指令源），超时测试用配置原值。 -fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender) { - let (bus_side, plant_side) = CanLink::pair(512); - let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); - spawn_plant(plant_side, &esc_ids, config.bus_options().period); - - let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap(); - - let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); - let motors: [dji_esc::EscMotor; 4] = - bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); - let gears: [f32; 4] = config - .motors - .iter() - .map(|m| m.gear_ratio) - .collect::>() - .try_into() - .unwrap_or_else(|_| unreachable!("exactly 4 motors")); - let controller = ChassisController::new( - config.kinematics(), - wrap_wheels(motors, gears), - (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega), - Duration::from_millis(command_timeout_ms), - ); - controller.spawn(twist_rx, config.bus_options().period); - (bus, twist_tx) -} - -async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool { - let mut waited = 0u64; - while waited < total_ms { - if cond() { - return true; - } - sleep(Duration::from_millis(5)).await; - waited += 5; - } - cond() -} - -fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] { - bus.motors() - .iter() - .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio) - .collect::>() - .try_into() - .unwrap() -} - -/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。 -const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762); - -#[tokio::test(start_paused = true)] -async fn forward_command_drives_all_wheels_equally() { - let config = Config::mock_default(); - let (bus, twist_tx) = stack(&config, 10_000); - - twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); - let target = RPM_PER_MPS; - let ok = wait_until(3000, || { - wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target) - }) - .await; - let rpm = wheel_rpms(&bus); - assert!(ok, "未收敛到 {target} rpm: {rpm:?}"); - - // 停车：指令归零后各轮减速到停。 - twist_tx.send_replace(Twist::ZERO); - let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; - let rpm = wheel_rpms(&bus); - assert!(stopped, "未停车: {rpm:?}"); - bus.shutdown().await; -} - -#[tokio::test(start_paused = true)] -async fn rotation_command_respects_wheel_signs() { - let config = Config::mock_default(); - let (bus, twist_tx) = stack(&config, 10_000); - - twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO }); - // a·ω = (0.2 + 0.25) m/s 的轮线速度。 - let target = 0.45 * RPM_PER_MPS; - let ok = wait_until(3000, || { - let rpm = wheel_rpms(&bus); - // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。 - [ - (rpm[0] + target).abs(), - (rpm[1] - target).abs(), - (rpm[2] + target).abs(), - (rpm[3] - target).abs(), - ] - .iter() - .all(|e| *e < 0.05 * target) - }) - .await; - let rpm = wheel_rpms(&bus); - assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}"); - bus.shutdown().await; -} - -#[tokio::test(start_paused = true)] -async fn command_timeout_stops_chassis() { - let config = Config::mock_default(); - // 指令超时使用配置原值（200ms）。 - let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms); - - // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。 - twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); - let moving = - wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await; - assert!(moving, "指令后底盘应当运动"); - - sleep(Duration::from_millis(400)).await; // 超过 200ms 超时 - let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; - let rpm = wheel_rpms(&bus); - assert!(stopped, "指令超时后未停车: {rpm:?}"); - bus.shutdown().await; -} - -#[tokio::test(start_paused = true)] -async fn speed_limits_scale_motion() { - let mut config = Config::mock_default(); - config.chassis.max_vx = 1.0; // 限速 1 m/s - let (bus, twist_tx) = stack(&config, 10_000); - - twist_tx.send_replace(Twist { - vx: 3.0, // 超限指令 - vy: 2.0, - omega: 0.0, - }); - // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy： - // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。 - let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS; - let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS; - let tol = 0.08 * diag_fast; - let ok = wait_until(3000, || { - let rpm = wheel_rpms(&bus); - [ - (rpm[0] - diag_slow).abs(), - (rpm[1] - diag_fast).abs(), - (rpm[2] - diag_fast).abs(), - (rpm[3] - diag_slow).abs(), - ] - .iter() - .all(|e| *e < tol) - }) - .await; - let rpm = wheel_rpms(&bus); - assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}"); - bus.shutdown().await; -} diff --git a/crates/delta10-protocol/Cargo.toml b/crates/delta10-protocol/Cargo.toml new file mode 100644 index 0000000..f63cafc --- /dev/null +++ b/crates/delta10-protocol/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "delta10-protocol" +description = "DETA10 IMU wire protocol" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +imu = { workspace = true } +thiserror = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/delta10-protocol/src/lib.rs b/crates/delta10-protocol/src/lib.rs new file mode 100644 index 0000000..8893169 --- /dev/null +++ b/crates/delta10-protocol/src/lib.rs @@ -0,0 +1,301 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! DETA10 IMU 的 CAN 帧编解码（ARCHITECTURE.md §5.2 / 决策点 **D1**）。 +//! +//! # 为什么这里没有"实现好"的解码 +//! +//! ARCHITECTURE.md §11.4 把「delta10 的 CAN ID 和协议模式」列为**需要人工确认的 +//! 决策点 D1**（需查阅数据手册），而文档 §12.2 给出的手册链接 +//! `https://fdisystems.github.io/DETA10_Docs/` **已经 404**，厂商现手册为 +//! `fdisystems.cn` 上的 PDF。 +//! +//! 在没有手册的前提下臆造字节偏移，比留一个空实现更糟：它会以"看起来能用" +//! 的姿态把错误的 roll/pitch/yaw 送进云台闭环。因此本模块： +//! +//! 1. 把**能确定的部分**做实：帧 ID 匹配、DLC 校验、`ImuData` 组装路径； +//! 2. 把**不能确定的部分**参数化为 [`Delta10Layout`]（字段偏移 + 标度）； +//! 3. 未配置布局时解码返回 [`ImuError::LayoutUnconfirmed`]（显式、可测、不会 +//! 静默给错数据）。D1 确认后**只需填一组常量**，无需改动任何调用方。 +//! +//! # 已知的确定事实 +//! +//! - 设备经 CAN 接入，挂在与电机共享的总线上（ARCHITECTURE.md §2.1）； +//! - 文档记录了两种协议模式：FDILINK 与「标准」，见 [`Delta10Mode`]； +//! - 上层需要的是 [`ImuData`]（accel / gyro / mag / roll / pitch / yaw / 时间戳）。 + +use imu::{ImuData, ImuError}; +use thiserror::Error; + +use can::CanFrame; + +/// 载荷里数值字段的宽度与编码。 +/// +/// DETA10 的 FDILink 帧用 `float32`，但一个 8 字节 CAN 帧装不下三个 `f32`， +/// 因此真实设备必然是"多帧"或"定点压缩"之一——**这正是 D1 要确认的内容**。 +/// 把宽度显式建模出来，D1 确认后只需填一张常量表。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum FieldWidth { + /// 4 字节小端 `f32`。 + F32, + /// 2 字节小端 `i16`（定点，配合 `scale`）。 + I16, +} + +impl FieldWidth { + /// 字节数。 + #[must_use] + pub const fn bytes(self) -> usize { + match self { + Self::F32 => 4, + Self::I16 => 2, + } + } +} + +/// 载荷里一个数值字段的定义（偏移 + 宽度 + 标度）。 +/// +/// 有了它，"确认手册后要改什么"就只剩一张表。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct FieldLayout { + /// 在 8 字节载荷中的起始偏移。 + pub offset: usize, + /// 数值宽度与编码。 + pub width: FieldWidth, + /// 原始值 → 物理量的比例（如 0.001 表示原始值单位是 1/1000）。 + pub scale: f32, +} + +impl FieldLayout { + /// 构造。 + #[must_use] + pub const fn new(offset: usize, width: FieldWidth, scale: f32) -> Self { + Self { offset, width, scale } + } + + /// 从载荷里取一个字段；越界返回 `None`。 + #[must_use] + pub fn read(&self, data: &[u8; 8]) -> Option { + let end = self.offset.checked_add(self.width.bytes())?; + if end > data.len() { + return None; + } + let raw = match self.width { + FieldWidth::F32 => { + let mut bytes = [0u8; 4]; + bytes.copy_from_slice(&data[self.offset..end]); + f32::from_le_bytes(bytes) + } + FieldWidth::I16 => { + let mut bytes = [0u8; 2]; + bytes.copy_from_slice(&data[self.offset..end]); + f32::from(i16::from_le_bytes(bytes)) + } + }; + Some(raw * self.scale) + } +} + +/// DETA10 载荷布局（**待 D1 确认**）。 +/// +/// 单帧 8 字节装不下 9 个浮点量，因此真实设备要么用多帧、要么用定点压缩。 +/// 本结构支持"单帧 + 字段子集"的形态；多帧重组待手册确认后按同样思路扩展。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Delta10Layout { + /// 横滚角（度）字段。 + pub roll: FieldLayout, + /// 俯仰角（度）字段。 + pub pitch: FieldLayout, + /// 偏航角（度）字段。 + pub yaw: FieldLayout, +} + +/// 协议模式（ARCHITECTURE.md §2.2 记录了两者，先按哪种接需 D1 确认）。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum Delta10Mode { + /// FDILINK 协议模式。 + Fdilink, + /// 标准模式。 + Standard, +} + +/// DETA10 接入配置。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Delta10Config { + /// 反馈帧的 CAN ID（**待 D1 确认**，默认取 `0x100` 占位）。 + pub can_id: u32, + /// 协议模式。 + pub mode: Delta10Mode, + /// 载荷布局；`None` 表示尚未确认，解码会返回 + /// [`ImuError::LayoutUnconfirmed`]。 + pub layout: Option, +} + +impl Default for Delta10Config { + fn default() -> Self { + // 0x100 只是占位（ARCHITECTURE.md §10.3 的 robot.toml 里也把它写成 + // `imu_can_id = 0x100`，同样标注为待确认）。它**不应**被当作事实使用： + // 布局为 None，因此默认配置下解码一定报 LayoutUnconfirmed。 + Self { can_id: 0x100, mode: Delta10Mode::Fdilink, layout: None } + } +} + +impl Delta10Config { + /// 换一个 CAN ID（D1 确认后由配置注入）。 + #[must_use] + pub const fn with_can_id(mut self, can_id: u32) -> Self { + self.can_id = can_id; + self + } + + /// 换协议模式。 + #[must_use] + pub const fn with_mode(mut self, mode: Delta10Mode) -> Self { + self.mode = mode; + self + } + + /// 装上载荷布局（D1 确认后调用）。 + #[must_use] + pub const fn with_layout(mut self, layout: Delta10Layout) -> Self { + self.layout = Some(layout); + self + } +} + +/// 解码结果：`Ok(None)` = 不是 IMU 帧（正常情况，总线上大部分帧是电机反馈）。 +/// +/// # Errors +/// +/// - [`ImuError::BadFrame`]：ID 匹配但长度不足； +/// - [`ImuError::LayoutUnconfirmed`]：ID 匹配但布局未确认（D1）； +/// - [`ImuError::Checksum`]：字段读取越界（布局与载荷不匹配）。 +pub fn try_decode( + config: &Delta10Config, + frame: &CanFrame, + timestamp_ns: u64, +) -> Result, ImuError> { + if frame.id != config.can_id { + return Ok(None); + } + if !frame.is_complete() { + return Err(ImuError::BadFrame); + } + let Some(layout) = config.layout else { + return Err(ImuError::LayoutUnconfirmed); + }; + let roll = layout.roll.read(&frame.data).ok_or(ImuError::Checksum)?; + let pitch = layout.pitch.read(&frame.data).ok_or(ImuError::Checksum)?; + let yaw = layout.yaw.read(&frame.data).ok_or(ImuError::Checksum)?; + Ok(Some(ImuData { + roll, + pitch, + yaw, + timestamp_ns, + // 本帧只承载姿态角；加速度 / 角速度需要额外的帧（待手册确认） + ..ImuData::default() + })) +} + +/// 配置错误：把"不确定"和"确定错"分开。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)] +pub enum Delta10ConfigError { + /// 布局字段越过 8 字节载荷。 + #[error("DETA10 字段偏移 {offset} 超出 8 字节载荷")] + FieldOutOfRange { + /// 越界的偏移。 + offset: usize, + }, + /// 布局非有限标度。 + #[error("DETA10 字段标度必须为有限正值")] + NonFiniteScale, +} + +/// 校验布局能否落在载荷内（D1 确认后填入常量时用）。 +/// +/// # Errors +/// +/// 字段越界或标度非法时返回错误。 +pub fn validate_layout(layout: &Delta10Layout) -> Result<(), Delta10ConfigError> { + for field in [layout.roll, layout.pitch, layout.yaw] { + if !(field.scale.is_finite() && field.scale > 0.0) { + return Err(Delta10ConfigError::NonFiniteScale); + } + if field.offset + field.width.bytes() > 8 { + return Err(Delta10ConfigError::FieldOutOfRange { offset: field.offset }); + } + } + Ok(()) +} + +#[cfg(test)] +mod tests { + use super::*; + + /// 一个**能装进 8 字节**的合成布局：3 × i16 定点（0.001 度/LSB）。 + /// + /// 它只用来证明管道可用；真实 DETA10 的偏移/宽度/标度待 D1 确认。 + fn fitted_layout() -> Delta10Layout { + Delta10Layout { + roll: FieldLayout::new(0, FieldWidth::I16, 0.001), + pitch: FieldLayout::new(2, FieldWidth::I16, 0.001), + yaw: FieldLayout::new(4, FieldWidth::I16, 0.001), + } + } + + #[test] + fn non_imu_frames_are_ignored_not_errors() { + let config = Delta10Config::default().with_can_id(0x100); + let motor = CanFrame::new(0x201, [0; 8]); + assert_eq!(try_decode(&config, &motor, 0), Ok(None)); + } + + #[test] + fn default_config_refuses_to_guess_the_layout() { + let config = Delta10Config::default().with_can_id(0x100); + let imu_frame = CanFrame::new(0x100, [1, 2, 3, 4, 5, 6, 7, 8]); + assert_eq!(try_decode(&config, &imu_frame, 7), Err(ImuError::LayoutUnconfirmed)); + } + + #[test] + fn short_imu_frames_are_rejected() { + let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()); + let short = CanFrame::new(0x100, [0; 8]).with_dlc(4); + assert_eq!(try_decode(&config, &short, 0), Err(ImuError::BadFrame)); + } + + #[test] + fn configured_layout_decodes_attitude_and_stamps_time() { + let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()); + let mut data = [0u8; 8]; + // roll = 1.5°, pitch = -2.25°（原始值为毫度） + data[0..2].copy_from_slice(&1500i16.to_le_bytes()); + data[2..4].copy_from_slice(&(-2250i16).to_le_bytes()); + let frame = CanFrame::new(0x100, data); + let decoded = try_decode(&config, &frame, 42).unwrap().unwrap(); + assert!((decoded.roll - 1.5).abs() < 1e-6, "{}", decoded.roll); + assert!((decoded.pitch + 2.25).abs() < 1e-6, "{}", decoded.pitch); + assert_eq!(decoded.timestamp_ns, 42); + assert!(decoded.is_finite()); + } + + #[test] + fn layout_validation_catches_out_of_range_offsets() { + // offset 6 + 4 字节 f32 = 10 > 8：越界 + let bad = Delta10Layout { + roll: FieldLayout::new(0, FieldWidth::F32, 1.0), + pitch: FieldLayout::new(2, FieldWidth::I16, 1.0), + yaw: FieldLayout::new(6, FieldWidth::F32, 1.0), + }; + assert!(matches!( + validate_layout(&bad), + Err(Delta10ConfigError::FieldOutOfRange { offset: 6 }) + )); + let zero_scale = Delta10Layout { + roll: FieldLayout::new(0, FieldWidth::I16, 0.0), + pitch: FieldLayout::new(2, FieldWidth::I16, 1.0), + yaw: FieldLayout::new(4, FieldWidth::I16, 1.0), + }; + assert_eq!(validate_layout(&zero_scale), Err(Delta10ConfigError::NonFiniteScale)); + assert!(validate_layout(&fitted_layout()).is_ok()); + } +} diff --git a/crates/delta10/Cargo.toml b/crates/delta10/Cargo.toml new file mode 100644 index 0000000..fd8a90a --- /dev/null +++ b/crates/delta10/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "delta10" +description = "DETA10 IMU driver" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +imu = { workspace = true } +delta10-protocol = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } diff --git a/crates/delta10/src/lib.rs b/crates/delta10/src/lib.rs new file mode 100644 index 0000000..71bcfae --- /dev/null +++ b/crates/delta10/src/lib.rs @@ -0,0 +1,193 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! DETA10 IMU 驱动：把总线上的 IMU 帧变成 [`ImuData`]（ARCHITECTURE.md §5.3）。 +//! +//! 本驱动只负责**收发与筛选**：帧格式与载荷布局的解析在 +//! [`motor_protocol::delta10`]，那里也记录了为什么载荷布局仍是待确认的决策点 D1。 +//! 因此本模块在默认配置下会返回 [`ImuError::LayoutUnconfirmed`]—— +//! 这是**有意的**：宁可明确报错，也不把猜出来的姿态角送进云台闭环。 + +use can::CanBus; +use delta10_protocol::{self as delta10, Delta10Config}; +use imu::{Imu, ImuData, ImuError}; + +fn now_ns() -> u64 { + use std::time::{SystemTime, UNIX_EPOCH}; + SystemTime::now().duration_since(UNIX_EPOCH).map(|d| d.as_nanos() as u64).unwrap_or_default() +} + +/// 挂在任意 [`CanBus`] 上的 DETA10 驱动。 +/// +/// 与电机共享总线（ARCHITECTURE.md §2.1），因此 [`Delta10Driver::read`] 会 +/// **跳过**所有不是 IMU 的帧（总线上大部分帧是电机反馈）。 +#[derive(Debug)] +pub struct Delta10Driver { + bus: B, + config: Delta10Config, +} + +impl Delta10Driver { + /// 组装。 + #[must_use] + pub fn new(bus: B, config: Delta10Config) -> Self { + Self { bus, config } + } + + /// 当前配置（D1 确认后由上层换成带布局的配置）。 + #[must_use] + pub const fn config(&self) -> &Delta10Config { + &self.config + } + + /// 热更新配置（例如运行期从配置中心注入确认后的布局）。 + pub fn set_config(&mut self, config: Delta10Config) { + self.config = config; + } + + /// 取回底层总线。 + #[must_use] + pub fn into_inner(self) -> B { + self.bus + } +} + +impl Delta10Driver { + /// 阻塞读取下一帧**姿态**数据，跳过非 IMU 帧。 + /// + /// # Errors + /// + /// - 总线关闭 → [`ImuError::Closed`]； + /// - 帧 ID 命中但布局未确认 → [`ImuError::LayoutUnconfirmed`]（D1）； + /// - 帧不合法 → [`ImuError::BadFrame`]。 + pub async fn read(&mut self) -> Result { + loop { + let frame = self.bus.recv().await.map_err(|_| ImuError::Closed)?; + // 非 IMU 帧（总线上大部分是电机反馈）直接丢弃，继续等下一帧。 + if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? { + return Ok(data); + } + } + } + + /// 非阻塞读取：本轮没有 IMU 帧时返回 `Ok(None)`。 + /// + /// # Errors + /// + /// 同 [`Delta10Driver::read`]（只是不会等待）。 + pub fn try_read(&mut self) -> Result, ImuError> { + loop { + let frame = match self.bus.try_recv() { + Ok(Some(frame)) => frame, + Ok(None) => return Ok(None), + Err(_) => return Err(ImuError::Closed), + }; + if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? { + return Ok(Some(data)); + } + } + } +} + +impl Imu for Delta10Driver { + type Error = ImuError; + + async fn read(&mut self) -> Result { + Self::read(self).await + } + + fn try_read(&mut self) -> Result, Self::Error> { + Self::try_read(self) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use can::CanFrame; + use delta10_protocol::{Delta10Layout, FieldLayout, FieldWidth}; + + use can::MockBus; + + fn fitted_layout() -> Delta10Layout { + Delta10Layout { + roll: FieldLayout::new(0, FieldWidth::I16, 0.001), + pitch: FieldLayout::new(2, FieldWidth::I16, 0.001), + yaw: FieldLayout::new(4, FieldWidth::I16, 0.001), + } + } + + fn imu_frame(roll_mdeg: i16) -> CanFrame { + let mut data = [0u8; 8]; + data[0..2].copy_from_slice(&roll_mdeg.to_le_bytes()); + CanFrame::new(0x100, data) + } + + #[tokio::test] + async fn read_skips_motor_frames_and_returns_attitude() { + let mut bus = MockBus::new(); + bus.inject(CanFrame::new(0x201, [0; 8])); // 电机反馈：必须被跳过 + bus.inject(CanFrame::new(0x202, [0; 8])); + bus.inject(imu_frame(2500)); + let mut driver = Delta10Driver::new( + bus, + Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()), + ); + let data = driver.read().await.unwrap(); + assert!((data.roll - 2.5).abs() < 1e-6, "{}", data.roll); + assert!(data.is_finite()); + } + + #[tokio::test] + async fn default_config_refuses_to_guess() { + let mut bus = MockBus::new(); + bus.inject(imu_frame(1000)); + let mut driver = Delta10Driver::new(bus, Delta10Config::default().with_can_id(0x100)); + assert_eq!(driver.read().await.unwrap_err(), ImuError::LayoutUnconfirmed); + } + + #[tokio::test] + async fn try_read_polls_without_blocking() { + let bus = MockBus::new(); + let mut driver = Delta10Driver::new( + bus, + Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()), + ); + assert_eq!(driver.try_read().unwrap(), None, "空总线不是错误"); + + driver.into_inner().inject(imu_frame(-1500)); + // 重新组装以取回总线（inject 需要 &mut）。 + let mut bus = MockBus::new(); + bus.inject(imu_frame(-1500)); + let mut driver = Delta10Driver::new( + bus, + Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()), + ); + let data = driver.try_read().unwrap().unwrap(); + assert!((data.roll + 1.5).abs() < 1e-6, "{}", data.roll); + } + + #[tokio::test] + async fn closed_bus_is_reported_as_closed() { + let mut bus = MockBus::new(); + bus.close(); + let mut driver = Delta10Driver::new( + bus, + Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()), + ); + assert_eq!(driver.read().await.unwrap_err(), ImuError::Closed); + assert_eq!(driver.try_read().unwrap_err(), ImuError::Closed); + } + + #[tokio::test] + async fn works_through_the_imu_trait() { + let mut bus = MockBus::new(); + bus.inject(imu_frame(3000)); + let mut driver = Delta10Driver::new( + bus, + Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()), + ); + // 经 `Imu` trait 调用（证明驱动满足 L1 的抽象边界）。 + let data: ImuData = Imu::read(&mut driver).await.unwrap(); + assert!((data.roll - 3.0).abs() < 1e-6); + } +} diff --git a/crates/dji-esc-sim/src/ensemble.rs b/crates/dji-esc-sim/src/ensemble.rs deleted file mode 100644 index 09c8a70..0000000 --- a/crates/dji-esc-sim/src/ensemble.rs +++ /dev/null @@ -1,186 +0,0 @@ -//! 被控对象集成：用同一组参数的**抖动样本**表达模型不确定性。 -//! -//! 单点模型整定出来的增益往往对增益 / 时间常数 / 延迟 / 摩擦高度敏感 -//! （真车轮子之间、电量与温度变化都会漂移）。整定应针对一个**集成**做 -//! 鲁棒优化，而不是针对一条理想曲线做最优化。 - -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 deleted file mode 100644 index 7fcdbdb..0000000 --- a/crates/dji-esc-sim/src/harness.rs +++ /dev/null @@ -1,258 +0,0 @@ -//! 离线场景评估：同步、无 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 deleted file mode 100644 index 68147fe..0000000 --- a/crates/dji-esc-sim/src/lib.rs +++ /dev/null @@ -1,39 +0,0 @@ -//! 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 deleted file mode 100644 index e164d21..0000000 --- a/crates/dji-esc-sim/src/plant.rs +++ /dev/null @@ -1,361 +0,0 @@ -//! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。 -//! -//! 电调坐标系（正电流 → 正转速），逐拍离散（`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 deleted file mode 100644 index f81b5c9..0000000 --- a/crates/dji-esc-sim/src/rig.rs +++ /dev/null @@ -1,147 +0,0 @@ -//! 异步 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 deleted file mode 100644 index b9eaf67..0000000 --- a/crates/dji-esc-sim/src/scenario.rs +++ /dev/null @@ -1,236 +0,0 @@ -//! 场景定义：设定值曲线 + 采样节拍。 -//! -//! 场景与具体被控对象解耦：同一场景可在 [`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 deleted file mode 100644 index 8903429..0000000 --- a/crates/dji-esc-sim/src/trace.rs +++ /dev/null @@ -1,230 +0,0 @@ -//! 试验轨迹：一次场景重放的全部采样。 - -/// 一条完整试验轨迹（各列等长，索引即采样序号）。 -#[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-slcan/Cargo.toml b/crates/dji-esc-slcan/Cargo.toml deleted file mode 100644 index f3f4cc1..0000000 --- a/crates/dji-esc-slcan/Cargo.toml +++ /dev/null @@ -1,18 +0,0 @@ -[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-socketcan/Cargo.toml b/crates/dji-esc-socketcan/Cargo.toml deleted file mode 100644 index efad310..0000000 --- a/crates/dji-esc-socketcan/Cargo.toml +++ /dev/null @@ -1,25 +0,0 @@ -[package] -name = "dji-esc-socketcan" -version.workspace = true -edition.workspace = true -rust-version.workspace = true -license.workspace = true -description = "SocketCAN backend bridging real Linux CAN interfaces to can-link channels" - -[lints] -workspace = true - -[dependencies] -can-link = { workspace = true } -tokio = { workspace = true } -tracing = { workspace = true } - -[target.'cfg(target_os = "linux")'.dependencies] -socketcan = { workspace = true } - -[target.'cfg(target_os = "linux")'.dev-dependencies] -can-link = { workspace = true } -dji-esc = { workspace = true } -motor-core = { workspace = true } -socketcan = { workspace = true } -tokio = { workspace = true } diff --git a/crates/dji-esc-socketcan/src/bridge.rs b/crates/dji-esc-socketcan/src/bridge.rs deleted file mode 100644 index fa867d6..0000000 --- a/crates/dji-esc-socketcan/src/bridge.rs +++ /dev/null @@ -1,96 +0,0 @@ -//! 真实总线桥接实现（Linux）。 - -use std::io; -use std::sync::Arc; - -use can_link::{CanFrame as LinkFrame, CanLink}; -use socketcan::tokio::CanSocket; -use socketcan::{ - CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, - SocketOptions as _, -}; -use tokio::sync::mpsc; -use tracing::{debug, error}; - -/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。 -const CHANNEL_BOUND: usize = 512; - -/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。 -/// -/// 内部： -/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208， -/// 非反馈帧再由协议层解码拒绝，双保险）； -/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。 -/// -/// # Errors -/// -/// 接口不存在 / 权限不足等 `io::Error`。 -pub fn open_link(interface: &str) -> io::Result { - let socket = CanSocket::open(interface)?; - socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?; - let socket = Arc::new(socket); - - let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND); - let CanLink { outgoing_tx, incoming_rx } = bridge_side; - - tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx)); - tokio::spawn(tx_bridge(socket, incoming_rx)); - Ok(bus_side) -} - -/// socket → 通道：真实总线反馈进入协议 bus。 -async fn rx_bridge(socket: Arc, tx: mpsc::Sender) { - loop { - match socket.read_frame().await { - Ok(frame) => { - if let Some(frame) = to_link_frame(&frame) { - if tx.send(frame).await.is_err() { - debug!("bus side closed, rx bridge exiting"); - return; - } - } - } - Err(e) => { - // 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程， - // 由 systemd 拉起重连。 - error!(error = %e, "CAN read failed, rx bridge exiting"); - return; - } - } - } -} - -/// 通道 → socket：协议 bus 命令写入真实总线。 -async fn tx_bridge(socket: Arc, mut rx: mpsc::Receiver) { - while let Some(frame) = rx.recv().await { - match to_socketcan_frame(&frame) { - Some(sc_frame) => { - if let Err(e) = socket.write_frame(sc_frame).await { - error!(error = %e, "CAN write failed, tx bridge exiting"); - return; - } - } - None => { - debug!(id = frame.id, "drop frame with non-standard CAN id"); - } - } - } - debug!("bus side closed, tx bridge exiting"); -} - -/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。 -fn to_link_frame(frame: &ScFrame) -> Option { - match frame { - ScFrame::Data(df) if df.data().len() == 8 => { - Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?)) - } - _ => None, - } -} - -/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。 -fn to_socketcan_frame(frame: &LinkFrame) -> Option { - let id = socketcan::StandardId::new(frame.id as u16)?; - let df = CanDataFrame::new(id, &frame.data)?; - Some(ScFrame::Data(df)) -} diff --git a/crates/dji-esc-socketcan/src/lib.rs b/crates/dji-esc-socketcan/src/lib.rs deleted file mode 100644 index b4e262c..0000000 --- a/crates/dji-esc-socketcan/src/lib.rs +++ /dev/null @@ -1,11 +0,0 @@ -//! SocketCAN 桥接：把真实 Linux CAN 接口接入 [`can_link::CanLink`] 通道对。 -//! -//! 本 crate 主体仅在 Linux 下编译（依赖 socketcan）。其他平台编译为空壳， -//! 使整个 workspace 可以在任意开发机上构建与测试；实车部署目标板均为 -//! Linux。 - -#[cfg(target_os = "linux")] -mod bridge; - -#[cfg(target_os = "linux")] -pub use bridge::open_link; diff --git a/crates/dji-esc/src/codec.rs b/crates/dji-esc/src/codec.rs deleted file mode 100644 index c0ec91f..0000000 --- a/crates/dji-esc/src/codec.rs +++ /dev/null @@ -1,249 +0,0 @@ -//! DJI 电调（C620 / C610）CAN 协议编解码。 -//! -//! 帧格式（大端）： -//! -//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）： -//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令， -//! 槽位按组内电调顺序。 -//! - 反馈（`0x200 + 电调ID`）： -//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。 -//! -//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在 -//! [`crate::bus`] 中按节拍调用。 - -use crate::frame::{ - COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE, -}; -use thiserror::Error; - -/// 电调 ID 下限。 -pub const MIN_ESC_ID: u8 = 1; -/// 电调 ID 上限。 -pub const MAX_ESC_ID: u8 = 8; - -/// 电调型号决定电流标度。 -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum EscModel { - /// C620（配 GM3508）：±16384 ↔ ±20 A。 - C620, - /// C610（配 M2006）：±10000 ↔ ±10 A。 - /// - /// 接入前需实测核对。 - C610, -} - -impl EscModel { - /// 满量程电流命令（raw）。 - #[must_use] - pub const fn max_current_raw(self) -> i16 { - match self { - Self::C620 => 16384, - Self::C610 => 10_000, - } - } - - /// 满量程电流（安培）。 - #[must_use] - pub const fn max_current_amp(self) -> f32 { - match self { - Self::C620 => 20.0, - Self::C610 => 10.0, - } - } - - /// 安培 → raw（限幅）。 - #[must_use] - pub fn raw_from_amp(self, amp: f32) -> i16 { - let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp(); - (raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16) - .clamp(-self.max_current_raw(), self.max_current_raw()) - } - - /// raw → 安培。 - #[must_use] - pub fn amp_from_raw(self, raw: i16) -> f32 { - f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw()) - } -} - -/// 一帧电调反馈（原始、电调侧，未经方向修正）。 -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub struct EscFeedback { - /// 转子机械角原始值（0..8191，对应 0..360°）。 - pub angle_raw: u16, - /// 转子转速（rpm，电调侧符号）。 - pub speed_rpm: i16, - /// 转矩电流 raw（电调侧符号）。 - pub current_raw: i16, - /// 电机温度（℃）。 - pub temperature_c: i8, -} - -/// 帧解码错误。 -#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)] -pub enum DecodeError { - /// 帧不属于电调反馈范围（0x201..=0x208）。 - #[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")] - NotFeedback { - /// 收到的帧 ID。 - id: u32, - }, -} - -/// 电调 ID → 其电流命令所在的组控制帧 ID。 -/// -/// 返回 `None` 表示 ID 越界。 -#[must_use] -pub const fn command_frame_id(esc_id: u8) -> Option { - match esc_id { - 1..=4 => Some(COMMAND_FRAME_ID_PRIMARY), - 5..=8 => Some(COMMAND_FRAME_ID_SECONDARY), - _ => None, - } -} - -/// 将 raw 电流限幅到电调满量程。 -#[must_use] -pub const fn clamp_current(model: EscModel, raw: i16) -> i16 { - if raw > model.max_current_raw() { - model.max_current_raw() - } else if raw < -model.max_current_raw() { - -model.max_current_raw() - } else { - raw - } -} - -/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。 -/// -/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。 -/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成 -/// （见 [`clamp_current`]）。 -#[must_use] -pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame { - let mut data = [0u8; 8]; - for (slot, raw) in currents.iter().enumerate() { - data[slot * 2] = (raw >> 8) as u8; - data[slot * 2 + 1] = (raw & 0xFF) as u8; - } - CanFrame::new(frame_id, data) -} - -/// 组装一帧电调反馈（仿真器 / 测试用）。 -#[must_use] -pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame { - let mut data = [0u8; 8]; - data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes()); - data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes()); - data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes()); - data[6] = feedback.temperature_c as u8; - CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data) -} - -/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。 -/// -/// # Errors -/// -/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。 -pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> { - if frame.id <= FEEDBACK_FRAME_ID_BASE - || frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID) - { - return Err(DecodeError::NotFeedback { id: frame.id }); - } - let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8; - Ok(( - esc_id, - EscFeedback { - angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]), - speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]), - current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]), - temperature_c: frame.data[6] as i8, - }, - )) -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn command_frame_id_routing() { - assert_eq!(command_frame_id(1), Some(0x200)); - assert_eq!(command_frame_id(4), Some(0x200)); - assert_eq!(command_frame_id(5), Some(0x1FF)); - assert_eq!(command_frame_id(8), Some(0x1FF)); - assert_eq!(command_frame_id(0), None); - assert_eq!(command_frame_id(9), None); - } - - #[test] - fn encode_current_frame_bytes() { - let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]); - assert_eq!(frame.id, 0x200); - assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]); - - let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]); - assert_eq!(frame.id, 0x1FF); - assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]); - } - - #[test] - fn decode_feedback_known_bytes() { - let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]); - let (esc_id, fb) = decode_feedback(&frame).unwrap(); - assert_eq!(esc_id, 5); - assert_eq!(fb.angle_raw, 8191); - assert_eq!(fb.speed_rpm, -364); - assert_eq!(fb.current_raw, 123); - assert_eq!(fb.temperature_c, 45); - } - - #[test] - fn decode_feedback_rejects_non_feedback_ids() { - for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] { - let frame = CanFrame::new(id, [0; 8]); - assert_eq!( - decode_feedback(&frame), - Err(DecodeError::NotFeedback { id }), - "id = 0x{id:03X}" - ); - } - } - - #[test] - fn feedback_roundtrip_all_ids() { - let fb = EscFeedback { - angle_raw: 8191, - speed_rpm: -9254, - current_raw: -16384, - temperature_c: -40, - }; - for esc_id in MIN_ESC_ID..=MAX_ESC_ID { - let frame = encode_feedback_frame(esc_id, &fb); - assert_eq!(frame.id, 0x200 + u32::from(esc_id)); - assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb)); - } - } - - #[test] - fn amp_conversion() { - let c620 = EscModel::C620; - assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4); - assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4); - assert_eq!(c620.raw_from_amp(20.0), 16384); - assert_eq!(c620.raw_from_amp(-5.0), -4096); - // 超量程限幅。 - assert_eq!(c620.raw_from_amp(99.0), 16384); - let c610 = EscModel::C610; - assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4); - assert_eq!(c610.raw_from_amp(10.0), 10000); - } - - #[test] - fn clamp_current_respects_model() { - assert_eq!(clamp_current(EscModel::C620, 20000), 16384); - assert_eq!(clamp_current(EscModel::C610, 15000), 10000); - assert_eq!(clamp_current(EscModel::C620, 1000), 1000); - } -} diff --git a/crates/dji-esc/src/frame.rs b/crates/dji-esc/src/frame.rs deleted file mode 100644 index 41cd0f6..0000000 --- a/crates/dji-esc/src/frame.rs +++ /dev/null @@ -1,13 +0,0 @@ -//! DJI 电调协议帧 ID 常量。 -//! -//! 帧与链路类型（[`CanFrame`] / [`CanLink`]）由 [`can_link`] 提供， -//! 本模块只保留 **C620/C610 电流协议族**的 ID 约定。 - -pub use can_link::{CanFrame, CanLink}; - -/// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。 -pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200; -/// 控制帧 ID：电调 5–8 的电流命令。 -pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF; -/// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。 -pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200; diff --git a/crates/dji-esc/src/lib.rs b/crates/dji-esc/src/lib.rs deleted file mode 100644 index 6960c73..0000000 --- a/crates/dji-esc/src/lib.rs +++ /dev/null @@ -1,29 +0,0 @@ -//! DJI RoboMaster 电调（C620/C610）CAN 协议与异步驱动。 -//! -//! 分层： -//! -//! - [`codec`]：纯函数帧编解码（C620 手册协议）； -//! - [`turn`]：单圈编码器 → 连续多圈角； -//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）； -//! - [`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 turn; - -pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor}; -pub use can_link::{CanFrame, CanLink}; -pub use codec::{ - DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id, - 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 turn::{ENCODER_TICKS, TurnTracker}; diff --git a/crates/dji-esc/src/turn.rs b/crates/dji-esc/src/turn.rs deleted file mode 100644 index 78faadd..0000000 --- a/crates/dji-esc/src/turn.rs +++ /dev/null @@ -1,134 +0,0 @@ -//! 多圈累计：单圈编码器原始角 → 连续圈数。 - -/// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。 -pub const ENCODER_TICKS: u16 = 8192; - -/// 半圈阈值：帧间角变化超过它视为发生回绕。 -const HALF_TURN_TICKS: i32 = 4096; - -/// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。 -/// -/// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约 -/// 9300 rpm ≈ 0.155 圈/ms，余量充足。 -#[derive(Debug, Clone, Default)] -pub struct TurnTracker { - revolutions: i32, - prev_raw: Option, - last_angle_rev: f32, -} - -impl TurnTracker { - /// 创建跟踪器。 - #[must_use] - pub const fn new() -> Self { - Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 } - } - - /// 送入新的原始角，返回连续累计角（圈）。 - pub fn update(&mut self, raw: u16) -> f32 { - if let Some(prev) = self.prev_raw { - let delta = i32::from(raw) - i32::from(prev); - if delta > HALF_TURN_TICKS { - // 正向跨越 8191 → 0。 - self.revolutions -= 1; - } else if delta < -HALF_TURN_TICKS { - // 反向跨越 0 → 8191。 - self.revolutions += 1; - } - } - self.prev_raw = Some(raw); - self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0; - self.last_angle_rev - } - - /// 最近一次 [`TurnTracker::update`] 的返回值。 - #[must_use] - pub const fn last_angle_rev(&self) -> f32 { - self.last_angle_rev - } - - /// 清零回到初始状态。 - pub fn reset(&mut self) { - *self = Self::new(); - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn first_reading_is_raw_fraction() { - let mut t = TurnTracker::new(); - assert!((t.update(4096) - 0.5).abs() < 1e-4); - } - - #[test] - fn forward_wrap_counts_up() { - let mut t = TurnTracker::new(); - t.update(8000); - let after = t.update(100); // 8000 → 8191 → 0 → 100 - assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}"); - } - - #[test] - fn reverse_wrap_counts_down() { - let mut t = TurnTracker::new(); - t.update(100); - let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向） - assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}"); - } - - #[test] - fn small_deltas_do_not_wrap() { - let mut t = TurnTracker::new(); - t.update(0); - assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4); - assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4); - } - - #[test] - fn multi_turn_forward_then_reverse_returns() { - let mut t = TurnTracker::new(); - // 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。 - for _ in 0..3 { - t.update(0); - t.update(4000); - t.update(8000); - t.update(100); - } - let fwd = t.last_angle_rev(); - let expected_fwd = 3.0 + 100.0 / 8192.0; - assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}"); - - // 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。 - for _ in 0..3 { - t.update(8000); - t.update(4000); - t.update(0); - } - let back = t.last_angle_rev(); - assert!(back.abs() < 1e-4, "should return to 0, got {back}"); - } - - #[test] - fn jitter_at_boundary_stays_put() { - // 0 附近抖动不产生虚假圈数。 - let mut t = TurnTracker::new(); - t.update(8191); - assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次 - assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4); - assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回 - assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4); - } - - #[test] - fn reset_forgets_history() { - let mut t = TurnTracker::new(); - t.update(8000); - t.update(100); - t.reset(); - assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits()); - assert!((t.update(4096) - 0.5).abs() < 1e-4); - } -} diff --git a/crates/esc-bus/Cargo.toml b/crates/esc-bus/Cargo.toml new file mode 100644 index 0000000..6529726 --- /dev/null +++ b/crates/esc-bus/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "esc-bus" +description = "Asynchronous ESC frame pump, state, leases, and safety" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +motor = { workspace = true } +motor-protocol = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } + +[dev-dependencies] +sim-harness = { workspace = true } +motor-control = { workspace = true } +motor-model = { workspace = true } +experiment = { workspace = true } +tokio = { workspace = true, features = ["test-util"] } +approx = { workspace = true } diff --git a/crates/dji-esc/src/bus.rs b/crates/esc-bus/src/lib.rs similarity index 71% rename from crates/dji-esc/src/bus.rs rename to crates/esc-bus/src/lib.rs index a44165d..1110a2b 100644 --- a/crates/dji-esc/src/bus.rs +++ b/crates/esc-bus/src/lib.rs @@ -1,3 +1,5 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + //! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。 //! //! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务： @@ -12,21 +14,21 @@ use std::sync::{Arc, Mutex, MutexGuard}; use std::time::Duration; -use motor_core::{ - Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController, - PositionDrive, VelocityController, VelocityDrive, VelocityGains, +use can::{CanFrame, CanLink}; +use motor_protocol::common::{ + EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame, }; +use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker}; use thiserror::Error; use tokio::sync::{mpsc, watch}; use tokio::task::JoinHandle; use tokio::time::{Instant, Interval, MissedTickBehavior}; use tracing::{debug, info, warn}; -use crate::codec::{ - EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame, +use motor::{ + Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput, + PolicySample, PositionDrive, VelocityDrive, }; -use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink}; -use crate::turn::TurnTracker; /// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌， /// 状态本身仍一致，驱动机不应因此停摆）。 @@ -45,7 +47,7 @@ pub struct EscConfig { pub model: EscModel, /// 减速比（电机转子 : 输出轴）。 /// - /// 驱动层**不使用**此字段；由上层 [`motor_core::OutputShaft`] 应用。 + /// 驱动层**不使用**此字段；由上层 `crate::OutputShaft` 应用。 /// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。 pub gear_ratio: f32, /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转， @@ -53,14 +55,6 @@ pub struct EscConfig { pub direction: f32, /// effort 限幅（raw，`1..=型号满量程`）。 pub current_limit: i16, - /// 速度环比例增益（raw / rpm）。 - pub kp: f32, - /// 速度环积分增益（raw / (rpm·s)）。 - pub ki: f32, - /// 速度环微分增益。 - pub kd: f32, - /// 速度前馈（raw / rpm）。 - pub feedforward: f32, } impl EscConfig { @@ -75,10 +69,6 @@ impl EscConfig { gear_ratio: 3591.0 / 187.0, direction, current_limit: 12_000, - kp: 10.0, - ki: 0.5, - kd: 0.0, - feedforward: 1.5, } } @@ -95,19 +85,9 @@ impl EscConfig { gear_ratio: 36.0, direction, current_limit: 8_000, - kp: 8.0, - ki: 0.4, - kd: 0.0, - feedforward: 1.0, } } - /// 速度环增益组（可整定参数）。 - #[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)); @@ -187,9 +167,6 @@ struct MotorShared { cmd: Mutex, /// 命令写入时刻（看门狗用）。 cmd_at: Mutex>, - last_mode: Mutex, - vel_ctrl: Mutex, - pos_ctrl: Mutex, turn: Mutex, last_feedback: Mutex>, offline_warned: std::sync::atomic::AtomicBool, @@ -197,17 +174,11 @@ struct MotorShared { impl MotorShared { fn new(cfg: EscConfig) -> Self { - let limit = cfg.current_limit; - let pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit)); - let pos_pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit)); Self { - vel_ctrl: Mutex::new(VelocityController::new(pid, cfg.feedforward)), - pos_ctrl: Mutex::new(PositionController::direct(pos_pid)), 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), offline_warned: std::sync::atomic::AtomicBool::new(false), @@ -219,21 +190,6 @@ impl MotorShared { *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); @@ -251,68 +207,26 @@ impl MotorShared { self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed); } - /// 复位主机侧控制器（离线或模式切换时）。 - fn reset_controllers(&self) { - lock(&self.vel_ctrl).reset(); - lock(&self.pos_ctrl).reset(); - } - - /// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。 - fn compute_effort( + /// Produce one policy sample while enforcing feedback and command leases. + fn sample( &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) { - warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current"); - } - // 复位控制器，避免重新上线时旧积分冲击。 - self.reset_controllers(); - *lock(&self.last_mode) = CommandMode::Disabled; - return 0; + ) -> PolicySample { + let online = matches!( + *lock(&self.last_feedback), + Some(t) if now.saturating_duration_since(t) <= feedback_timeout + ); + if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) { + warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current"); } - // 命令看门狗：上层任务死亡/卡死后旧指令不得持续输出。 - 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); - // 模式切换复位：避免积分/微分记忆冲击。 - { - let mut last_mode = lock(&self.last_mode); - if *last_mode != cmd.mode() { - debug!(motor = %self.cfg.name, from = ?*last_mode, to = ?cmd.mode(), "command mode switch, resetting controllers"); - self.reset_controllers(); - *last_mode = cmd.mode(); - } - } - let dir = self.cfg.direction; - let raw = match cmd { - Command::Disabled => 0, - Command::EffortRaw(raw) => (f32::from(raw) * dir) as i16, - Command::Velocity(target) => { - let measured = lock(&self.state).rotor_speed_rpm; - let out = lock(&self.vel_ctrl).update(target, measured, dt); - (out * dir) as i16 - } - Command::Position(target) => { - let (measured_rev, measured_rpm) = { - let st = lock(&self.state); - (st.rotor_revolutions, st.rotor_speed_rpm) - }; - let out = lock(&self.pos_ctrl).update(target, measured_rev, measured_rpm, dt); - (out * dir) as i16 - } - }; - let limit = self.cfg.current_limit; - clamp_current(self.cfg.model, raw.clamp(-limit, limit)) + let fresh = matches!( + *lock(&self.cmd_at), + Some(t) if now.saturating_duration_since(t) <= command_timeout + ); + let command = if online && fresh { *lock(&self.cmd) } else { Command::Disabled }; + PolicySample { id: self.cfg.esc_id, state: *lock(&self.state), command, online } } } @@ -353,25 +267,6 @@ 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 { @@ -426,11 +321,12 @@ impl EscBus { /// # Errors /// /// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。 - pub fn spawn( + pub fn spawn( link: CanLink, configs: Vec, options: BusOptions, - ) -> Result { + policy: P, + ) -> Result, EscBusError> { if configs.is_empty() { return Err(EscBusError::NoMotors); } @@ -448,23 +344,30 @@ impl EscBus { let (shutdown_tx, shutdown_rx) = watch::channel(false); let CanLink { outgoing_tx, incoming_rx } = link; let motors = Arc::new(motors); + let policy = Arc::new(Mutex::new(policy)); let rx_handle = tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone())); - let tx_handle = - tokio::spawn(tx_loop(outgoing_tx, Arc::clone(&motors), options, shutdown_rx)); + let tx_handle = tokio::spawn(tx_loop( + outgoing_tx, + Arc::clone(&motors), + Arc::clone(&policy), + options, + shutdown_rx, + )); - Ok(EscBusHandle { motors, shutdown_tx, tasks: vec![rx_handle, tx_handle] }) + Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] }) } } /// `EscBus` 的控制句柄。 -pub struct EscBusHandle { +pub struct EscBusHandle

{ motors: Arc>, + policy: Arc>, shutdown_tx: watch::Sender, tasks: Vec>, } -impl EscBusHandle { +impl EscBusHandle

{ /// 按电调 ID 取电机句柄。 #[must_use] pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> { @@ -477,6 +380,11 @@ impl EscBusHandle { &self.motors } + /// Mutate the composed control policy between bus ticks. + pub fn with_policy(&self, update: impl FnOnce(&mut P) -> R) -> R { + update(&mut lock(&self.policy)) + } + /// 停止收发任务并等待退出。 pub async fn shutdown(self) { let _ = self.shutdown_tx.send(true); @@ -512,15 +420,15 @@ async fn rx_loop( } } -async fn tx_loop( +async fn tx_loop( outgoing: mpsc::Sender, motors: Arc>, + policy: Arc>, options: BusOptions, mut shutdown: watch::Receiver, ) { 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! { @@ -530,8 +438,21 @@ async fn tx_loop( 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); + let mut samples: [PolicySample; 8] = std::array::from_fn(|index| PolicySample { + id: u8::try_from(index + 1).unwrap_or(0), + state: MotorState::default(), + command: Command::Disabled, + online: false, + }); + for motor in motors.iter() { + let sample = + motor.shared.sample(options.feedback_timeout, options.command_timeout, now); + if let Some(slot) = samples.get_mut(usize::from(sample.id.saturating_sub(1))) { + *slot = sample; + } + } + let efforts = lock(&policy).step(&PolicyInput { samples, dt }); for (frame_id, group_start) in [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)] { @@ -545,11 +466,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, - options.command_timeout, - now, + let policy_index = usize::from(motor.id().saturating_sub(1)); + let physical = efforts.get(policy_index).copied().unwrap_or(0); + let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16; + currents[slot] = clamp_current( + motor.shared.cfg.model, + directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit), ); } if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() { diff --git a/crates/dji-esc-sim/tests/closed_loop.rs b/crates/esc-bus/tests/closed_loop.rs similarity index 85% rename from crates/dji-esc-sim/tests/closed_loop.rs rename to crates/esc-bus/tests/closed_loop.rs index d690f92..f00f08a 100644 --- a/crates/dji-esc-sim/tests/closed_loop.rs +++ b/crates/esc-bus/tests/closed_loop.rs @@ -6,24 +6,35 @@ use std::time::Duration; -use dji_esc::{ - BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback, - encode_feedback_frame, -}; -use dji_esc_sim::spawn_plant; -use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive, VelocityGains}; +use can::{CanFrame, CanLink}; +use esc_bus::{BusOptions, EscBus, EscBusError, EscConfig}; +use motor::{Disable, EffortDrive, MotorFeedback, VelocityDrive}; +use motor_control::{MotorPolicy, VelocityGains}; +use motor_protocol::{EscFeedback, encode_feedback_frame}; +use sim_harness::spawn_plant; use tokio::sync::mpsc; use tokio::task::JoinHandle; use tokio::time::{MissedTickBehavior, interval, sleep, timeout}; /// 与 `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; - cfg.ki = 2.0; - cfg.kd = 0.0; - cfg.feedforward = 16384.0 / 9254.0; - cfg + EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction) +} + +fn tuned_gains() -> VelocityGains { + VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0) +} + +fn spawn_bus( + link: CanLink, + configs: Vec, + options: BusOptions, +) -> Result, EscBusError> { + let mut policy = MotorPolicy::new(); + for config in &configs { + policy.configure(config.esc_id, config.current_limit, tuned_gains()); + } + EscBus::spawn(link, configs, options, policy) } /// 轮询直到条件满足或超时（毫秒，仿真时间）。 @@ -93,7 +104,7 @@ fn slot_current(frame: &CanFrame, slot: usize) -> i16 { async fn velocity_loop_converges() { let (bus_link, plant_link) = CanLink::pair(512); let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1)); - let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); let motor = bus.motor(1).unwrap().clone(); motor.set_velocity(3000.0); @@ -121,7 +132,7 @@ async fn reversed_motor_converges_in_physical_frame() { // direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。 let (bus_link, plant_link) = CanLink::pair(512); let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1)); - let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); let motor = bus.motor(2).unwrap().clone(); motor.set_velocity(2000.0); @@ -143,7 +154,7 @@ async fn reversed_motor_converges_in_physical_frame() { async fn motor_without_feedback_commands_zero_current() { let (bus_link, mut peer) = CanLink::pair(64); // 不接 plant → 永远没有反馈。 - let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); bus.motor(1).unwrap().set_velocity(5000.0); for _ in 0..5 { @@ -159,7 +170,7 @@ async fn current_limit_clamps_saturation() { let (bus_link, mut peer) = CanLink::pair(64); let mut cfg = tuned(1, 1.0); cfg.current_limit = 5000; - let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![cfg], BusOptions::default()).unwrap(); send_feedback(&peer, &[1]).await; bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和 @@ -203,7 +214,7 @@ fn spawn_feedback(yield_tx: mpsc::Sender, esc_ids: Vec) -> JoinHan 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 bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); let motor = bus.motor(1).unwrap().clone(); // 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。 @@ -248,7 +259,7 @@ async fn command_lease_expires_and_renews() { async fn current_commands_route_to_group_slots() { let (bus_link, mut peer) = CanLink::pair(256); let configs = (1..=5).map(|id| tuned(id, 1.0)).collect(); - let bus = EscBus::spawn(bus_link, configs, BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, configs, BusOptions::default()).unwrap(); bus.motor(3).unwrap().set_effort_raw(1234); bus.motor(5).unwrap().set_effort_raw(-567); @@ -306,7 +317,7 @@ fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool #[tokio::test] async fn feedback_updates_state_with_direction() { let (bus_link, peer) = CanLink::pair(64); - let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); // 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。 let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 }; @@ -337,7 +348,7 @@ async fn feedback_updates_state_with_direction() { async fn shutdown_stops_tasks_and_closes_link() { let (bus_link, mut peer) = CanLink::pair(128); // 不接 plant，直接验证任务生命周期。 - let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); bus.motor(1).unwrap().set_velocity(1000.0); sleep(Duration::from_millis(20)).await; // 有命令帧在流动。 @@ -358,17 +369,14 @@ async fn shutdown_stops_tasks_and_closes_link() { #[test] fn spawn_rejects_bad_configs() { let link = || CanLink::pair(8).0; + assert_eq!(spawn_bus(link(), vec![], BusOptions::default()).err(), Some(EscBusError::NoMotors)); assert_eq!( - EscBus::spawn(link(), vec![], BusOptions::default()).err(), - Some(EscBusError::NoMotors) - ); - assert_eq!( - EscBus::spawn(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(), + spawn_bus(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(), Some(EscBusError::DuplicateEscId(2)) ); let mut bad = tuned(1, 1.0); bad.direction = 0.0; - let err = EscBus::spawn(link(), vec![bad], BusOptions::default()).err(); + let err = spawn_bus(link(), vec![bad], BusOptions::default()).err(); assert!( matches!(err, Some(EscBusError::InvalidConfig { .. })), "expected InvalidConfig, got {err:?}" @@ -386,22 +394,22 @@ async fn velocity_gains_can_be_hot_swapped() { 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 bus = spawn_bus(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap(); let motor = bus.motor(1).unwrap().clone(); // ① 初始增益来自 EscConfig（含前馈）。 - assert_eq!(motor.velocity_gains(), cfg.velocity_gains()); + assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), tuned_gains()); // 让旧增益的积分先积累起来（大静差 + 深度饱和）。 motor.set_velocity(9000.0); hold(&motor, 9000.0, 200).await; - let stale = motor.velocity_gains(); + let stale = bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()); // ② 热更新：控制器被重建。 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); + bus.with_policy(|policy| policy.set_velocity_gains(motor.id(), swapped)); + assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), swapped); + assert_ne!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), stale); // 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。 motor.set_velocity(30_000.0); @@ -412,7 +420,11 @@ async fn velocity_gains_can_be_hot_swapped() { // ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。 let effort = i32::from(motor.state().effort_raw); assert_eq!(effort, limit, "饱和值必须正好是 current_limit"); - assert_eq!(motor.velocity_gains(), swapped, "热更新后增益保持不变"); + assert_eq!( + bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), + swapped, + "热更新后增益保持不变" + ); plant.abort(); bus.shutdown().await; diff --git a/crates/dji-esc-sim/tests/harness_parity.rs b/crates/esc-bus/tests/harness_parity.rs similarity index 87% rename from crates/dji-esc-sim/tests/harness_parity.rs rename to crates/esc-bus/tests/harness_parity.rs index 803e483..a245c70 100644 --- a/crates/dji-esc-sim/tests/harness_parity.rs +++ b/crates/esc-bus/tests/harness_parity.rs @@ -1,6 +1,6 @@ //! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。 //! -//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是 +//! 自动整定跑在 [`sim_harness::simulate`]（同步、无通道）上，而实车跑的是 //! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移， //! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与 //! 同一条场景下比较稳态与 effort。 @@ -12,9 +12,13 @@ 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 can::CanLink; +use esc_bus::{BusOptions, EscBus, EscConfig}; +use experiment::Scenario; +use motor::{MotorFeedback, VelocityDrive}; +use motor_control::{MotorPolicy, VelocityGains}; +use motor_model::PlantParams; +use sim_harness::{LoopLimits, simulate, spawn_plant}; use tokio::time::sleep; /// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。 @@ -40,12 +44,11 @@ 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 mut policy = MotorPolicy::new(); + policy.configure(cfg.esc_id, cfg.current_limit, GAINS); + let bus = + EscBus::spawn(bus_link, vec![cfg], BusOptions::default(), policy).expect("bus must start"); let motor = bus.motor(1).expect("motor 1 exists").clone(); // 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。 diff --git a/crates/experiment/Cargo.toml b/crates/experiment/Cargo.toml new file mode 100644 index 0000000..4e1eecc --- /dev/null +++ b/crates/experiment/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "experiment" +description = "Shared experiment scenarios, traces, windows, and metrics" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] + + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/experiment/src/lib.rs b/crates/experiment/src/lib.rs new file mode 100644 index 0000000..aa9c88c --- /dev/null +++ b/crates/experiment/src/lib.rs @@ -0,0 +1,747 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod scenario { + //! 场景定义：设定值曲线 + 采样节拍。 + //! + //! 场景与具体被控对象解耦：同一场景可在 [`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); + } + } +} + +pub use scenario::*; + +pub mod trace { + //! 试验轨迹：一次场景重放的全部采样。 + + /// 一条完整试验轨迹（各列等长，索引即采样序号）。 + #[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); + } + } +} + +pub use trace::*; + +pub mod metrics { + //! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。 + //! + //! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能 + //! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。 + + use crate::{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()); + } + } +} + +pub use metrics::*; diff --git a/crates/can-link/Cargo.toml b/crates/imu/Cargo.toml similarity index 56% rename from crates/can-link/Cargo.toml rename to crates/imu/Cargo.toml index 9c43737..96e2469 100644 --- a/crates/can-link/Cargo.toml +++ b/crates/imu/Cargo.toml @@ -1,13 +1,13 @@ [package] -name = "can-link" -description = "CAN 传输抽象：帧与链路通道（无协议知识）" +name = "imu" +description = "IMU data contract" version.workspace = true edition.workspace = true rust-version.workspace = true license.workspace = true -[dependencies] -tokio = { workspace = true } - [lints] workspace = true + +[dependencies] + diff --git a/crates/imu/src/lib.rs b/crates/imu/src/lib.rs new file mode 100644 index 0000000..4ab52e6 --- /dev/null +++ b/crates/imu/src/lib.rs @@ -0,0 +1,131 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! IMU 抽象：姿态数据与读取接口（ARCHITECTURE.md §5.1）。 +//! +//! `driver crates::delta10` 为 DETA10 实现 [`Imu`]；测试可用 +//! `motor::testing` 风格的替身。 +//! +//! # 单位与坐标系 +//! +//! 与 ARCHITECTURE.md §5.1 的 [`ImuData`] 一致： +//! +//! | 字段 | 单位 | 坐标系 | +//! | --- | --- | --- | +//! | `accel` | m/s² | 机体系（x 前、y 左、z 上） | +//! | `gyro` | rad/s | 机体系 | +//! | `mag` | µT | 机体系 | +//! | `roll` / `pitch` / `yaw` | 度 | 机体 → 世界（yaw 逆时针为正） | +//! +//! 时间戳为单调时钟纳秒（见 [`crate::time`]），用于和电机反馈对齐。 + +/// 一帧 IMU 数据。 +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct ImuData { + /// 加速度（m/s²，机体系 x/y/z）。 + pub accel: [f32; 3], + /// 角速度（rad/s，机体系 x/y/z）。 + pub gyro: [f32; 3], + /// 磁力计（µT，机体系 x/y/z）；无磁力计时为 `[0.0; 3]`。 + pub mag: [f32; 3], + /// 横滚角（度）。 + pub roll: f32, + /// 俯仰角（度）。 + pub pitch: f32, + /// 偏航角（度）。 + pub yaw: f32, + /// 单调时钟时间戳（纳秒）。 + pub timestamp_ns: u64, +} + +impl ImuData { + /// 所有字段是否有限（NaN / inf 不得进入控制律）。 + #[must_use] + pub fn is_finite(&self) -> bool { + self.accel.iter().chain(&self.gyro).chain(&self.mag).all(|v| v.is_finite()) + && self.roll.is_finite() + && self.pitch.is_finite() + && self.yaw.is_finite() + } +} + +/// IMU 读取错误。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum ImuError { + /// 链路关闭。 + Closed, + /// 帧 ID / 长度不符（多半是总线接错或协议模式不对）。 + BadFrame, + /// **帧载荷布局尚未确认**（ARCHITECTURE.md 决策点 D1）。 + /// + /// 见 `delta10-protocol`：DETA10 手册链接已失效，载荷偏移在 + /// 拿到数据手册前不得臆造，因此解码器会显式返回本错误。 + LayoutUnconfirmed, + /// 校验和 / 校验失败。 + Checksum, + /// 底层 IO 失败。 + Io, +} + +impl core::fmt::Display for ImuError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::Closed => write!(f, "IMU 链路已关闭"), + Self::BadFrame => write!(f, "IMU 帧 ID 或长度不符"), + Self::LayoutUnconfirmed => { + write!(f, "IMU 载荷布局未确认（ARCHITECTURE.md 决策点 D1：需 DETA10 数据手册）") + } + Self::Checksum => write!(f, "IMU 校验失败"), + Self::Io => write!(f, "IMU 底层 IO 失败"), + } + } +} + +impl core::error::Error for ImuError {} + +/// 硬件无关的 IMU 边界。 +pub trait Imu { + /// 底层错误类型。 + type Error: core::error::Error + Send + Sync + 'static; + + /// 读取一帧姿态数据（阻塞直到有帧或链路关闭）。 + /// + /// # Errors + /// + /// 链路关闭、帧不合法、载荷布局未确认时返回错误。 + fn read(&mut self) -> impl core::future::Future>; + + /// 非阻塞读取：本轮没有新帧时返回 `Ok(None)`。 + /// + /// # Errors + /// + /// 仅在链路确定损坏时返回错误。 + fn try_read(&mut self) -> Result, Self::Error> { + Ok(None) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn default_data_is_zero_and_finite() { + let data = ImuData::default(); + assert!((data.yaw).abs() < 1e-6); + assert!(data.is_finite()); + } + + #[test] + fn non_finite_attitude_is_detected() { + let data = ImuData { pitch: f32::NAN, ..ImuData::default() }; + assert!(!data.is_finite()); + let data = ImuData { gyro: [0.0, f32::INFINITY, 0.0], ..ImuData::default() }; + assert!(!data.is_finite()); + } + + #[test] + fn layout_unconfirmed_error_names_the_decision_point() { + let text = ImuError::LayoutUnconfirmed.to_string(); + assert!(text.contains("D1"), "{text}"); + } +} diff --git a/crates/mecanum/Cargo.toml b/crates/mecanum/Cargo.toml index fbcc7d0..ee44335 100644 --- a/crates/mecanum/Cargo.toml +++ b/crates/mecanum/Cargo.toml @@ -1,16 +1,17 @@ [package] name = "mecanum" +description = "Mecanum chassis kinematics" version.workspace = true edition.workspace = true rust-version.workspace = true license.workspace = true -description = "Mecanum wheel chassis forward/inverse kinematics" [lints] workspace = true [dependencies] + [dev-dependencies] proptest = { workspace = true } approx = { workspace = true } diff --git a/crates/mecanum/proptest-regressions/lib.txt b/crates/mecanum/proptest-regressions/lib.txt deleted file mode 100644 index 993a75c..0000000 --- a/crates/mecanum/proptest-regressions/lib.txt +++ /dev/null @@ -1,7 +0,0 @@ -# Seeds for failure cases proptest has generated in the past. It is -# automatically read and these particular cases re-run before any -# novel cases are generated. -# -# It is recommended to check this file in to source control so that -# everyone who runs the test benefits from these saved cases. -cc 698ba63bf948a3702145ece17ed0f014133de0d0b3797fd8bdda514c5d2480a7 # shrinks to vx = 0.0, vy = 0.0, omega = -2.3901656 diff --git a/crates/mecanum/src/lib.rs b/crates/mecanum/src/lib.rs index b5f4614..3b5857a 100644 --- a/crates/mecanum/src/lib.rs +++ b/crates/mecanum/src/lib.rs @@ -1,3 +1,5 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + //! 麦克纳姆轮底盘正 / 逆运动学。 //! //! # 约定 diff --git a/crates/motor-control/Cargo.toml b/crates/motor-control/Cargo.toml new file mode 100644 index 0000000..8023e1d --- /dev/null +++ b/crates/motor-control/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "motor-control" +description = "Motor velocity, position, cascade, and feedforward control" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +pid = { workspace = true } +motor = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/motor-control/src/lib.rs b/crates/motor-control/src/lib.rs new file mode 100644 index 0000000..49f81b1 --- /dev/null +++ b/crates/motor-control/src/lib.rs @@ -0,0 +1,798 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod feedforward { + //! 前馈补偿（ARCHITECTURE.md §5.5）。 + //! + //! 速度环的前馈是**比例前馈**：稳态下所需控制量与目标转速近似成正比， + //! 因此 `u_ff = k_ff · setpoint` 可以直接把积分项从"慢慢顶"变成"一开始就到位"。 + //! + //! # 为什么前馈在空载台架上必须为 0 + //! + //! `k_ff ≈ 1/K`，其中 `K` 是被控对象的稳态增益（GM3508 空载实测 + //! `K ≈ 0.57 rpm/raw` → `k_ff ≈ 1.77`）。空载时维持转速只需要克服摩擦的极小 + //! 电流，任何接近 1.77 的前馈都会把轮子直接推到最高转速（见 `docs/chassis.md` §7.2）。 + + /// 比例前馈：`输出 = k_ff · setpoint`。 + #[derive(Debug, Clone, Copy, PartialEq, Default)] + pub struct ProportionalFeedforward { + /// 前馈系数（控制量 / 目标量）。 + pub k_ff: f32, + } + + impl ProportionalFeedforward { + /// 构造。 + #[must_use] + pub const fn new(k_ff: f32) -> Self { + Self { k_ff } + } + + /// 由被控对象稳态增益换算：`k_ff = 1 / K`。 + /// + /// `K` 非有限或 ≤ 0 时返回 0（宁可不前馈，也不要一个发散的系数）。 + #[must_use] + pub fn from_plant_gain(plant_gain: f32) -> Self { + if plant_gain.is_finite() && plant_gain > 0.0 { + Self { k_ff: 1.0 / plant_gain } + } else { + Self { k_ff: 0.0 } + } + } + + /// 计算前馈量。 + #[must_use] + pub fn compute(&self, setpoint: f32) -> f32 { + if setpoint.is_finite() { self.k_ff * setpoint } else { 0.0 } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn inverse_plant_gain_matches_the_documented_bench_feedforward() { + // GM3508 空载 K ≈ 0.5648 rpm/raw → k_ff ≈ 1.77 raw/rpm + let feedforward = ProportionalFeedforward::from_plant_gain(9254.0 / 16384.0); + assert!((feedforward.k_ff - 1.77).abs() < 0.02, "{}", feedforward.k_ff); + } + + #[test] + fn degenerate_plant_gain_yields_no_feedforward() { + assert!((ProportionalFeedforward::from_plant_gain(0.0).k_ff).abs() < 1e-6); + assert!((ProportionalFeedforward::from_plant_gain(-1.0).k_ff).abs() < 1e-6); + assert!((ProportionalFeedforward::from_plant_gain(f32::NAN).k_ff).abs() < 1e-6); + } + + #[test] + fn compute_is_linear_and_safe() { + let feedforward = ProportionalFeedforward::new(2.0); + assert!((feedforward.compute(3.0) - 6.0).abs() < 1e-6); + assert!((feedforward.compute(f32::INFINITY)).abs() < 1e-6); + assert!((ProportionalFeedforward::default().compute(100.0)).abs() < 1e-6); + } + } +} + +pub use feedforward::*; + +pub mod plant { + //! 被控对象抽象：让**仿真与实机共用同一套 PID**（ARCHITECTURE.md §5.5）。 + //! + //! 这是整定能否迁移到实机的关键 seam：整定器只认识 [`ControlPlant`]， + //! 因此"在仿真里搜出来的增益"与"在台架上验出来的增益"评估的是同一条接口。 + //! + //! 实现方： + //! - 仿真：`sim-harness::SimPlant`（浮点积分，无 IO，可并行上千次）； + //! - 实机/台架：`bench` 的台架采样（每次试验 = 真机跑一条场景）。 + + /// 单输入单输出的被控对象。 + /// + /// 约定：`step` 推进 `dt` 秒并返回**本次的测量输出**；输入是控制量 + /// （`motor-control` 里即 effort raw）。实现必须保证 `dt <= 0` 时不推进状态。 + pub trait ControlPlant { + /// 推进一拍的误差类型（平台相关实现可以自定义）。 + type Error; + + /// 推进一步，返回测量输出。 + /// + /// # Errors + /// + /// 数值发散或输入非法时返回错误，由调用方折算成硬约束罚值。 + fn step(&mut self, input: f32, dt: f32) -> Result; + + /// 复位到初始状态（两次试验之间必须调用，否则结果依赖历史）。 + fn reset(&mut self); + + /// 人类可读描述（写进报告，便于复现实验条件）。 + fn describe(&self) -> String; + } + + /// 无错误的被控对象（纯代数模型 / 已在内部处理发散的实现）用它的适配器。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] + pub struct Infallible; + + impl core::fmt::Display for Infallible { + fn fmt(&self, _f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + Ok(()) + } + } + + impl core::error::Error for Infallible {} + + /// 一阶惯性环节：`y' = (k·u − y) / tau`，显式欧拉。 + /// + /// 这是最小可用的 [`ControlPlant`] 实现，用来给控制律 / 整定器做单元测试； + /// 带摩擦与延迟的完整电机模型在 `sim-harness`。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct FirstOrderPlant { + /// 稳态增益。 + pub gain: f32, + /// 时间常数（秒）。 + pub tau_s: f32, + /// 当前输出。 + pub output: f32, + } + + impl FirstOrderPlant { + /// 构造（`tau_s` 必须为正，否则退化为直通）。 + #[must_use] + pub fn new(gain: f32, tau_s: f32) -> Self { + Self { gain, tau_s: if tau_s > 0.0 { tau_s } else { f32::EPSILON }, output: 0.0 } + } + } + + impl ControlPlant for FirstOrderPlant { + type Error = Infallible; + + fn step(&mut self, input: f32, dt: f32) -> Result { + if dt > 0.0 && input.is_finite() { + let alpha = (dt / self.tau_s).clamp(0.0, 1.0); + let target = self.gain * input; + self.output += alpha * (target - self.output); + } + Ok(self.output) + } + + fn reset(&mut self) { + self.output = 0.0; + } + + fn describe(&self) -> String { + format!("first-order(gain={}, tau={}s)", self.gain, self.tau_s) + } + } + + /// 标准二阶系统：`G(s) = gain·wn² / (s² + 2ζ·wn·s + wn²)`，显式欧拉。 + /// + /// 用作"整定算法是否真的收敛"的试金石——ARCHITECTURE.md §5.5 的验收标准就要求 + /// 继电整定在标准二阶系统上收敛。 + /// + /// # 相频特性（做继电实验时必须知道） + /// + /// `∠G(jω) = −atan2(2ζ·wn·ω, wn² − ω²)`：在 `ω = wn` 处只有 **−90°**， + /// 随 `ω → ∞` **渐近**趋于 −180°，**不存在有限的 −180° 穿越点**。 + /// 因此纯二阶对象在继电反馈下的极限环由**滞环宽度**决定（幅值 ≈ 滞环量级、 + /// 周期远小于 `2π/wn`），而不是由 `Tu = 2π/wn` 决定。 + /// 真实电机带延迟（`sim-harness` 的模型有 2 拍命令延迟 + 摩擦），相频会真正穿过 + /// −180°，那时的 `Tu` 才是有物理意义的临界周期。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct SecondOrderPlant { + /// 自然频率（rad/s）。 + pub wn: f32, + /// 阻尼比。 + pub zeta: f32, + /// 直流增益。 + pub gain: f32, + /// 位置状态。 + pub position: f32, + /// 速度状态。 + pub velocity: f32, + } + + impl SecondOrderPlant { + /// 构造（`wn > 0`；`zeta` 取绝对值）。 + #[must_use] + pub fn new(wn: f32, zeta: f32, gain: f32) -> Self { + Self { + wn: if wn > 0.0 { wn } else { f32::EPSILON }, + zeta: zeta.abs(), + gain, + position: 0.0, + velocity: 0.0, + } + } + } + + impl ControlPlant for SecondOrderPlant { + type Error = Infallible; + + fn step(&mut self, input: f32, dt: f32) -> Result { + if dt > 0.0 && input.is_finite() { + let accel = self.gain * self.wn * self.wn * input + - 2.0 * self.zeta * self.wn * self.velocity + - self.wn * self.wn * self.position; + self.velocity += accel * dt; + self.position += self.velocity * dt; + } + Ok(self.position) + } + + fn reset(&mut self) { + self.position = 0.0; + self.velocity = 0.0; + } + + fn describe(&self) -> String { + format!("second-order(wn={} rad/s, zeta={}, gain={})", self.wn, self.zeta, self.gain) + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn second_order_settles_at_dc_gain() { + let mut plant = SecondOrderPlant::new(20.0, 0.7, 2.0); + for _ in 0..20_000 { + plant.step(1.0, 0.0001).unwrap(); + } + assert!((plant.position - 2.0).abs() < 5e-2, "{}", plant.position); + plant.reset(); + assert!((plant.position).abs() < 1e-6); + assert!(plant.describe().contains("second-order")); + } + + #[test] + fn second_order_ignores_non_positive_dt() { + let mut plant = SecondOrderPlant::new(20.0, 0.5, 1.0); + assert!((plant.step(1.0, 0.0).unwrap()).abs() < 1e-6); + assert!((plant.velocity).abs() < 1e-6); + } + + #[test] + fn first_order_converges_to_gain_times_input() { + let mut plant = FirstOrderPlant::new(2.0, 0.1); + for _ in 0..2000 { + plant.step(1.0, 0.001).unwrap(); + } + assert!((plant.output - 2.0).abs() < 1e-3, "{}", plant.output); + } + + #[test] + fn non_positive_dt_does_not_advance() { + let mut plant = FirstOrderPlant::new(1.0, 0.1); + assert!((plant.step(5.0, 0.0).unwrap()).abs() < 1e-6); + assert!((plant.step(5.0, -1.0).unwrap()).abs() < 1e-6); + assert!((plant.step(f32::NAN, 0.001).unwrap()).abs() < 1e-6); + } + + #[test] + fn reset_clears_history() { + let mut plant = FirstOrderPlant::new(1.0, 0.05); + for _ in 0..100 { + plant.step(1.0, 0.001).unwrap(); + } + assert!(plant.output > 0.0); + plant.reset(); + assert!((plant.output).abs() < 1e-6); + assert!(plant.describe().contains("first-order")); + } + } +} + +pub use plant::*; + +pub mod position { + //! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。 + + use crate::velocity::VelocityController; + use pid::Pid; + + /// 位置环拓扑。 + #[derive(Debug, Clone)] + pub enum PositionLoop { + /// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。 + Direct(Pid), + /// 级联：位置 → 目标转速 → 速度环 → effort_raw。 + Cascade { + /// 外环位置 PID（输出限幅 = 最大 rpm）。 + pos: Pid, + /// 内环速度控制器（输出限幅 = 最大 effort）。 + vel: VelocityController, + }, + } + + /// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。 + #[derive(Debug, Clone)] + pub struct PositionController { + inner: PositionLoop, + } + + impl PositionController { + /// 单环位置控制器。 + #[must_use] + pub fn direct(pid: Pid) -> Self { + Self { inner: PositionLoop::Direct(pid) } + } + + /// 级联位置控制器（限速）。 + #[must_use] + pub fn cascade(pos: Pid, vel: VelocityController) -> Self { + Self { inner: PositionLoop::Cascade { pos, vel } } + } + + /// 单步更新，返回 effort_raw（已含限幅）。 + /// + /// `measured_rpm` 仅级联内环使用；单环忽略该参数。 + pub fn update( + &mut self, + target_rev: f32, + measured_rev: f32, + measured_rpm: f32, + dt: f32, + ) -> f32 { + match &mut self.inner { + PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt), + PositionLoop::Cascade { pos, vel } => { + let vel_target = pos.update(target_rev, measured_rev, dt); + vel.update(vel_target, measured_rpm, dt) + } + } + } + + /// 清空内部状态（切换模式 / 重新使能时调用）。 + pub fn reset(&mut self) { + match &mut self.inner { + PositionLoop::Direct(pid) => pid.reset(), + PositionLoop::Cascade { pos, vel } => { + pos.reset(); + vel.reset(); + } + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + const DT: f32 = 0.001; + + #[test] + fn direct_outputs_pid() { + let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0)); + let out = pc.update(1.0, 0.5, 0.0, DT); + assert!((out - 1.0).abs() < 1e-4, "out = {out}"); + } + + #[test] + fn cascade_limits_velocity_then_effort() { + // 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。 + let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0); + let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0); + let mut pc = PositionController::cascade(pos, vel); + let out = pc.update(1.0, 0.0, 0.0, DT); + // 外环饱和到 5 rpm，内环 ff=2 → 10 raw。 + assert!((out - 10.0).abs() < 1e-3, "out = {out}"); + } + + #[test] + fn reset_clears_state() { + let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6)); + pc.update(1.0, 0.0, 0.0, DT); + pc.reset(); + let out = pc.update(1.0, 0.0, 0.0, DT); + // 重置后只剩本步积分：10 * 1 * 0.001。 + assert!((out - 0.01).abs() < 1e-5, "out = {out}"); + } + } +} + +pub use position::*; + +pub mod velocity { + //! 速度环控制器：PID + 速度前馈。 + + use 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，物理方向），输出为开环控制量 + /// effort_raw（协议 raw：ESC=电流，GM6020=电压）。 + /// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量 + /// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。 + /// + /// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。 + #[derive(Debug, Clone)] + pub struct VelocityController { + pid: Pid, + feedforward: f32, + } + + impl VelocityController { + /// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。 + #[must_use] + pub fn new(pid: Pid, feedforward: f32) -> Self { + 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); + (pid_out + self.feedforward * target_rpm).clamp( + // 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。 + self.pid.output_min(), + self.pid.output_max(), + ) + } + + /// 清空 PID 内部状态（切换模式 / 重新使能时调用）。 + pub fn reset(&mut self) { + self.pid.reset(); + } + + /// 前馈系数（raw / rpm）。 + #[must_use] + pub fn feedforward(&self) -> f32 { + self.feedforward + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn pure_feedforward_tracks_target() { + let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0); + assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3); + // 前馈 -18000 被限幅到 -16384。 + assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3); + } + + #[test] + fn feedforward_plus_pid_respects_limit() { + let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0); + // 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。 + let out = vc.update(1000.0, 0.0, 0.001); + assert!((out - 12000.0).abs() < 1e-3, "out = {out}"); + } + + #[test] + fn zero_error_yields_pure_feedforward() { + // 误差为 0 时 PID 贡献为 0，输出只剩前馈。 + let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0); + let out = vc.update(1000.0, 1000.0, 0.001); + assert!((out - 9000.0).abs() < 1e-4, "out = {out}"); + } + + #[test] + fn reset_clears_pid_state() { + let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0); + for _ in 0..10 { + vc.update(100.0, 0.0, 0.001); + } + vc.reset(); + // 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。 + 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)); + } + } +} + +pub use velocity::*; + +pub mod cascade { + //! 串级控制：位置环（外）+ 速度环（内）（ARCHITECTURE.md §5.5）。 + //! + //! 外环输出的是**速度目标**而不是控制量，因此内环必须在每个节拍都跑一次—— + //! 这也是云台（位置）与底盘（速度）共用的结构：底盘只用内环，云台两环都用。 + //! + //! ```text + //! 位置目标 ──►[ 外环 PID ]──► 速度目标 ──►[ 内环 PID ]──► effort + //! ▲ ▲ + //! 位置反馈 速度反馈 + //! ``` + + use pid::Pid; + + /// 位置-速度串级控制器。 + #[derive(Debug, Clone)] + pub struct CascadeController { + /// 外环：位置 → 速度目标。 + outer: Pid, + /// 内环：速度 → 控制量。 + inner: Pid, + } + + impl CascadeController { + /// 由两个已配置好的 PID 组装。 + #[must_use] + pub const fn new(outer: Pid, inner: Pid) -> Self { + Self { outer, inner } + } + + /// 单步更新，返回内环输出（控制量）。 + /// + /// 非有限输入会被内环防御性忽略（见 [`Pid::update`]），因此本函数不会 panic。 + pub fn compute( + &mut self, + position_setpoint: f32, + position_feedback: f32, + velocity_feedback: f32, + dt: f32, + ) -> f32 { + let velocity_setpoint = self.outer.update(position_setpoint, position_feedback, dt); + self.inner.update(velocity_setpoint, velocity_feedback, dt) + } + + /// 外环当前建议的速度目标（诊断 / 限幅用）。 + #[must_use] + pub fn last_velocity_setpoint(&self) -> f32 { + self.outer.last_output() + } + + /// 清空两环的积分与微分记忆。 + pub fn reset(&mut self) { + self.outer.reset(); + self.inner.reset(); + } + } + + #[cfg(test)] + mod tests { + use super::*; + use crate::plant::{ControlPlant, FirstOrderPlant}; + + #[test] + fn cascade_drives_a_first_order_plant_to_the_position_setpoint() { + // 内环（速度）与对象同量纲简化：直接把外环输出当对象输入， + // 用真实的一阶对象验证"两环串联能收敛"。 + let mut cascade = CascadeController::new( + Pid::new(4.0, 8.0, 0.0, -100.0, 100.0), + Pid::new(6.0, 20.0, 0.0, -100.0, 100.0), + ); + let mut plant = FirstOrderPlant::new(1.0, 0.05); + let dt = 0.001; + let mut position = 0.0f32; + for _ in 0..4000 { + let effort = cascade.compute(1.0, position, 0.0, dt); + let velocity = plant.step(effort, dt).unwrap(); + position += velocity * dt; + } + assert!((position - 1.0).abs() < 0.05, "position = {position}"); + assert!(plant.describe().contains("first-order")); + } + + #[test] + fn reset_clears_integral_windup() { + let mut cascade = CascadeController::new( + Pid::new(1.0, 1.0, 0.0, -10.0, 10.0), + Pid::new(1.0, 1.0, 0.0, -10.0, 10.0), + ); + for _ in 0..100 { + cascade.compute(100.0, 0.0, 0.0, 0.001); + } + assert!(cascade.last_velocity_setpoint().abs() > 0.0); + cascade.reset(); + assert!((cascade.last_velocity_setpoint()).abs() < 1e-6); + } + } +} + +pub use cascade::*; + +/// Per-motor controller state used by [`MotorPolicy`]. +struct PolicyLoop { + current_limit: i16, + gains: VelocityGains, + velocity: VelocityController, + position: PositionController, + last_mode: motor::CommandMode, +} + +impl PolicyLoop { + fn new(limit: i16, gains: VelocityGains) -> Self { + let bound = f32::from(limit); + Self { + current_limit: limit, + gains, + velocity: VelocityController::from_gains(limit, gains), + position: PositionController::direct(pid::Pid::new( + gains.kp, gains.ki, gains.kd, -bound, bound, + )), + last_mode: motor::CommandMode::Disabled, + } + } + + fn reset(&mut self) { + self.velocity.reset(); + self.position.reset(); + self.last_mode = motor::CommandMode::Disabled; + } + + fn effort(&mut self, sample: motor::PolicySample, dt: f32) -> i16 { + if !sample.online { + self.reset(); + return 0; + } + if self.last_mode != sample.command.mode() { + self.reset(); + self.last_mode = sample.command.mode(); + } + match sample.command { + motor::Command::Disabled => { + self.reset(); + 0 + } + motor::Command::EffortRaw(raw) => raw, + motor::Command::Velocity(target) => { + self.velocity.update(target, sample.state.rotor_speed_rpm, dt) as i16 + } + motor::Command::Position(target) => self.position.update( + target, + sample.state.rotor_revolutions, + sample.state.rotor_speed_rpm, + dt, + ) as i16, + } + } +} + +/// Concrete velocity/position policy for up to eight ESCs. +pub struct MotorPolicy { + loops: [Option; 8], +} + +impl MotorPolicy { + /// Create an empty policy. + #[must_use] + pub fn new() -> Self { + Self { loops: std::array::from_fn(|_| None) } + } + + /// Configure one ESC controller. + pub fn configure(&mut self, esc_id: u8, current_limit: i16, gains: VelocityGains) { + if let Some(slot) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) { + *slot = Some(PolicyLoop::new(current_limit, gains)); + } + } + + /// Read one controller's current gains. + #[must_use] + pub fn velocity_gains(&self, esc_id: u8) -> Option { + self.loops + .get(usize::from(esc_id.saturating_sub(1))) + .and_then(Option::as_ref) + .map(|loop_| loop_.gains) + } + + /// Replace one controller's gains without changing its output limit. + pub fn set_velocity_gains(&mut self, esc_id: u8, gains: VelocityGains) { + if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) { + let limit = loop_.current_limit; + *loop_ = PolicyLoop::new(limit, gains); + } + } +} + +impl Default for MotorPolicy { + fn default() -> Self { + Self::new() + } +} + +impl motor::EffortPolicy for MotorPolicy { + fn step(&mut self, input: &motor::PolicyInput) -> [i16; 8] { + std::array::from_fn(|index| { + let sample = input.samples[index]; + match self.loops[index].as_mut() { + Some(loop_) if sample.id == u8::try_from(index + 1).unwrap_or(0) => { + loop_.effort(sample, input.dt) + } + _ => 0, + } + }) + } + + fn reset(&mut self, esc_id: u8) { + if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) { + loop_.reset(); + } + } +} diff --git a/crates/motor-core/src/command.rs b/crates/motor-core/src/command.rs deleted file mode 100644 index 70388f4..0000000 --- a/crates/motor-core/src/command.rs +++ /dev/null @@ -1,41 +0,0 @@ -//! 驱动层命令寄存器（最后写入胜出）。 - -/// 发给驱动层的电机命令。 -#[derive(Debug, Clone, Copy, PartialEq)] -pub enum Command { - /// 停用：目标清零，下发 0 输出。 - Disabled, - /// 直接下发开环 raw（协议量程，物理方向），绕过闭环。 - /// ESC = 电流 raw；GM6020 = 电压 raw。 - EffortRaw(i16), - /// 速度环跟踪目标转子转速（rpm，物理方向）。 - Velocity(f32), - /// 位置环跟踪目标转子多圈位置（圈，物理方向）。 - Position(f32), -} - -/// 命令模式标签（不含设定值）——用于切换时复位控制器。 -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum CommandMode { - /// 停用。 - Disabled, - /// 开环 raw。 - EffortRaw, - /// 速度环。 - Velocity, - /// 位置环。 - Position, -} - -impl Command { - /// 模式标签。 - #[must_use] - pub const fn mode(self) -> CommandMode { - match self { - Self::Disabled => CommandMode::Disabled, - Self::EffortRaw(_) => CommandMode::EffortRaw, - Self::Velocity(_) => CommandMode::Velocity, - Self::Position(_) => CommandMode::Position, - } - } -} diff --git a/crates/motor-core/src/gearbox.rs b/crates/motor-core/src/gearbox.rs deleted file mode 100644 index 8877747..0000000 --- a/crates/motor-core/src/gearbox.rs +++ /dev/null @@ -1,182 +0,0 @@ -//! 输出轴单位适配器：减速比换算。 -//! -//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。 -//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。 - -use std::time::Duration; - -use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive}; - -/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。 -pub struct OutputShaft { - inner: T, - gear_ratio: f32, -} - -impl OutputShaft { - /// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。 - /// - /// # Panics - /// - /// `gear_ratio` 非有限或 ≤ 0 时 panic。 - #[must_use] - pub fn new(inner: T, gear_ratio: f32) -> Self { - assert!( - gear_ratio.is_finite() && gear_ratio > 0.0, - "gear_ratio must be > 0, got {gear_ratio}" - ); - Self { inner, gear_ratio } - } - - /// 减速比。 - #[must_use] - pub fn gear_ratio(&self) -> f32 { - self.gear_ratio - } - - /// 底层电机引用。 - #[must_use] - pub fn inner(&self) -> &T { - &self.inner - } - - /// 转子量 → 输出轴量。 - #[must_use] - pub fn to_output(&self, rotor: f32) -> f32 { - rotor / self.gear_ratio - } - - /// 输出轴量 → 转子量。 - #[must_use] - pub fn to_rotor(&self, output: f32) -> f32 { - output * self.gear_ratio - } - - /// 由反馈快照推算输出轴转速（rpm）。 - #[must_use] - pub fn output_speed_rpm(&self, state: &MotorState) -> f32 { - state.rotor_speed_rpm / self.gear_ratio - } -} - -impl MotorFeedback for OutputShaft { - fn id(&self) -> u8 { - self.inner.id() - } - - fn state(&self) -> MotorState { - // 只透传转子量；不伪造输出轴字段。 - self.inner.state() - } - - fn feedback_age(&self) -> Option { - self.inner.feedback_age() - } -} - -impl VelocityDrive for OutputShaft { - /// `output_rpm` 为**输出轴**转速。 - fn set_velocity(&self, output_rpm: f32) { - self.inner.set_velocity(self.to_rotor(output_rpm)); - } -} - -impl EffortDrive for OutputShaft { - fn set_effort_raw(&self, raw: i16) { - self.inner.set_effort_raw(raw); - } -} - -impl PositionDrive for OutputShaft { - /// `output_rev` 为**输出轴**圈数。 - fn set_position(&self, output_rev: f32) { - self.inner.set_position(self.to_rotor(output_rev)); - } -} - -impl Disable for OutputShaft { - fn disable(&self) { - self.inner.disable(); - } -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::Command; - - struct Stub { - cmd: std::sync::Mutex, - } - - impl Stub { - fn new() -> Self { - Self { cmd: std::sync::Mutex::new(Command::Disabled) } - } - } - - impl MotorFeedback for Stub { - fn id(&self) -> u8 { - 1 - } - 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) { - *self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm); - } - } - impl EffortDrive for Stub { - fn set_effort_raw(&self, raw: i16) { - *self.cmd.lock().unwrap() = Command::EffortRaw(raw); - } - } - impl PositionDrive for Stub { - fn set_position(&self, rotor_rev: f32) { - *self.cmd.lock().unwrap() = Command::Position(rotor_rev); - } - } - impl Disable for Stub { - fn disable(&self) { - *self.cmd.lock().unwrap() = Command::Disabled; - } - } - - #[test] - fn velocity_scales_by_gear() { - let shaft = OutputShaft::new(Stub::new(), 19.203); - shaft.set_velocity(100.0); - match *shaft.inner().cmd.lock().unwrap() { - Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"), - ref c => panic!("unexpected {c:?}"), - } - } - - #[test] - fn position_scales_by_gear() { - let shaft = OutputShaft::new(Stub::new(), 36.0); - shaft.set_position(2.0); - match *shaft.inner().cmd.lock().unwrap() { - Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"), - ref c => panic!("unexpected {c:?}"), - } - } - - #[test] - fn effort_passes_through() { - let shaft = OutputShaft::new(Stub::new(), 10.0); - shaft.set_effort_raw(-1234); - assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234)); - } - - #[test] - #[should_panic = "gear_ratio"] - fn rejects_nonpositive_gear() { - let _ = OutputShaft::new(Stub::new(), 0.0); - } -} diff --git a/crates/motor-core/src/heading.rs b/crates/motor-core/src/heading.rs deleted file mode 100644 index d605cc0..0000000 --- a/crates/motor-core/src/heading.rs +++ /dev/null @@ -1,21 +0,0 @@ -//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。 -//! -//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。 -//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。 - -use std::future::Future; - -/// 绝对航向观测源。 -pub trait HeadingSource: Send { - /// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。 - fn heading_rad(&self) -> impl Future + Send; -} - -/// 云台目标角的参考系。 -#[derive(Debug, Clone, Copy, PartialEq, Eq)] -pub enum HeadingFrame { - /// 电机转子多圈（上电相对）。 - RotorMultiTurn, - /// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。 - AbsoluteField, -} diff --git a/crates/motor-core/src/lib.rs b/crates/motor-core/src/lib.rs deleted file mode 100644 index aa157ef..0000000 --- a/crates/motor-core/src/lib.rs +++ /dev/null @@ -1,93 +0,0 @@ -//! 协议无关的电机抽象层。 -//! -//! 本 crate 不做任何 I/O：它定义电机能力 trait（[`MotorFeedback`] / -//! [`EffortDrive`] / [`VelocityDrive`] / [`PositionDrive`] / [`Disable`]）、 -//! 公共数据类型（[`MotorState`]、[`Command`]）以及纯控制算法（[`Pid`]、 -//! [`VelocityController`]、[`PositionController`]）。具体总线协议在其上实现。 -//! -//! # 单一职责 -//! -//! - [`traits`]：能力面（小而正交）； -//! - [`state`] / [`command`]：反馈快照与命令寄存器； -//! - [`pid`] / [`velocity`] / [`position`]：纯控制律； -//! - [`gearbox`]：减速比单位适配（输出轴 ↔ 转子）； -//! - [`heading`]：航向观测 seam（云台绝对角融合用）； -//! - [`testing`]：测试替身。 -//! -//! # 单位与方向约定 -//! -//! - 所有呈现给上层的量都是**安装方向修正后**的物理量：`direction = -1` -//! 的电机由驱动层在读写两个方向上统一翻转符号。 -//! - 转速一律为电机**转子**转速（rpm）；输出轴换算见 [`OutputShaft`]。 -//! - 开环控制量以协议原始值（raw）为主（C620 电流：±16384 ↔ ±20 A； -//! GM6020 电压：±30000 ≈ 满电压）。 -#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] - -pub mod command; -pub mod gearbox; -pub mod heading; -pub mod pid; -pub mod position; -pub mod state; -pub mod testing; -pub mod traits; -pub mod velocity; - -pub use command::{Command, CommandMode}; -pub use gearbox::OutputShaft; -pub use heading::{HeadingFrame, HeadingSource}; -pub use pid::Pid; -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, VelocityGains}; - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn default_state_has_no_feedback() { - let state = MotorState::default(); - assert!(!state.has_feedback()); - assert_eq!(state.feedback_count, 0); - } - - #[test] - fn feedback_gate_flips_on_first_frame() { - let state = MotorState { feedback_count: 1, ..MotorState::default() }; - assert!(state.has_feedback()); - } - - #[test] - fn command_variants_roundtrip_by_value() { - let cmds = [ - Command::Disabled, - Command::EffortRaw(-16384), - Command::Velocity(4820.5), - Command::Position(2.5), - ]; - assert_eq!(cmds[0], Command::Disabled); - assert_eq!(cmds[1], Command::EffortRaw(-16384)); - assert_eq!(cmds[2], Command::Velocity(4820.5)); - assert_eq!(cmds[3], Command::Position(2.5)); - } - - #[test] - fn command_mode_labels() { - assert_eq!(Command::Disabled.mode(), CommandMode::Disabled); - assert_eq!(Command::EffortRaw(0).mode(), CommandMode::EffortRaw); - assert_eq!(Command::Velocity(0.0).mode(), CommandMode::Velocity); - assert_eq!(Command::Position(0.0).mode(), CommandMode::Position); - } - - #[test] - fn mock_motor_is_full_motor() { - fn takes_motor(m: &impl Motor) { - m.set_velocity(1.0); - m.disable(); - } - takes_motor(&MockMotor::new(1)); - } -} diff --git a/crates/motor-core/src/position.rs b/crates/motor-core/src/position.rs deleted file mode 100644 index 8409066..0000000 --- a/crates/motor-core/src/position.rs +++ /dev/null @@ -1,103 +0,0 @@ -//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。 - -use crate::pid::Pid; -use crate::velocity::VelocityController; - -/// 位置环拓扑。 -#[derive(Debug, Clone)] -pub enum PositionLoop { - /// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。 - Direct(Pid), - /// 级联：位置 → 目标转速 → 速度环 → effort_raw。 - Cascade { - /// 外环位置 PID（输出限幅 = 最大 rpm）。 - pos: Pid, - /// 内环速度控制器（输出限幅 = 最大 effort）。 - vel: VelocityController, - }, -} - -/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。 -#[derive(Debug, Clone)] -pub struct PositionController { - inner: PositionLoop, -} - -impl PositionController { - /// 单环位置控制器。 - #[must_use] - pub fn direct(pid: Pid) -> Self { - Self { inner: PositionLoop::Direct(pid) } - } - - /// 级联位置控制器（限速）。 - #[must_use] - pub fn cascade(pos: Pid, vel: VelocityController) -> Self { - Self { inner: PositionLoop::Cascade { pos, vel } } - } - - /// 单步更新，返回 effort_raw（已含限幅）。 - /// - /// `measured_rpm` 仅级联内环使用；单环忽略该参数。 - pub fn update( - &mut self, - target_rev: f32, - measured_rev: f32, - measured_rpm: f32, - dt: f32, - ) -> f32 { - match &mut self.inner { - PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt), - PositionLoop::Cascade { pos, vel } => { - let vel_target = pos.update(target_rev, measured_rev, dt); - vel.update(vel_target, measured_rpm, dt) - } - } - } - - /// 清空内部状态（切换模式 / 重新使能时调用）。 - pub fn reset(&mut self) { - match &mut self.inner { - PositionLoop::Direct(pid) => pid.reset(), - PositionLoop::Cascade { pos, vel } => { - pos.reset(); - vel.reset(); - } - } - } -} - -#[cfg(test)] -mod tests { - use super::*; - - const DT: f32 = 0.001; - - #[test] - fn direct_outputs_pid() { - let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0)); - let out = pc.update(1.0, 0.5, 0.0, DT); - assert!((out - 1.0).abs() < 1e-4, "out = {out}"); - } - - #[test] - fn cascade_limits_velocity_then_effort() { - // 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。 - let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0); - let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0); - let mut pc = PositionController::cascade(pos, vel); - let out = pc.update(1.0, 0.0, 0.0, DT); - // 外环饱和到 5 rpm，内环 ff=2 → 10 raw。 - assert!((out - 10.0).abs() < 1e-3, "out = {out}"); - } - - #[test] - fn reset_clears_state() { - let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6)); - pc.update(1.0, 0.0, 0.0, DT); - pc.reset(); - let out = pc.update(1.0, 0.0, 0.0, DT); - // 重置后只剩本步积分：10 * 1 * 0.001。 - assert!((out - 0.01).abs() < 1e-5, "out = {out}"); - } -} diff --git a/crates/motor-core/src/state.rs b/crates/motor-core/src/state.rs deleted file mode 100644 index 77d0727..0000000 --- a/crates/motor-core/src/state.rs +++ /dev/null @@ -1,29 +0,0 @@ -//! 电机反馈快照（协议无关，方向修正后的物理量）。 -//! -//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`] -//! 或上层完成，不在此泄漏底盘语义。 - -/// 电机反馈快照。 -#[derive(Debug, Clone, Copy, PartialEq, Default)] -pub struct MotorState { - /// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。 - pub rotor_angle_raw: u16, - /// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。 - pub rotor_revolutions: f32, - /// 转子转速（rpm，物理方向）。 - pub rotor_speed_rpm: f32, - /// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。 - pub effort_raw: i16, - /// 电机温度（℃）。 - pub temperature_c: i8, - /// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。 - pub feedback_count: u64, -} - -impl MotorState { - /// 是否已收到过反馈（上电后首帧门控）。 - #[must_use] - pub fn has_feedback(&self) -> bool { - self.feedback_count > 0 - } -} diff --git a/crates/motor-core/src/testing.rs b/crates/motor-core/src/testing.rs deleted file mode 100644 index 0283447..0000000 --- a/crates/motor-core/src/testing.rs +++ /dev/null @@ -1,145 +0,0 @@ -//! 测试替身：内存电机与航向源。 - -use std::future::Future; -use std::sync::{Arc, Mutex, MutexGuard}; -use std::time::{Duration, Instant}; - -use crate::{ - Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive, - VelocityDrive, -}; - -/// 锁辅助：中毒锁直接恢复内部数据。 -fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { - mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) -} - -/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。 -/// -/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。 -#[derive(Clone)] -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))), - 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)); - } - - /// 最近一次命令。 - #[must_use] - pub fn last_command(&self) -> Command { - lock(&self.published).1 - } -} - -impl MotorFeedback for MockMotor { - fn id(&self) -> u8 { - self.id - } - 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 { - fn set_velocity(&self, rotor_rpm: f32) { - lock(&self.published).1 = Command::Velocity(rotor_rpm); - } -} - -impl EffortDrive for MockMotor { - fn set_effort_raw(&self, raw: i16) { - lock(&self.published).1 = Command::EffortRaw(raw); - } -} - -impl PositionDrive for MockMotor { - fn set_position(&self, rotor_rev: f32) { - lock(&self.published).1 = Command::Position(rotor_rev); - } -} - -impl Disable for MockMotor { - fn disable(&self) { - lock(&self.published).1 = Command::Disabled; - } -} - -/// 恒定航向测试源。 -pub struct MockHeading { - rad: f32, -} - -impl MockHeading { - /// 创建固定航向源。 - #[must_use] - pub const fn new(rad: f32) -> Self { - Self { rad } - } -} - -impl HeadingSource for MockHeading { - fn heading_rad(&self) -> impl Future + Send { - std::future::ready(self.rad) - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn mock_motor_records_commands() { - let m = MockMotor::new(3); - m.set_velocity(1000.0); - assert_eq!(m.last_command(), Command::Velocity(1000.0)); - m.disable(); - assert_eq!(m.last_command(), Command::Disabled); - } - - #[test] - 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 deleted file mode 100644 index c03ddca..0000000 --- a/crates/motor-core/src/traits.rs +++ /dev/null @@ -1,55 +0,0 @@ -//! 能力面 trait：小而正交，按硬件事实拆分。 -//! -//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）： -//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`] -//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]） -//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`] -//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`] - -use std::time::Duration; - -use crate::MotorState; - -/// 读反馈。所有可观察设备的最小面。 -pub trait MotorFeedback: Send + Sync { - /// 控制器内的电机 ID（对 DJI 电调即电调 ID）。 - fn id(&self) -> u8; - - /// 最新反馈快照。 - fn state(&self) -> MotorState; - - /// 距最近一帧反馈的时长；`None` 表示从未收到。 - /// - /// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`] - /// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。 - fn feedback_age(&self) -> Option; -} - -/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。 -pub trait EffortDrive: Send + Sync { - /// 直接下发开环 raw（协议量程，物理方向）。 - fn set_effort_raw(&self, raw: i16); -} - -/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。 -pub trait VelocityDrive: Send + Sync { - /// 设定目标转子转速。 - fn set_velocity(&self, rotor_rpm: f32); -} - -/// 位置目标（转子多圈，圈；物理方向）。 -pub trait PositionDrive: Send + Sync { - /// 设定目标转子多圈位置。 - fn set_position(&self, rotor_rev: f32); -} - -/// 停用（零输出）。 -pub trait Disable: Send + Sync { - /// 停用电机。 - fn disable(&self); -} - -/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。 -pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {} - -impl Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {} diff --git a/crates/motor-core/src/velocity.rs b/crates/motor-core/src/velocity.rs deleted file mode 100644 index 227d9ab..0000000 --- a/crates/motor-core/src/velocity.rs +++ /dev/null @@ -1,180 +0,0 @@ -//! 速度环控制器：PID + 速度前馈。 - -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，物理方向），输出为开环控制量 -/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。 -/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量 -/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。 -/// -/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。 -#[derive(Debug, Clone)] -pub struct VelocityController { - pid: Pid, - feedforward: f32, -} - -impl VelocityController { - /// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。 - #[must_use] - pub fn new(pid: Pid, feedforward: f32) -> Self { - 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); - (pid_out + self.feedforward * target_rpm).clamp( - // 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。 - self.pid.output_min(), - self.pid.output_max(), - ) - } - - /// 清空 PID 内部状态（切换模式 / 重新使能时调用）。 - pub fn reset(&mut self) { - self.pid.reset(); - } - - /// 前馈系数（raw / rpm）。 - #[must_use] - pub fn feedforward(&self) -> f32 { - self.feedforward - } -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn pure_feedforward_tracks_target() { - let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0); - assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3); - // 前馈 -18000 被限幅到 -16384。 - assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3); - } - - #[test] - fn feedforward_plus_pid_respects_limit() { - let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0); - // 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。 - let out = vc.update(1000.0, 0.0, 0.001); - assert!((out - 12000.0).abs() < 1e-3, "out = {out}"); - } - - #[test] - fn zero_error_yields_pure_feedforward() { - // 误差为 0 时 PID 贡献为 0，输出只剩前馈。 - let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0); - let out = vc.update(1000.0, 1000.0, 0.001); - assert!((out - 9000.0).abs() < 1e-4, "out = {out}"); - } - - #[test] - fn reset_clears_pid_state() { - let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0); - for _ in 0..10 { - vc.update(100.0, 0.0, 0.001); - } - vc.reset(); - // 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。 - 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/crates/motor-model/Cargo.toml b/crates/motor-model/Cargo.toml new file mode 100644 index 0000000..2ff3297 --- /dev/null +++ b/crates/motor-model/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "motor-model" +description = "DC motor physical simulation model" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +motor-protocol = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/motor-model/src/lib.rs b/crates/motor-model/src/lib.rs new file mode 100644 index 0000000..06fad59 --- /dev/null +++ b/crates/motor-model/src/lib.rs @@ -0,0 +1,426 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! DC motor physical models. + +pub mod dc_motor { + //! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。 + //! + //! 电调坐标系（正电流 → 正转速），逐拍离散（`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 motor_protocol::{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, "复位后角度归零"); + } + } +} + +pub mod gm3508 { + //! GM3508 的被控对象参数（ARCHITECTURE.md §5.6 / §8.2）。 + //! + //! 这里只放**有实测依据**的两组参数： + //! + //! - [`ideal_params`]：无摩擦、无延迟的理想一阶模型。历史闭环测试与 + //! `chassisd --mock` 依赖它，因此行为**逐位不变**； + //! - [`bench_params`]：在理想模型上叠加台架实测的静 / 库仑摩擦 + //! （400~600 raw 量级）与 2 拍命令延迟，是自动整定的默认被控对象。 + //! + //! 数值口径见 `docs/chassis.md` §7.4 与 `config/tune.toml`。 + + use super::dc_motor::PlantParams; + + /// 理想模型：无摩擦、无延迟（历史默认行为）。 + #[must_use] + pub fn ideal_params() -> PlantParams { + PlantParams::gm3508() + } + + /// 台架量级模型：静 / 库仑摩擦 + 2 拍延迟（整定默认）。 + #[must_use] + pub fn bench_params() -> PlantParams { + PlantParams::gm3508_bench() + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn ideal_model_has_no_friction_or_delay() { + let params = ideal_params(); + assert!(params.static_raw.abs() < 1e-6); + assert!(params.coulomb_raw.abs() < 1e-6); + assert_eq!(params.delay_ticks, 0); + } + + #[test] + fn bench_model_has_the_measured_friction_and_delay() { + let params = bench_params(); + assert!((400.0..=600.0).contains(¶ms.static_raw), "{}", params.static_raw); + assert!(params.coulomb_raw > 0.0); + assert_eq!(params.delay_ticks, 2); + // 台架模型只在摩擦 / 延迟上更真实，增益与上限应保持一致。 + let ideal = ideal_params(); + assert!((params.gain_rpm_per_raw - ideal.gain_rpm_per_raw).abs() < 1e-9); + assert!((params.max_rpm - ideal.max_rpm).abs() < 1e-6); + } + } +} + +pub use dc_motor::{PlantMotor, PlantParams}; +pub use gm3508::{bench_params, ideal_params}; diff --git a/crates/motor-protocol/Cargo.toml b/crates/motor-protocol/Cargo.toml new file mode 100644 index 0000000..1e76583 --- /dev/null +++ b/crates/motor-protocol/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "motor-protocol" +description = "DJI C620/C610 and GM6020 wire protocols" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +thiserror = { workspace = true } + +[dev-dependencies] +proptest = { workspace = true } +approx = { workspace = true } diff --git a/crates/motor-protocol/src/lib.rs b/crates/motor-protocol/src/lib.rs new file mode 100644 index 0000000..d008b8a --- /dev/null +++ b/crates/motor-protocol/src/lib.rs @@ -0,0 +1,911 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! DJI motor wire protocols. + +pub mod common { + //! DJI 电调（C620 / C610）CAN 协议编解码。 + //! + //! 帧格式（大端）： + //! + //! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）： + //! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令， + //! 槽位按组内电调顺序。 + //! - 反馈（`0x200 + 电调ID`）： + //! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。 + //! + //! 本模块只做纯编解码；主机侧速度/位置环由 `motor-control` 的算法在 + //! `motor / esc-bus` 的总线驱动中按节拍调用。 + + use crate::frame::{ + COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE, + }; + use thiserror::Error; + + /// 电调 ID 下限。 + pub const MIN_ESC_ID: u8 = 1; + /// 电调 ID 上限。 + pub const MAX_ESC_ID: u8 = 8; + + /// 电调型号决定电流标度。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum EscModel { + /// C620（配 GM3508）：±16384 ↔ ±20 A。 + C620, + /// C610（配 M2006）：±10000 ↔ ±10 A。 + /// + /// 接入前需实测核对。 + C610, + } + + impl EscModel { + /// 满量程电流命令（raw）。 + #[must_use] + pub const fn max_current_raw(self) -> i16 { + match self { + Self::C620 => 16384, + Self::C610 => 10_000, + } + } + + /// 满量程电流（安培）。 + #[must_use] + pub const fn max_current_amp(self) -> f32 { + match self { + Self::C620 => 20.0, + Self::C610 => 10.0, + } + } + + /// 安培 → raw（限幅）。 + #[must_use] + pub fn raw_from_amp(self, amp: f32) -> i16 { + let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp(); + (raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) + as i16) + .clamp(-self.max_current_raw(), self.max_current_raw()) + } + + /// raw → 安培。 + #[must_use] + pub fn amp_from_raw(self, raw: i16) -> f32 { + f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw()) + } + } + + /// 一帧电调反馈（原始、电调侧，未经方向修正）。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct EscFeedback { + /// 转子机械角原始值（0..8191，对应 0..360°）。 + pub angle_raw: u16, + /// 转子转速（rpm，电调侧符号）。 + pub speed_rpm: i16, + /// 转矩电流 raw（电调侧符号）。 + pub current_raw: i16, + /// 电机温度（℃）。 + pub temperature_c: i8, + } + + /// 帧解码错误。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Error)] + pub enum DecodeError { + /// 帧不属于电调反馈范围（0x201..=0x208）。 + #[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")] + NotFeedback { + /// 收到的帧 ID。 + id: u32, + }, + /// 数据长度不足 8 字节（协议载荷固定 8 字节）。 + #[error("frame carries only {dlc} bytes; the ESC feedback payload needs 8")] + ShortFrame { + /// 实际数据长度码。 + dlc: u8, + }, + } + + /// 电调 ID → 其电流命令所在的组控制帧 ID。 + /// + /// 返回 `None` 表示 ID 越界。 + #[must_use] + pub const fn command_frame_id(esc_id: u8) -> Option { + match esc_id { + 1..=4 => Some(COMMAND_FRAME_ID_PRIMARY), + 5..=8 => Some(COMMAND_FRAME_ID_SECONDARY), + _ => None, + } + } + + /// 电调 ID → 其反馈帧 ID（`0x200 + 电调ID`）。 + /// + /// 返回 `None` 表示 ID 越界。GM6020 用**另一张表**，见 + /// [`crate::gm6020::feedback_frame_id`]。 + #[must_use] + pub const fn feedback_frame_id(esc_id: u8) -> Option { + if esc_id >= MIN_ESC_ID && esc_id <= MAX_ESC_ID { + Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32) + } else { + None + } + } + + /// 将 raw 电流限幅到电调满量程。 + #[must_use] + pub const fn clamp_current(model: EscModel, raw: i16) -> i16 { + if raw > model.max_current_raw() { + model.max_current_raw() + } else if raw < -model.max_current_raw() { + -model.max_current_raw() + } else { + raw + } + } + + /// 组装一个控制组帧（`0x200` 或 `0x1FF`）。 + /// + /// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。 + /// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成 + /// （见 [`clamp_current`]）。 + #[must_use] + pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame { + let mut data = [0u8; 8]; + for (slot, raw) in currents.iter().enumerate() { + data[slot * 2] = (raw >> 8) as u8; + data[slot * 2 + 1] = (raw & 0xFF) as u8; + } + CanFrame::new(frame_id, data) + } + + /// 组装一帧电调反馈（仿真器 / 测试用）。 + #[must_use] + pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame { + let mut data = [0u8; 8]; + data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes()); + data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes()); + data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes()); + data[6] = feedback.temperature_c as u8; + CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data) + } + + /// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。 + /// + /// # Errors + /// + /// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]； + /// 数据长度不足 8 字节时返回 [`DecodeError::ShortFrame`]。 + pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> { + if !frame.is_complete() { + return Err(DecodeError::ShortFrame { dlc: frame.dlc }); + } + if frame.id <= FEEDBACK_FRAME_ID_BASE + || frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID) + { + return Err(DecodeError::NotFeedback { id: frame.id }); + } + let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8; + Ok(( + esc_id, + EscFeedback { + angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]), + speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]), + current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]), + temperature_c: frame.data[6] as i8, + }, + )) + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn command_frame_id_routing() { + assert_eq!(command_frame_id(1), Some(0x200)); + assert_eq!(command_frame_id(4), Some(0x200)); + assert_eq!(command_frame_id(5), Some(0x1FF)); + assert_eq!(command_frame_id(8), Some(0x1FF)); + assert_eq!(command_frame_id(0), None); + assert_eq!(command_frame_id(9), None); + } + + #[test] + fn encode_current_frame_bytes() { + let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]); + assert_eq!(frame.id, 0x200); + assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]); + + let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]); + assert_eq!(frame.id, 0x1FF); + assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]); + } + + #[test] + fn decode_feedback_known_bytes() { + let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]); + let (esc_id, fb) = decode_feedback(&frame).unwrap(); + assert_eq!(esc_id, 5); + assert_eq!(fb.angle_raw, 8191); + assert_eq!(fb.speed_rpm, -364); + assert_eq!(fb.current_raw, 123); + assert_eq!(fb.temperature_c, 45); + } + + #[test] + fn decode_feedback_rejects_non_feedback_ids() { + for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] { + let frame = CanFrame::new(id, [0; 8]); + assert_eq!( + decode_feedback(&frame), + Err(DecodeError::NotFeedback { id }), + "id = 0x{id:03X}" + ); + } + } + + #[test] + fn feedback_roundtrip_all_ids() { + let fb = EscFeedback { + angle_raw: 8191, + speed_rpm: -9254, + current_raw: -16384, + temperature_c: -40, + }; + for esc_id in MIN_ESC_ID..=MAX_ESC_ID { + let frame = encode_feedback_frame(esc_id, &fb); + assert_eq!(frame.id, 0x200 + u32::from(esc_id)); + assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb)); + } + } + + #[test] + fn amp_conversion() { + let c620 = EscModel::C620; + assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4); + assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4); + assert_eq!(c620.raw_from_amp(20.0), 16384); + assert_eq!(c620.raw_from_amp(-5.0), -4096); + // 超量程限幅。 + assert_eq!(c620.raw_from_amp(99.0), 16384); + let c610 = EscModel::C610; + assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4); + assert_eq!(c610.raw_from_amp(10.0), 10000); + } + + #[test] + fn clamp_current_respects_model() { + assert_eq!(clamp_current(EscModel::C620, 20000), 16384); + assert_eq!(clamp_current(EscModel::C610, 15000), 10000); + assert_eq!(clamp_current(EscModel::C620, 1000), 1000); + } + } +} + +pub mod gm6020 { + //! GM6020 云台电机协议（ARCHITECTURE.md §5.2）。 + //! + //! 与 C620/C610 的差别只有两点，因此两个协议族共享同一套帧布局： + //! + //! | 项目 | C620/C610（M3508/M2006） | GM6020 | + //! | --- | --- | --- | + //! | 控制量 | **电流**（±16384 / ±10000 ↔ 满量程电流） | **电压**（±30000 ↔ 满电压） | + //! | 控制帧 ID | `0x200`（1–4）、`0x1FF`（5–8） | `0x1FF`（1–4）、`0x2FF`（5–8） | + //! | 反馈帧 ID | `0x200 + ID`（0x201–0x208） | `0x204 + ID`（0x205–0x208，ID 1–4） | + //! | 反馈载荷 | u16 角度 + i16 转速 + i16 电流 + i8 温度 | 同上 | + //! + //! > 命名说明：ARCHITECTURE.md §5.2 把命令字段写作 `current`，但 GM6020 的 + //! > 控制字是**电压**占空比命令（±30000）。这里用 `voltage_raw` 以免把 + //! > "电流环在电调内部"这件事记反——GM6020 内部同样只闭电流环。 + + use can::CanFrame; + + use crate::common::{DecodeError, MAX_ESC_ID, MIN_ESC_ID}; + use crate::id::MotorId; + + /// 控制帧 ID：GM6020 ID 1–4。 + pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x1FF; + /// 控制帧 ID：GM6020 ID 5–8。 + pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x2FF; + /// 反馈帧 ID 基址：`0x204 + ID`（ID 1–4 → 0x205–0x208）。 + pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x204; + /// 电压命令满量程（raw）。 + pub const VOLTAGE_MAX_RAW: i16 = 30_000; + /// 编码器单圈刻度（与 C620 相同）。 + pub const ENCODER_TICKS: u16 = 8192; + + /// GM6020 一帧反馈。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct Gm6020Feedback { + /// 机械角（度，0..360）。 + pub angle_deg: f32, + /// 单圈原始角度（0..8191）。 + pub angle_raw: u16, + /// 转子转速（rpm）。 + pub speed_rpm: i16, + /// 转矩电流（raw，±30000 ↔ 满量程）。 + pub current_raw: i16, + /// 温度（℃）。 + pub temperature_c: i8, + } + + /// GM6020 电压命令。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct Gm6020Command { + /// 电压命令（raw，`-30000..=30000`）。 + pub voltage_raw: i16, + } + + impl Gm6020Command { + /// 构造并夹到合法范围。 + #[must_use] + pub const fn new(voltage_raw: i16) -> Self { + Self { voltage_raw: clamp_voltage(voltage_raw) } + } + + /// 编码为**广播控制帧**（同组四个槽位）。 + /// + /// # Errors + /// + /// 帧 ID 非法时返回 [`DecodeError::NotFeedback`] 之外的 `BadFrameId`。 + /// + /// 说明：单电机编码只是"其余槽位填 0"的特例，因此本函数要求调用方 + /// 提供整组电压；单电机场景见 [`Gm6020Command::to_frame`]。 + #[must_use] + pub fn encode_group(frame_id: u32, voltages: [i16; 4]) -> CanFrame { + encode_voltage_frame(frame_id, voltages) + } + + /// 单电机编码：其余槽位补 0（会**覆盖**同组其它电机，仅用于单机联调）。 + /// + /// 注意用的是 **GM6020 自己的**控制帧映射（`0x1FF` / `0x2FF`）， + /// 而不是 [`MotorId::command_frame_id`] 的 C620 映射（`0x200` / `0x1FF`）—— + /// 两个协议族的帧 ID 表不同，混淆会导致云台收不到任何命令。 + #[must_use] + pub fn to_frame(&self, id: MotorId) -> CanFrame { + let esc_id = id.get(); + let mut voltages = [0i16; 4]; + let slot = command_slot(esc_id).unwrap_or(0); + voltages[slot] = self.voltage_raw; + let frame_id = command_frame_id(esc_id).unwrap_or(COMMAND_FRAME_ID_PRIMARY); + encode_voltage_frame(frame_id, voltages) + } + } + + /// 夹到电压满量程。 + #[must_use] + pub const fn clamp_voltage(raw: i16) -> i16 { + // `i16::clamp` 在 const fn 里尚不可用（Ord 未 const 稳定），手写三段。 + if raw > VOLTAGE_MAX_RAW { + VOLTAGE_MAX_RAW + } else if raw < -VOLTAGE_MAX_RAW { + -VOLTAGE_MAX_RAW + } else { + raw + } + } + + /// 由电调 ID 推出控制帧槽位（`None` 表示 ID 越界）。 + #[must_use] + pub const fn command_slot(esc_id: u8) -> Option { + if esc_id >= MIN_ESC_ID && esc_id <= 4 { + Some((esc_id - 1) as usize) + } else if esc_id >= 5 && esc_id <= MAX_ESC_ID { + Some((esc_id - 5) as usize) + } else { + None + } + } + + /// 由电调 ID 推出控制帧 ID（`None` 表示 ID 越界）。 + #[must_use] + pub const fn command_frame_id(esc_id: u8) -> Option { + match command_slot(esc_id) { + Some(_) if esc_id <= 4 => Some(COMMAND_FRAME_ID_PRIMARY), + Some(_) => Some(COMMAND_FRAME_ID_SECONDARY), + None => None, + } + } + + /// 反馈帧 ID（`0x204 + ID`；ID 5–8 为 `0x208 + ID`，见模块文档）。 + #[must_use] + pub const fn feedback_frame_id(esc_id: u8) -> Option { + if esc_id >= MIN_ESC_ID && esc_id <= 4 { + Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32) + } else if esc_id >= 5 && esc_id <= MAX_ESC_ID { + Some(0x208 + esc_id as u32) + } else { + None + } + } + + /// 编码一组电压命令（4 × i16 大端）。 + #[must_use] + pub fn encode_voltage_frame(frame_id: u32, voltages: [i16; 4]) -> CanFrame { + let mut data = [0u8; 8]; + for (slot, raw) in voltages.iter().enumerate() { + let clamped = clamp_voltage(*raw); + data[slot * 2] = (clamped >> 8) as u8; + data[slot * 2 + 1] = (clamped & 0xFF) as u8; + } + CanFrame::new(frame_id, data) + } + + /// 组装一帧 GM6020 反馈（仿真器 / 测试用）。 + #[must_use] + pub fn encode_feedback_frame(esc_id: u8, feedback: &Gm6020Feedback) -> CanFrame { + let mut data = [0u8; 8]; + data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes()); + data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes()); + data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes()); + data[6] = feedback.temperature_c as u8; + CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data) + } + + /// 解码一帧 GM6020 反馈，返回 `(电调ID, 反馈)`。 + /// + /// # Errors + /// + /// 帧 ID 不在 GM6020 反馈范围（0x205–0x208、0x20D–0x210）内时返回 + /// [`DecodeError::NotFeedback`]；长度不足时返回 [`DecodeError::ShortFrame`]。 + pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, Gm6020Feedback), DecodeError> { + if !frame.is_complete() { + return Err(DecodeError::ShortFrame { dlc: frame.dlc }); + } + let esc_id = if (0x205..=0x208).contains(&frame.id) { + (frame.id - FEEDBACK_FRAME_ID_BASE) as u8 + } else if (0x20D..=0x210).contains(&frame.id) { + (frame.id - 0x208) as u8 + } else { + return Err(DecodeError::NotFeedback { id: frame.id }); + }; + let angle_raw = u16::from_be_bytes([frame.data[0], frame.data[1]]); + let speed_rpm = i16::from_be_bytes([frame.data[2], frame.data[3]]); + let current_raw = i16::from_be_bytes([frame.data[4], frame.data[5]]); + let temperature_c = frame.data[6] as i8; + Ok(( + esc_id, + Gm6020Feedback { + angle_raw: angle_raw % ENCODER_TICKS, + angle_deg: f32::from(angle_raw) * 360.0 / f32::from(ENCODER_TICKS), + speed_rpm, + current_raw, + temperature_c, + }, + )) + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn voltage_is_clamped_to_full_scale() { + assert_eq!(Gm6020Command::new(32_000).voltage_raw, VOLTAGE_MAX_RAW); + assert_eq!(Gm6020Command::new(-32_000).voltage_raw, -VOLTAGE_MAX_RAW); + assert_eq!(Gm6020Command::new(1234).voltage_raw, 1234); + } + + #[test] + fn single_motor_encoding_lands_in_the_right_slot() { + let frame = Gm6020Command::new(1000).to_frame(MotorId::<1>::NEW); + assert_eq!(frame.id, 0x1FF); + assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 1000); + assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), 0); + + let frame = Gm6020Command::new(-1000).to_frame(MotorId::<6>::NEW); + assert_eq!(frame.id, 0x2FF, "ID 5–8 走副组帧"); + assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -1000); + } + + #[test] + fn group_encoding_matches_slot_order() { + let frame = encode_voltage_frame(COMMAND_FRAME_ID_PRIMARY, [10, -20, 30, -40]); + assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 10); + assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -20); + assert_eq!(i16::from_be_bytes([frame.data[4], frame.data[5]]), 30); + assert_eq!(i16::from_be_bytes([frame.data[6], frame.data[7]]), -40); + } + + #[test] + fn feedback_round_trips_and_converts_angle() { + let feedback = Gm6020Feedback { + angle_deg: 0.0, + angle_raw: 2048, + speed_rpm: -321, + current_raw: 4567, + temperature_c: 42, + }; + let frame = encode_feedback_frame(2, &feedback); + assert_eq!(frame.id, 0x206); + let (id, decoded) = decode_feedback(&frame).unwrap(); + assert_eq!(id, 2); + assert_eq!(decoded.angle_raw, 2048); + assert!((decoded.angle_deg - 90.0).abs() < 1e-3, "2048/8192 应为 90°"); + assert_eq!(decoded.speed_rpm, -321); + assert_eq!(decoded.current_raw, 4567); + assert_eq!(decoded.temperature_c, 42); + } + + #[test] + fn feedback_id_range_is_enforced() { + assert_eq!(feedback_frame_id(1), Some(0x205)); + assert_eq!(feedback_frame_id(4), Some(0x208)); + assert_eq!(feedback_frame_id(9), None); + let c620 = CanFrame::new(0x201, [0; 8]); + assert!(matches!(decode_feedback(&c620), Err(DecodeError::NotFeedback { .. }))); + } + + #[test] + fn short_frames_are_rejected() { + let short = CanFrame::new(0x205, [0; 8]).with_dlc(4); + assert!(matches!(decode_feedback(&short), Err(DecodeError::ShortFrame { .. }))); + } + } +} + +pub mod m2006 { + //! M2006 + C610：拨弹 / 小负载电机。本项目只用到协议常量，用于未来的 + //! 发射机构（发射机构尚未实现，见 ARCHITECTURE.md §5.7 的 `shooter` 任务）。 + + use crate::common::EscModel; + + /// C610 电调型号（M2006 的配套电调）。 + /// + /// 电流标度 ±10000 ↔ ±10 A **接入前需实测核对**（ARCHITECTURE.md §2.2 注）。 + pub const C610: EscModel = EscModel::C610; + + /// 减速比（转子 : 输出轴）＝ 36 : 1（M2006 常见配置，接硬件前核对）。 + pub const GEAR_RATIO: f32 = 36.0; + + /// 编码器单圈刻度。 + pub const ENCODER_TICKS: u16 = 8192; + + /// 额定电流满量程（A，对应 raw 满量程 ±10000）。 + pub const MAX_CURRENT_AMP: f32 = 10.0; + + #[cfg(test)] + mod tests { + use super::*; + use crate::common::clamp_current; + + #[test] + fn c610_saturation_uses_the_c610_scale() { + assert_eq!(C610.max_current_raw(), 10_000); + assert_eq!(clamp_current(C610, 20_000), 10_000); + assert_eq!(clamp_current(C610, -20_000), -10_000); + } + + #[test] + fn max_current_amp_matches_the_raw_scale() { + assert!((C610.max_current_amp() - MAX_CURRENT_AMP).abs() < 1e-6); + } + } +} + +pub mod m3508 { + //! GM3508 + C620：底盘驱动轮的电流协议族（本项目的主力电机）。 + //! + //! 全部编解码都在 [`crate::common`]（C620 与 C610 帧布局相同，只有 + //! 电流标度不同），本模块只固定 GM3508 的**型号常量与机械参数**，避免 + //! 每个使用点各自抄一遍数字。 + + use crate::common::EscModel; + + /// C620 电调型号（GM3508 的配套电调）。 + pub const C620: EscModel = EscModel::C620; + + /// 减速比（转子 : 输出轴）＝ 3591 / 187 ≈ 19.203。 + pub const GEAR_RATIO: f32 = 3591.0 / 187.0; + + /// 编码器单圈刻度。 + pub const ENCODER_TICKS: u16 = 8192; + + /// 额定电流满量程（A，对应 raw 满量程 ±16384）。 + pub const MAX_CURRENT_AMP: f32 = 20.0; + + /// 空载转速-电流增益（rpm / raw），来自台架实测（≈ 9254 / 16384）。 + pub const NO_LOAD_GAIN_RPM_PER_RAW: f32 = 9254.0 / 16384.0; + + /// 空载转速上限（rpm，转子）。 + pub const NO_LOAD_MAX_RPM: f32 = 9254.0; + + #[cfg(test)] + mod tests { + use super::*; + use crate::common::clamp_current; + + #[test] + fn gear_ratio_matches_the_manual() { + assert!((GEAR_RATIO - 19.203).abs() < 5e-3, "{GEAR_RATIO}"); + } + + #[test] + fn c620_saturation_uses_the_c620_scale() { + assert_eq!(C620.max_current_raw(), 16384); + assert_eq!(clamp_current(C620, 30_000), 16384); + assert_eq!(clamp_current(C620, -30_000), -16384); + } + + #[test] + fn no_load_gain_is_the_inverse_of_the_documented_feedforward() { + // 台架文档里的前馈 ≈ 1.77 raw/rpm 正是 1 / 空载增益。 + let feedforward = 1.0 / NO_LOAD_GAIN_RPM_PER_RAW; + assert!((feedforward - 1.77).abs() < 0.02, "{feedforward}"); + } + } +} + +pub mod frame { + //! 协议帧 ID 常量与帧类型再导出。 + //! + //! 帧类型（[`CanFrame`]）由 L1 的 [`can`] 提供；本模块只保留各协议族的 + //! **ID 约定**，因此"改 ID 常量"与"改帧结构"是两件互不影响的事。 + //! + //! - C620/C610 电流协议族：本模块的常量 + [`crate::common`]； + //! - GM6020 电压协议族：见 [`crate::gm6020`]（ID 常量在那边）； + //! - DETA10 IMU：见 [`crate::delta10`]。 + + pub use can::CanFrame; + + /// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。 + pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200; + /// 控制帧 ID：电调 5–8 的电流命令。 + pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF; + /// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。 + pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200; +} + +pub mod id { + //! 编译期电机 ID 校验（ARCHITECTURE.md §5.2）。 + //! + //! 电调 ID 写错是实车上最常见的低级错误之一：ID 9 不会报错，只会**静默** + //! 收不到反馈。把 ID 放进 const 泛型后，"ID 必须在 1–8"变成**编译期**约束， + //! 接错线之前先接错类型。 + //! + //! ```ignore + //! // 编译通过 + //! let id = MotorId::<3>::NEW; + //! // 编译失败：Motor ID must be 1-8 + //! let bad = MotorId::<9>::NEW; + //! ``` + + /// 合法的电调 ID 范围。 + pub const MOTOR_ID_RANGE: core::ops::RangeInclusive = 1..=8; + + /// 编译期已校验的电调 ID。 + /// + /// 只能通过 [`MotorId::NEW`] 构造；该常量的求值会断言 ID 在 1–8 之间， + /// 越界即编译失败。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)] + pub struct MotorId; + + impl MotorId { + /// 编译期校验并构造。 + /// + /// # Panics + /// + /// `ID` 不在 `1..=8` 时在**编译期** panic（const 求值失败即编译错误）。 + pub const NEW: Self = { + assert!(ID >= 1 && ID <= 8, "Motor ID must be 1-8"); + Self + }; + + /// 取出 ID 数值。 + #[must_use] + pub const fn get(&self) -> u8 { + ID + } + + /// 所属控制帧 ID（**C620/C610 电流协议族**：主组 `0x200` / 副组 `0x1FF`）。 + /// + /// GM6020 的电压协议族用的是**另一张表**（`0x1FF` / `0x2FF`），见 + /// [`crate::gm6020::command_frame_id`]。两者不可混用。 + #[must_use] + pub const fn command_frame_id(&self) -> u32 { + if ID <= 4 { + crate::frame::COMMAND_FRAME_ID_PRIMARY + } else { + crate::frame::COMMAND_FRAME_ID_SECONDARY + } + } + + /// 组内槽位（0..4），即控制帧里第几个 i16。 + #[must_use] + pub const fn slot(&self) -> usize { + if ID <= 4 { (ID - 1) as usize } else { (ID - 5) as usize } + } + + /// 反馈帧 ID（`0x200 + ID`）。 + #[must_use] + pub const fn feedback_frame_id(&self) -> u32 { + crate::frame::FEEDBACK_FRAME_ID_BASE + ID as u32 + } + } + + /// 运行期已知 ID 的校验（用于读配置 / 解析命令行）。 + /// + /// # Errors + /// + /// ID 不在 `1..=8` 时返回原值，便于报错信息带上用户输入。 + pub const fn validate_motor_id(id: u8) -> Result { + if id >= 1 && id <= 8 { Ok(id) } else { Err(id) } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn primary_and_secondary_groups_split_correctly() { + assert_eq!(MotorId::<1>::NEW.command_frame_id(), 0x200); + assert_eq!(MotorId::<4>::NEW.command_frame_id(), 0x200); + assert_eq!(MotorId::<5>::NEW.command_frame_id(), 0x1FF); + assert_eq!(MotorId::<8>::NEW.command_frame_id(), 0x1FF); + + assert_eq!(MotorId::<1>::NEW.slot(), 0); + assert_eq!(MotorId::<4>::NEW.slot(), 3); + assert_eq!(MotorId::<5>::NEW.slot(), 0); + assert_eq!(MotorId::<8>::NEW.slot(), 3); + } + + #[test] + fn feedback_ids_follow_0x200_plus_id() { + assert_eq!(MotorId::<1>::NEW.feedback_frame_id(), 0x201); + assert_eq!(MotorId::<8>::NEW.feedback_frame_id(), 0x208); + assert_eq!(MotorId::<3>::NEW.get(), 3); + } + + #[test] + fn runtime_validation_returns_the_offending_id() { + assert_eq!(validate_motor_id(1), Ok(1)); + assert_eq!(validate_motor_id(8), Ok(8)); + assert_eq!(validate_motor_id(0), Err(0)); + assert_eq!(validate_motor_id(9), Err(9)); + } + } +} + +pub mod turn { + //! 多圈累计：单圈编码器原始角 → 连续圈数。 + + /// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。 + pub const ENCODER_TICKS: u16 = 8192; + + /// 半圈阈值：帧间角变化超过它视为发生回绕。 + const HALF_TURN_TICKS: i32 = 4096; + + /// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。 + /// + /// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约 + /// 9300 rpm ≈ 0.155 圈/ms，余量充足。 + #[derive(Debug, Clone, Default)] + pub struct TurnTracker { + revolutions: i32, + prev_raw: Option, + last_angle_rev: f32, + } + + impl TurnTracker { + /// 创建跟踪器。 + #[must_use] + pub const fn new() -> Self { + Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 } + } + + /// 送入新的原始角，返回连续累计角（圈）。 + pub fn update(&mut self, raw: u16) -> f32 { + if let Some(prev) = self.prev_raw { + let delta = i32::from(raw) - i32::from(prev); + if delta > HALF_TURN_TICKS { + // 正向跨越 8191 → 0。 + self.revolutions -= 1; + } else if delta < -HALF_TURN_TICKS { + // 反向跨越 0 → 8191。 + self.revolutions += 1; + } + } + self.prev_raw = Some(raw); + self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0; + self.last_angle_rev + } + + /// 最近一次 [`TurnTracker::update`] 的返回值。 + #[must_use] + pub const fn last_angle_rev(&self) -> f32 { + self.last_angle_rev + } + + /// 清零回到初始状态。 + pub fn reset(&mut self) { + *self = Self::new(); + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn first_reading_is_raw_fraction() { + let mut t = TurnTracker::new(); + assert!((t.update(4096) - 0.5).abs() < 1e-4); + } + + #[test] + fn forward_wrap_counts_up() { + let mut t = TurnTracker::new(); + t.update(8000); + let after = t.update(100); // 8000 → 8191 → 0 → 100 + assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}"); + } + + #[test] + fn reverse_wrap_counts_down() { + let mut t = TurnTracker::new(); + t.update(100); + let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向） + assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}"); + } + + #[test] + fn small_deltas_do_not_wrap() { + let mut t = TurnTracker::new(); + t.update(0); + assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4); + assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4); + } + + #[test] + fn multi_turn_forward_then_reverse_returns() { + let mut t = TurnTracker::new(); + // 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。 + for _ in 0..3 { + t.update(0); + t.update(4000); + t.update(8000); + t.update(100); + } + let fwd = t.last_angle_rev(); + let expected_fwd = 3.0 + 100.0 / 8192.0; + assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}"); + + // 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。 + for _ in 0..3 { + t.update(8000); + t.update(4000); + t.update(0); + } + let back = t.last_angle_rev(); + assert!(back.abs() < 1e-4, "should return to 0, got {back}"); + } + + #[test] + fn jitter_at_boundary_stays_put() { + // 0 附近抖动不产生虚假圈数。 + let mut t = TurnTracker::new(); + t.update(8191); + assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次 + assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4); + assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回 + assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4); + } + + #[test] + fn reset_forgets_history() { + let mut t = TurnTracker::new(); + t.update(8000); + t.update(100); + t.reset(); + assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits()); + assert!((t.update(4096) - 0.5).abs() < 1e-4); + } + } +} + +pub use common::{ + DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id, + decode_feedback, encode_current_frame, encode_feedback_frame, feedback_frame_id, +}; +pub use frame::*; +pub use gm6020::{Gm6020Command, Gm6020Feedback}; +pub use id::*; +pub use m2006::C610; +pub use m3508::C620; +pub use turn::*; diff --git a/crates/motor/Cargo.toml b/crates/motor/Cargo.toml new file mode 100644 index 0000000..a9e2a6f --- /dev/null +++ b/crates/motor/Cargo.toml @@ -0,0 +1,18 @@ +[package] +name = "motor" +description = "Protocol-independent motor device contract and registry" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +motor-protocol = { workspace = true } +thiserror = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/motor/src/lib.rs b/crates/motor/src/lib.rs new file mode 100644 index 0000000..539acac --- /dev/null +++ b/crates/motor/src/lib.rs @@ -0,0 +1,964 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! Protocol-independent motor device contracts. + +pub mod traits { + //! 能力面 trait：小而正交，按硬件事实拆分。 + //! + //! 消费者只声明自己用到的能力（静态分发，无 `dyn`）： + //! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`] + //! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]） + //! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`] + //! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`] + + use std::time::Duration; + + use crate::{Command, MotorState}; + + /// 读反馈。所有可观察设备的最小面。 + pub trait MotorFeedback: Send + Sync { + /// 控制器内的电机 ID（对 DJI 电调即电调 ID）。 + fn id(&self) -> u8; + + /// 最新反馈快照。 + fn state(&self) -> MotorState; + + /// 距最近一帧反馈的时长；`None` 表示从未收到。 + /// + /// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`] + /// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。 + fn feedback_age(&self) -> Option; + } + + /// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。 + pub trait EffortDrive: Send + Sync { + /// 直接下发开环 raw（协议量程，物理方向）。 + fn set_effort_raw(&self, raw: i16); + } + + /// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。 + pub trait VelocityDrive: Send + Sync { + /// 设定目标转子转速。 + fn set_velocity(&self, rotor_rpm: f32); + } + + /// 位置目标（转子多圈，圈；物理方向）。 + pub trait PositionDrive: Send + Sync { + /// 设定目标转子多圈位置。 + fn set_position(&self, rotor_rev: f32); + } + + /// 停用（零输出）。 + pub trait Disable: Send + Sync { + /// 停用电机。 + fn disable(&self); + } + + /// 便利别名（blanket impl，零成本）。实现方**不必**实现它。 + pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {} + + impl Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {} + + /// One motor's data for one control-policy evaluation. + #[derive(Debug, Clone, Copy)] + pub struct PolicySample { + /// ESC identifier. + pub id: u8, + /// Latest direction-corrected physical feedback. + pub state: MotorState, + /// Latest command lease value. + pub command: Command, + /// Whether fresh feedback is available. + pub online: bool, + } + + /// Snapshot passed to an external effort policy once per bus tick. + #[derive(Debug, Clone, Copy)] + pub struct PolicyInput { + /// Samples indexed by ESC id minus one. + pub samples: [PolicySample; 8], + /// Measured control period in seconds. + pub dt: f32, + } + + /// Computes raw physical effort from motor commands and feedback. + /// + /// `esc-bus` owns transport and safety but deliberately has no control-law + /// dependency. Control crates implement this trait and the application composes + /// the two sides. + pub trait EffortPolicy: Send + 'static { + /// Return physical effort raw values indexed by ESC id minus one. + fn step(&mut self, input: &PolicyInput) -> [i16; 8]; + /// Clear controller state after an offline event or command-mode change. + fn reset(&mut self, esc_id: u8); + } +} + +pub mod state { + //! 电机反馈快照（协议无关，方向修正后的物理量）。 + //! + //! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`] + //! 或上层完成，不在此泄漏底盘语义。 + + /// 电机反馈快照。 + #[derive(Debug, Clone, Copy, PartialEq, Default)] + pub struct MotorState { + /// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。 + pub rotor_angle_raw: u16, + /// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。 + pub rotor_revolutions: f32, + /// 转子转速（rpm，物理方向）。 + pub rotor_speed_rpm: f32, + /// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。 + pub effort_raw: i16, + /// 电机温度（℃）。 + pub temperature_c: i8, + /// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。 + pub feedback_count: u64, + } + + impl MotorState { + /// 是否已收到过反馈（上电后首帧门控）。 + #[must_use] + pub fn has_feedback(&self) -> bool { + self.feedback_count > 0 + } + } +} + +pub mod command { + //! 驱动层命令寄存器（最后写入胜出）。 + + /// 发给驱动层的电机命令。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub enum Command { + /// 停用：目标清零，下发 0 输出。 + Disabled, + /// 直接下发开环 raw（协议量程，物理方向），绕过闭环。 + /// ESC = 电流 raw；GM6020 = 电压 raw。 + EffortRaw(i16), + /// 速度环跟踪目标转子转速（rpm，物理方向）。 + Velocity(f32), + /// 位置环跟踪目标转子多圈位置（圈，物理方向）。 + Position(f32), + } + + /// 命令模式标签（不含设定值）——用于切换时复位控制器。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum CommandMode { + /// 停用。 + Disabled, + /// 开环 raw。 + EffortRaw, + /// 速度环。 + Velocity, + /// 位置环。 + Position, + } + + impl Command { + /// 模式标签。 + #[must_use] + pub const fn mode(self) -> CommandMode { + match self { + Self::Disabled => CommandMode::Disabled, + Self::EffortRaw(_) => CommandMode::EffortRaw, + Self::Velocity(_) => CommandMode::Velocity, + Self::Position(_) => CommandMode::Position, + } + } + } +} + +pub mod gearbox { + //! 输出轴单位适配器：减速比换算。 + //! + //! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。 + //! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。 + + use std::time::Duration; + + use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive}; + + /// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。 + pub struct OutputShaft { + inner: T, + gear_ratio: f32, + } + + impl OutputShaft { + /// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。 + /// + /// # Panics + /// + /// `gear_ratio` 非有限或 ≤ 0 时 panic。 + #[must_use] + pub fn new(inner: T, gear_ratio: f32) -> Self { + assert!( + gear_ratio.is_finite() && gear_ratio > 0.0, + "gear_ratio must be > 0, got {gear_ratio}" + ); + Self { inner, gear_ratio } + } + + /// 减速比。 + #[must_use] + pub fn gear_ratio(&self) -> f32 { + self.gear_ratio + } + + /// 底层电机引用。 + #[must_use] + pub fn inner(&self) -> &T { + &self.inner + } + + /// 转子量 → 输出轴量。 + #[must_use] + pub fn to_output(&self, rotor: f32) -> f32 { + rotor / self.gear_ratio + } + + /// 输出轴量 → 转子量。 + #[must_use] + pub fn to_rotor(&self, output: f32) -> f32 { + output * self.gear_ratio + } + + /// 由反馈快照推算输出轴转速（rpm）。 + #[must_use] + pub fn output_speed_rpm(&self, state: &MotorState) -> f32 { + state.rotor_speed_rpm / self.gear_ratio + } + } + + impl MotorFeedback for OutputShaft { + fn id(&self) -> u8 { + self.inner.id() + } + + fn state(&self) -> MotorState { + // 只透传转子量；不伪造输出轴字段。 + self.inner.state() + } + + fn feedback_age(&self) -> Option { + self.inner.feedback_age() + } + } + + impl VelocityDrive for OutputShaft { + /// `output_rpm` 为**输出轴**转速。 + fn set_velocity(&self, output_rpm: f32) { + self.inner.set_velocity(self.to_rotor(output_rpm)); + } + } + + impl EffortDrive for OutputShaft { + fn set_effort_raw(&self, raw: i16) { + self.inner.set_effort_raw(raw); + } + } + + impl PositionDrive for OutputShaft { + /// `output_rev` 为**输出轴**圈数。 + fn set_position(&self, output_rev: f32) { + self.inner.set_position(self.to_rotor(output_rev)); + } + } + + impl Disable for OutputShaft { + fn disable(&self) { + self.inner.disable(); + } + } + + #[cfg(test)] + mod tests { + use super::*; + use crate::Command; + + struct Stub { + cmd: std::sync::Mutex, + } + + impl Stub { + fn new() -> Self { + Self { cmd: std::sync::Mutex::new(Command::Disabled) } + } + } + + impl MotorFeedback for Stub { + fn id(&self) -> u8 { + 1 + } + 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) { + *self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm); + } + } + impl EffortDrive for Stub { + fn set_effort_raw(&self, raw: i16) { + *self.cmd.lock().unwrap() = Command::EffortRaw(raw); + } + } + impl PositionDrive for Stub { + fn set_position(&self, rotor_rev: f32) { + *self.cmd.lock().unwrap() = Command::Position(rotor_rev); + } + } + impl Disable for Stub { + fn disable(&self) { + *self.cmd.lock().unwrap() = Command::Disabled; + } + } + + #[test] + fn velocity_scales_by_gear() { + let shaft = OutputShaft::new(Stub::new(), 19.203); + shaft.set_velocity(100.0); + match *shaft.inner().cmd.lock().unwrap() { + Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"), + ref c => panic!("unexpected {c:?}"), + } + } + + #[test] + fn position_scales_by_gear() { + let shaft = OutputShaft::new(Stub::new(), 36.0); + shaft.set_position(2.0); + match *shaft.inner().cmd.lock().unwrap() { + Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"), + ref c => panic!("unexpected {c:?}"), + } + } + + #[test] + fn effort_passes_through() { + let shaft = OutputShaft::new(Stub::new(), 10.0); + shaft.set_effort_raw(-1234); + assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234)); + } + + #[test] + #[should_panic = "gear_ratio"] + fn rejects_nonpositive_gear() { + let _ = OutputShaft::new(Stub::new(), 0.0); + } + } +} + +pub mod heading { + //! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。 + //! + //! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。 + //! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。 + + use std::future::Future; + + /// 绝对航向观测源。 + pub trait HeadingSource: Send { + /// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。 + fn heading_rad(&self) -> impl Future + Send; + } + + /// 云台目标角的参考系。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum HeadingFrame { + /// 电机转子多圈（上电相对）。 + RotorMultiTurn, + /// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。 + AbsoluteField, + } +} + +pub mod registry { + //! 电机注册表：**按总线 + 反馈帧 ID** 找回设备（ARCHITECTURE.md §5.4）。 + //! + //! # 为什么需要它 + //! + //! 单总线底盘上，`EscBus` 自己就能把反馈帧分发给对应的 `EscMotor`。但整机是 + //! **双 CAN**（§2.1：底盘 `can0` + 功能总线 `can1`），云台 / 拨弹的帧与底盘的 + //! 帧可能落在**相同的 ID** 上（GM6020 与 C620 都可能是 0x2xx）。此时"这帧是谁的" + //! 只能由 `(总线, 帧 ID)` 共同决定。 + //! + //! [`MotorRegistry`] 就是这张映射表：注册时校验冲突，分发时 O(1) 命中。 + //! 当前 `chassisd` 的单总线底盘路径不经过它；它是云台 / 拨弹接入功能总线时的 + //! seam（先有接口与测试，避免将来把分发逻辑硬编码进每个任务）。 + + use can::CanFrame; + use motor_protocol::feedback_frame_id; + + /// 物理总线标识（ARCHITECTURE.md §2.1 的双 CAN 拓扑）。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)] + pub enum BusId { + /// 底盘总线（C620/C610 驱动轮）。 + Chassis, + /// 功能总线（云台 GM6020 + 拨弹 M2006/C610）。 + Function, + } + + impl BusId { + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Chassis => "chassis", + Self::Function => "function", + } + } + } + + /// 注册表条目。 + #[derive(Debug, Clone, PartialEq, Eq)] + pub struct RegisteredMotor { + /// 所属总线。 + pub bus: BusId, + /// 反馈帧 ID。 + pub frame_id: u32, + /// 轮名 / 设备名（配置里的 `name`）。 + pub name: String, + } + + /// 注册错误。 + /// + /// 手写 `Display` / `Error` 而不是 `derive(thiserror::Error)`：注册表属于 + /// **总是可用**的设备抽象（关掉 `bus` feature 也要能编译），不应为一个派生宏 + /// 引入可选依赖。 + #[derive(Debug, Clone, PartialEq, Eq)] + pub enum RegistryError { + /// 同一 `(总线, 帧 ID)` 被注册两次——几乎总是接线或配置写重了。 + Duplicate { + /// 总线。 + bus: &'static str, + /// 冲突的帧 ID。 + frame_id: u32, + /// 先注册的名字。 + existing: String, + /// 后注册的名字。 + name: String, + }, + /// 电调 ID 越界（1–8）。 + BadEscId(u8), + } + + impl std::fmt::Display for RegistryError { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + Self::Duplicate { bus, frame_id, existing, name } => write!( + f, + "总线 {bus} 上的帧 ID 0x{frame_id:03X} 已被 '{existing}' 占用，无法再注册 '{name}'" + ), + Self::BadEscId(id) => write!(f, "电调 ID {id} 越界（合法范围 1–8）"), + } + } + } + + impl std::error::Error for RegistryError {} + + /// 设备注册表。 + #[derive(Debug, Default, Clone)] + pub struct MotorRegistry { + entries: Vec, + } + + impl MotorRegistry { + /// 空表。 + #[must_use] + pub fn new() -> Self { + Self::default() + } + + /// 按电调 ID 注册（反馈帧 ID = `0x200 + 电调ID`，C620/C610 约定）。 + /// + /// # Errors + /// + /// 电调 ID 越界或该帧 ID 已被占用时返回错误。 + pub fn register_esc( + &mut self, + bus: BusId, + esc_id: u8, + name: impl Into, + ) -> Result<(), RegistryError> { + let frame_id = feedback_frame_id(esc_id).ok_or(RegistryError::BadEscId(esc_id))?; + self.register_frame(bus, frame_id, name) + } + + /// 按反馈帧 ID 注册（GM6020 或非 1–8 的 ID 用它）。 + /// + /// # Errors + /// + /// 该 `(总线, 帧 ID)` 已被占用时返回错误。 + pub fn register_frame( + &mut self, + bus: BusId, + frame_id: u32, + name: impl Into, + ) -> Result<(), RegistryError> { + let name = name.into(); + if let Some(existing) = self.resolve(bus, frame_id) { + return Err(RegistryError::Duplicate { + bus: bus.name(), + frame_id, + existing: existing.name.clone(), + name, + }); + } + self.entries.push(RegisteredMotor { bus, frame_id, name }); + Ok(()) + } + + /// 按 `(总线, 帧 ID)` 查找。 + #[must_use] + pub fn resolve(&self, bus: BusId, frame_id: u32) -> Option<&RegisteredMotor> { + self.entries.iter().find(|entry| entry.bus == bus && entry.frame_id == frame_id) + } + + /// 从一帧反馈找出它的归属设备（ARCHITECTURE.md §5.4 的 `update_from_frame`）。 + /// + /// 返回 `None` 表示这帧不属于任何已注册设备（例如 IMU 帧，或没配的电机）。 + #[must_use] + pub fn dispatch(&self, bus: BusId, frame: &CanFrame) -> Option<&RegisteredMotor> { + self.resolve(bus, frame.id) + } + + /// 已注册设备数。 + #[must_use] + pub fn len(&self) -> usize { + self.entries.len() + } + + /// 是否为空。 + #[must_use] + pub fn is_empty(&self) -> bool { + self.entries.is_empty() + } + + /// 全部条目（按注册顺序）。 + #[must_use] + pub fn entries(&self) -> &[RegisteredMotor] { + &self.entries + } + + /// 某条总线上的设备名。 + #[must_use] + pub fn names_on(&self, bus: BusId) -> Vec<&str> { + self.entries + .iter() + .filter(|entry| entry.bus == bus) + .map(|entry| entry.name.as_str()) + .collect() + } + } + + /// 底盘四轮的注册顺序（与 `mecanum` 的 FL/FR/RL/RR 一致）。 + pub const CHASSIS_WHEEL_NAMES: [&str; 4] = + ["front_left", "front_right", "rear_left", "rear_right"]; + + /// 把底盘四轮按顺序注册进表（电调 ID 从 1 开始连续）。 + /// + /// # Errors + /// + /// 任一电调 ID 越界或冲突时返回错误。 + pub fn register_chassis_wheels( + registry: &mut MotorRegistry, + esc_ids: [u8; 4], + ) -> Result<(), RegistryError> { + for (name, esc_id) in CHASSIS_WHEEL_NAMES.iter().zip(esc_ids) { + registry.register_esc(BusId::Chassis, esc_id, *name)?; + } + Ok(()) + } + + #[cfg(test)] + mod tests { + use super::*; + use motor_protocol::{MAX_ESC_ID, MIN_ESC_ID}; + + #[test] + fn registers_and_dispatches_chassis_wheels() { + let mut registry = MotorRegistry::new(); + register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap(); + assert_eq!(registry.len(), 4); + assert_eq!(registry.names_on(BusId::Chassis).len(), 4); + assert!(registry.names_on(BusId::Function).is_empty()); + + let frame = CanFrame::new(0x203, [0; 8]); + let found = registry.dispatch(BusId::Chassis, &frame).unwrap(); + assert_eq!(found.name, "rear_left", "0x203 = 电调 3 = 后左"); + } + + #[test] + fn same_frame_id_on_another_bus_is_not_a_conflict() { + let mut registry = MotorRegistry::new(); + registry.register_esc(BusId::Chassis, 1, "front_left").unwrap(); + // 功能总线上的 GM6020 ID 1 反馈同样是 0x205 —— 不同总线，互不冲突。 + registry.register_frame(BusId::Function, 0x205, "gimbal_yaw").unwrap(); + assert_eq!(registry.len(), 2); + assert_eq!(registry.resolve(BusId::Chassis, 0x205), None); + assert_eq!(registry.resolve(BusId::Function, 0x205).unwrap().name, "gimbal_yaw"); + } + + #[test] + fn duplicates_on_the_same_bus_are_rejected_with_both_names() { + let mut registry = MotorRegistry::new(); + registry.register_esc(BusId::Chassis, 2, "front_right").unwrap(); + let error = registry.register_esc(BusId::Chassis, 2, "typo_front_right").unwrap_err(); + let text = error.to_string(); + assert!(text.contains("front_right"), "{text}"); + assert!(text.contains("typo_front_right"), "{text}"); + assert_eq!(registry.len(), 1, "失败的注册不得写入"); + } + + #[test] + fn out_of_range_esc_ids_are_rejected() { + let mut registry = MotorRegistry::new(); + assert_eq!( + registry.register_esc(BusId::Chassis, 0, "ghost"), + Err(RegistryError::BadEscId(0)) + ); + assert_eq!( + registry.register_esc(BusId::Chassis, 9, "ghost"), + Err(RegistryError::BadEscId(9)) + ); + assert!(registry.is_empty()); + assert!(feedback_frame_id(MIN_ESC_ID).is_some()); + assert!(feedback_frame_id(MAX_ESC_ID).is_some()); + } + + #[test] + fn unknown_frames_dispatch_to_nothing() { + let mut registry = MotorRegistry::new(); + register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap(); + // DETA10 的 IMU 帧不是任何电机。 + let imu = CanFrame::new(0x100, [0; 8]); + assert!(registry.dispatch(BusId::Chassis, &imu).is_none()); + } + } +} + +pub mod testing { + //! 测试替身：内存电机与航向源。 + + use std::future::Future; + use std::sync::{Arc, Mutex, MutexGuard}; + use std::time::{Duration, Instant}; + + use crate::{ + Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive, + VelocityDrive, + }; + + /// 锁辅助：中毒锁直接恢复内部数据。 + fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { + mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) + } + + /// 内存电机：记录命令、可注入状态。供上层单测，无需总线。 + /// + /// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。 + #[derive(Clone)] + 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))), + 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)); + } + + /// 最近一次命令。 + #[must_use] + pub fn last_command(&self) -> Command { + lock(&self.published).1 + } + } + + impl MotorFeedback for MockMotor { + fn id(&self) -> u8 { + self.id + } + 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 { + fn set_velocity(&self, rotor_rpm: f32) { + lock(&self.published).1 = Command::Velocity(rotor_rpm); + } + } + + impl EffortDrive for MockMotor { + fn set_effort_raw(&self, raw: i16) { + lock(&self.published).1 = Command::EffortRaw(raw); + } + } + + impl PositionDrive for MockMotor { + fn set_position(&self, rotor_rev: f32) { + lock(&self.published).1 = Command::Position(rotor_rev); + } + } + + impl Disable for MockMotor { + fn disable(&self) { + lock(&self.published).1 = Command::Disabled; + } + } + + /// 恒定航向测试源。 + pub struct MockHeading { + rad: f32, + } + + impl MockHeading { + /// 创建固定航向源。 + #[must_use] + pub const fn new(rad: f32) -> Self { + Self { rad } + } + } + + impl HeadingSource for MockHeading { + fn heading_rad(&self) -> impl Future + Send { + std::future::ready(self.rad) + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn mock_motor_records_commands() { + let m = MockMotor::new(3); + m.set_velocity(1000.0); + assert_eq!(m.last_command(), Command::Velocity(1000.0)); + m.disable(); + assert_eq!(m.last_command(), Command::Disabled); + } + + #[test] + 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))); + } + } +} + +pub mod motor_common { + //! 电机种类：把"型号"变成可在设备层与配置层之间传递的事实。 + //! + //! 设备层不关心电调怎么接、齿轮多大，但必须知道**协议族**：电流环在电调内部， + //! 而不同协议族的控制字含义不同（电流 vs 电压）。[`MotorKind`] 就是这张表。 + + use motor_protocol::EscModel; + + /// 支持的电机型号。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] + pub enum MotorKind { + /// GM3508 + C620（底盘驱动轮）。 + Gm3508, + /// M2006 + C610（拨弹 / 小负载）。 + M2006, + /// GM6020（云台直驱，**电压**控制）。 + Gm6020, + } + + impl MotorKind { + /// 配套电调型号。 + #[must_use] + pub const fn esc_model(self) -> EscModel { + match self { + // GM6020 的反馈标度与 C620 相同，但控制字是电压。 + Self::Gm3508 | Self::Gm6020 => EscModel::C620, + Self::M2006 => EscModel::C610, + } + } + + /// 控制量是否是**电压**（而非电流）。 + /// + /// 总线驱动据此选择组帧方式：C620/C610 走 `encode_current_frame`， + /// GM6020 走 `motor_protocol::motor::gm6020::encode_voltage_frame`。 + #[must_use] + pub const fn encodes_voltage(self) -> bool { + matches!(self, Self::Gm6020) + } + + /// 该型号是否与其它型号**共用**电流控制帧（C620/C610 共用 `0x200`/`0x1FF`）。 + #[must_use] + pub const fn shares_current_frames(self) -> bool { + !self.encodes_voltage() + } + + /// 默认减速比（转子 : 输出轴）。接硬件前必须按实车核对。 + #[must_use] + pub const fn default_gear_ratio(self) -> f32 { + match self { + Self::Gm3508 => 3591.0 / 187.0, + // M2006 常见配置 36:1；与 ARCHITECTURE.md §8.2 的 motors.toml 一致。 + Self::M2006 => 36.0, + // 云台直驱。 + Self::Gm6020 => 1.0, + } + } + + /// 型号名（配置与日志用）。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Gm3508 => "GM3508", + Self::M2006 => "M2006", + Self::Gm6020 => "GM6020", + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn only_gm6020_encodes_voltage() { + assert!(MotorKind::Gm6020.encodes_voltage()); + assert!(!MotorKind::Gm3508.encodes_voltage()); + assert!(!MotorKind::M2006.encodes_voltage()); + assert!(MotorKind::Gm3508.shares_current_frames()); + assert!(!MotorKind::Gm6020.shares_current_frames()); + } + + #[test] + fn esc_models_match_the_scale_documentation() { + assert_eq!(MotorKind::Gm3508.esc_model(), EscModel::C620); + assert_eq!(MotorKind::Gm6020.esc_model(), EscModel::C620); + assert_eq!(MotorKind::M2006.esc_model(), EscModel::C610); + assert_eq!(MotorKind::M2006.esc_model().max_current_raw(), 10_000); + } + + #[test] + fn default_gear_ratios_are_sane() { + assert!((MotorKind::Gm3508.default_gear_ratio() - 19.203).abs() < 5e-3); + assert!((MotorKind::Gm6020.default_gear_ratio() - 1.0).abs() < 1e-6); + assert!(MotorKind::M2006.default_gear_ratio() > 0.0); + assert_eq!(MotorKind::Gm6020.name(), "GM6020"); + } + } +} + +pub mod motor_gm6020 { + //! GM6020 设备（云台直驱，**电压**控制）。 + //! + //! # 当前状态（务必读） + //! + //! 协议层的电压帧编解码已经可用（[`motor_protocol::gm6020`]），但**总线驱动 + //! `EscBus` 目前只实现 C620/C610 的电流路径**：它把 `set_effort_raw` 直接组 + //! 成电流帧。把 GM6020 接上云台还需要总线驱动增加"电压帧"分支 + //! （[`MotorKind::encodes_voltage`] 就是给那条分支准备的判据），这属于云台 + //! 部分的工作（ARCHITECTURE.md §5.7 `gimbal` 任务）。 + //! + //! 在此之前，本模块提供的是**型号事实**（协议族、减速比、ID 映射）， + //! 不含"已经能驱动 GM6020"的暗示。 + + use crate::MotorKind; + + /// 该型号的 [`MotorKind`]。 + pub const KIND: MotorKind = MotorKind::Gm6020; + + /// 云台直驱的减速比。 + pub const GEAR_RATIO: f32 = 1.0; + + /// 该型号的控制量是电压而非电流。 + pub const ENCODES_VOLTAGE: bool = true; + + /// 总线驱动是否已支持该型号的驱动路径。 + /// + /// 现在返回 `false`：电压帧分支随云台工作落地。用它做**运行期**判定， + /// 而不是让配置静默接受一个驱动不了的型号。 + #[must_use] + pub const fn drive_path_available() -> bool { + false + } + + #[cfg(test)] + mod tests { + use super::*; + use motor_protocol::gm6020::{COMMAND_FRAME_ID_PRIMARY, feedback_frame_id}; + + #[test] + fn kind_facts_match_the_protocol_module() { + assert_eq!(KIND, MotorKind::Gm6020); + assert!(KIND.encodes_voltage()); + assert!((KIND.default_gear_ratio() - GEAR_RATIO).abs() < 1e-6); + // 两个常量同源：`ENCODES_VOLTAGE` 就是 `KIND.encodes_voltage()` 的镜像。 + assert_eq!(ENCODES_VOLTAGE, KIND.encodes_voltage()); + } + + #[test] + fn gm6020_uses_its_own_frame_ids() { + assert_eq!(COMMAND_FRAME_ID_PRIMARY, 0x1FF); + assert_eq!(feedback_frame_id(1), Some(0x205)); + // 与 C620 不同：C620 的 ID 1 控制帧是 0x200。 + assert_ne!(COMMAND_FRAME_ID_PRIMARY, motor_protocol::COMMAND_FRAME_ID_PRIMARY); + } + + #[test] + fn drive_path_is_honestly_reported_as_unavailable() { + assert!(!drive_path_available(), "云台电压路径落地后本断言才应改为 true"); + } + } +} + +pub use command::*; +pub use gearbox::*; +pub use heading::*; +pub use motor_common::MotorKind; +pub use registry::*; +pub use state::*; +pub use testing::*; +pub use traits::*; diff --git a/crates/motor-core/Cargo.toml b/crates/pid/Cargo.toml similarity index 56% rename from crates/motor-core/Cargo.toml rename to crates/pid/Cargo.toml index 7daf5f3..5be517d 100644 --- a/crates/motor-core/Cargo.toml +++ b/crates/pid/Cargo.toml @@ -1,12 +1,16 @@ [package] -name = "motor-core" +name = "pid" +description = "PID controller" version.workspace = true edition.workspace = true rust-version.workspace = true license.workspace = true -description = "Protocol-agnostic motor abstraction: Motor trait, feedback state, and PID controllers" [lints] workspace = true [dependencies] + + +[dev-dependencies] +approx = { workspace = true } diff --git a/crates/motor-core/src/pid.rs b/crates/pid/src/lib.rs similarity index 97% rename from crates/motor-core/src/pid.rs rename to crates/pid/src/lib.rs index 19adac2..8a29e76 100644 --- a/crates/motor-core/src/pid.rs +++ b/crates/pid/src/lib.rs @@ -1,3 +1,5 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + //! 通用 PID 控制器。 /// 并联式 PID 控制器（f32，显式时间步）。 @@ -102,6 +104,12 @@ impl Pid { self.last_output = 0.0; } + /// 上一次 `update` 的输出（诊断 / 串级外环取速度目标用）。 + #[must_use] + pub fn last_output(&self) -> f32 { + self.last_output + } + /// 当前积分累加项。 #[must_use] pub fn integral(&self) -> f32 { diff --git a/crates/dji-esc-sim/Cargo.toml b/crates/sim-harness/Cargo.toml similarity index 52% rename from crates/dji-esc-sim/Cargo.toml rename to crates/sim-harness/Cargo.toml index cf82802..7a5ea32 100644 --- a/crates/dji-esc-sim/Cargo.toml +++ b/crates/sim-harness/Cargo.toml @@ -1,6 +1,6 @@ [package] -name = "dji-esc-sim" -description = "C620/C610 plant simulation: friction/delay model, scenario harness, and async plant rig" +name = "sim-harness" +description = "Closed-loop motor simulation harness and fake ESC rig" version.workspace = true edition.workspace = true rust-version.workspace = true @@ -10,9 +10,12 @@ license.workspace = true workspace = true [dependencies] -can-link = { workspace = true } -motor-core = { workspace = true } -dji-esc = { workspace = true } +can = { workspace = true } +motor = { workspace = true } +motor-protocol = { workspace = true } +motor-control = { workspace = true } +motor-model = { workspace = true } +experiment = { workspace = true } tokio = { workspace = true } thiserror = { workspace = true } tracing = { workspace = true } @@ -20,3 +23,4 @@ rand = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } +approx = { workspace = true } diff --git a/crates/sim-harness/src/lib.rs b/crates/sim-harness/src/lib.rs new file mode 100644 index 0000000..6736864 --- /dev/null +++ b/crates/sim-harness/src/lib.rs @@ -0,0 +1,1066 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod bus_model { + //! CAN 总线模型：延迟与丢帧（ARCHITECTURE.md §5.6）。 + //! + //! 为什么需要它：`driver crates` 里已经有两个真实教训（见 `docs/chassis.md` §6）—— + //! 命令队列积压会让速度环凭空多出上百毫秒延迟，反馈丢帧会让积分项被噪声牵着走。 + //! 这些现象在"理想通道"上是测不出来的，因此这里把它们变成**可复现的实验条件**。 + //! + //! 用法：把 [`BusModel`] 包在一条 `CanLink` 上，它就变成一个"会延迟、会丢帧的 + //! 总线"——`EscBus` 与假电调都察觉不到区别。 + + use std::time::Duration; + + use can::{CanBus, CanError, CanFrame}; + use tokio::sync::mpsc; + + /// 总线损伤模型。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct BusModel { + /// 命令方向（下行）的固定延迟（毫秒）。 + pub command_delay_ms: f32, + /// 反馈方向（上行）的固定延迟（毫秒）。 + pub feedback_delay_ms: f32, + /// 反馈丢帧概率（0..=1）。命令**不丢**：丢一条命令等于少一次续租， + /// 语义上应该由命令租约而不是丢帧来建模。 + pub feedback_drop_pct: f32, + } + + impl Default for BusModel { + fn default() -> Self { + // 默认无损伤：显式设置才有影响，避免"忘了配置"悄悄改变实验条件。 + Self { command_delay_ms: 0.0, feedback_delay_ms: 0.0, feedback_drop_pct: 0.0 } + } + } + + impl BusModel { + /// 台架实测量级：slcan 一次 write 约 2 ms。 + #[must_use] + pub const fn slcan_like() -> Self { + Self { command_delay_ms: 2.0, feedback_delay_ms: 1.0, feedback_drop_pct: 0.0 } + } + + /// 反馈延迟。 + #[must_use] + pub fn feedback_delay(&self) -> Duration { + Duration::from_secs_f32((self.feedback_delay_ms / 1000.0).max(0.0)) + } + + /// 命令延迟。 + #[must_use] + pub fn command_delay(&self) -> Duration { + Duration::from_secs_f32((self.command_delay_ms / 1000.0).max(0.0)) + } + } + + /// 判断第 `index` 条反馈是否应被丢弃（确定性：只依赖下标与概率阈值）。 + /// + /// 用确定性哈希而不是随机数，是为了让"带丢帧的仿真"同 seed 逐位可复现。 + #[must_use] + pub fn should_drop(index: u64, drop_pct: f32) -> bool { + if !drop_pct.is_finite() || drop_pct <= 0.0 { + return false; + } + if drop_pct >= 1.0 { + return true; + } + // splitmix64 的低位混合，够用且无依赖。 + let mut x = index.wrapping_add(0x9E37_79B9_7F4A_7C15); + x = (x ^ (x >> 30)).wrapping_mul(0xBF58_476D_1CE4_E5B9); + x = (x ^ (x >> 27)).wrapping_mul(0x94D0_49BB_1331_11EB); + x ^= x >> 31; + let unit = (x >> 11) as f64 / (1u64 << 53) as f64; + unit < f64::from(drop_pct) + } + + /// 会延迟 / 丢帧的总线端点（实现 [`CanBus`]，可替换真实链路）。 + #[derive(Debug)] + pub struct DelayedBus { + /// 下行：本端发出的帧。 + outgoing_tx: mpsc::Sender, + /// 上行：来自总线的帧。 + incoming_rx: mpsc::Receiver, + /// 损伤模型。 + model: BusModel, + /// 已观察的反馈帧计数（丢帧判定用）。 + feedback_index: u64, + /// 被丢弃的反馈帧数（诊断）。 + dropped: u64, + } + + impl DelayedBus { + /// 由一条链路与损伤模型组装。 + #[must_use] + pub fn new(link: can::CanLink, model: BusModel) -> Self { + Self { + outgoing_tx: link.outgoing_tx, + incoming_rx: link.incoming_rx, + model, + feedback_index: 0, + dropped: 0, + } + } + + /// 已丢弃的反馈帧数。 + #[must_use] + pub const fn dropped(&self) -> u64 { + self.dropped + } + + /// 损伤模型。 + #[must_use] + pub const fn model(&self) -> BusModel { + self.model + } + } + + impl CanBus for DelayedBus { + type Error = CanError; + + async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> { + if frame.dlc > 8 { + return Err(CanError::BadDlc(frame.dlc)); + } + if self.model.command_delay_ms > 0.0 { + tokio::time::sleep(self.model.command_delay()).await; + } + self.outgoing_tx.send(*frame).await.map_err(|_| CanError::Closed) + } + + async fn recv(&mut self) -> Result { + loop { + let frame = self.incoming_rx.recv().await.ok_or(CanError::Closed)?; + self.feedback_index += 1; + if should_drop(self.feedback_index, self.model.feedback_drop_pct) { + self.dropped += 1; + continue; + } + if self.model.feedback_delay_ms > 0.0 { + tokio::time::sleep(self.model.feedback_delay()).await; + } + return Ok(frame); + } + } + + fn try_recv(&mut self) -> Result, Self::Error> { + loop { + match self.incoming_rx.try_recv() { + Ok(frame) => { + self.feedback_index += 1; + if should_drop(self.feedback_index, self.model.feedback_drop_pct) { + self.dropped += 1; + continue; + } + return Ok(Some(frame)); + } + Err(mpsc::error::TryRecvError::Empty) => return Ok(None), + Err(mpsc::error::TryRecvError::Disconnected) => return Err(CanError::Closed), + } + } + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn default_model_is_lossless() { + let model = BusModel::default(); + assert_eq!(model.feedback_delay(), Duration::ZERO); + assert!(!should_drop(0, 0.0)); + for index in 0..100 { + assert!(!should_drop(index, model.feedback_drop_pct)); + } + } + + #[test] + fn slcan_like_model_has_the_measured_magnitude() { + let model = BusModel::slcan_like(); + assert_eq!(model.command_delay(), Duration::from_millis(2)); + assert_eq!(model.feedback_delay(), Duration::from_millis(1)); + assert!(model.feedback_drop_pct.abs() < f32::EPSILON); + } + + #[test] + fn drop_probability_is_deterministic_and_bounded() { + // 确定性：同一下标永远同一结论（仿真可复现的前提）。 + for index in [0u64, 1, 7, 12345, u64::MAX] { + assert_eq!(should_drop(index, 0.3), should_drop(index, 0.3)); + } + // 概率上界行为。 + assert!(!should_drop(999, 0.0)); + assert!(should_drop(999, 1.0)); + // 30% 附近的实际比例应落在合理区间（不是 0 也不是 100%）。 + let dropped = (0..10_000).filter(|i| should_drop(*i, 0.3)).count(); + assert!((2500..3500).contains(&dropped), "dropped = {dropped}"); + } + + #[tokio::test(start_paused = true)] + async fn delayed_bus_sends_and_receives() { + let (a, b) = can::CanLink::pair(8); + let mut bus = DelayedBus::new(a, BusModel::slcan_like()); + assert_eq!(bus.model(), BusModel::slcan_like()); + bus.send(&CanFrame::new(0x200, [1; 8])).await.unwrap(); + let mut peer = b; + let received = peer.incoming_rx.recv().await.unwrap(); + assert_eq!(received.id, 0x200); + + peer.outgoing_tx.send(CanFrame::new(0x201, [2; 8])).await.unwrap(); + let frame = bus.recv().await.unwrap(); + assert_eq!(frame.id, 0x201); + assert_eq!(bus.dropped(), 0); + } + + #[tokio::test(start_paused = true)] + async fn dropping_model_discards_feedback() { + let (a, b) = can::CanLink::pair(64); + let model = BusModel { feedback_drop_pct: 0.5, ..BusModel::default() }; + let mut bus = DelayedBus::new(a, model); + let peer = b; + for i in 0..10u8 { + peer.outgoing_tx.send(CanFrame::new(0x201, [i; 8])).await.unwrap(); + } + // 连读 5 帧，必然经历被丢弃的那些。 + for _ in 0..5 { + assert!(bus.recv().await.is_ok()); + } + assert!(bus.dropped() > 0, "50% 丢帧率下应当真的丢了帧"); + } + } +} + +pub mod ensemble { + //! 被控对象集成：用同一组参数的**抖动样本**表达模型不确定性。 + //! + //! 单点模型整定出来的增益往往对增益 / 时间常数 / 延迟 / 摩擦高度敏感 + //! （真车轮子之间、电量与温度变化都会漂移）。整定应针对一个**集成**做 + //! 鲁棒优化，而不是针对一条理想曲线做最优化。 + + use rand::Rng as _; + use rand::SeedableRng as _; + use rand::rngs::StdRng; + + use motor_model::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)); + } + } +} + +pub mod harness { + //! 离线场景评估：同步、无 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 motor_control::{VelocityController, VelocityGains}; + use motor_protocol::{EscModel, clamp_current}; + use thiserror::Error; + + use crate::ensemble::PlantEnsemble; + use experiment::Scenario; + use experiment::Trace; + use motor_model::{PlantMotor, PlantParams}; + + /// 闭环限幅与电调型号（决定 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 experiment::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(), + "摩擦 + 延迟只会拖慢跟随" + ); + } + } +} + +pub mod plant { + //! [`SimPlant`]：把 `sim-harness` 的电机模型适配成 [`ControlPlant`] + //! （ARCHITECTURE.md §5.6）。 + //! + //! 这是"仿真与实机共用同一套 PID"得以成立的那一步： + //! `motor-control` 的整定 / 控制律只认识 `ControlPlant`，而这里提供离线实现， + //! `bench` 的台架采样提供真机实现，两者可以评估同一组增益。 + + use motor_control::ControlPlant; + use motor_protocol::{EscFeedback, clamp_current}; + + use crate::harness::LoopLimits; + use motor_model::{PlantMotor, PlantParams}; + + /// 把被测电机的**转速**（rpm）当作输出、**effort raw** 当作输入的被控对象。 + /// + /// 命令的钳位方式与 [`crate::harness::simulate`] 完全一致（先按 `current_limit` + /// 夹、再按电调满量程夹），否则"仿真整定"与"闭环重放"会对同一组增益给出 + /// 不同的饱和行为。 + #[derive(Debug, Clone)] + pub struct SimPlant { + motor: PlantMotor, + limits: LoopLimits, + steps: u64, + } + + impl SimPlant { + /// 由参数构造（默认限幅与 [`LoopLimits::gm3508`] 一致）。 + #[must_use] + pub fn new(params: PlantParams) -> Self { + Self { motor: PlantMotor::new(params), limits: LoopLimits::gm3508(), steps: 0 } + } + + /// 换限幅与电调型号。 + #[must_use] + pub fn with_limits(mut self, limits: LoopLimits) -> Self { + self.limits = limits; + self + } + + /// 台架量级模型的 plant（整定默认）。 + #[must_use] + pub fn bench() -> Self { + Self::new(PlantParams::gm3508_bench()) + } + + /// 理想模型的 plant（历史闭环行为）。 + #[must_use] + pub fn ideal() -> Self { + Self::new(PlantParams::gm3508()) + } + + /// 当前转速（rpm，电调侧）。 + #[must_use] + pub fn speed_rpm(&self) -> f32 { + self.motor.speed_rpm() + } + + /// 已推进的拍数。 + #[must_use] + pub const fn steps(&self) -> u64 { + self.steps + } + + /// 当前限幅。 + #[must_use] + pub const fn limits(&self) -> LoopLimits { + self.limits + } + + /// 原始反馈（与真实反馈帧同构）。 + #[must_use] + pub fn feedback(&self) -> EscFeedback { + EscFeedback { + angle_raw: self.motor.angle_raw() as u16, + speed_rpm: self.motor.speed_rpm() as i16, + current_raw: 0, + temperature_c: 25, + } + } + } + + impl ControlPlant for SimPlant { + /// 仿真不会失败：模型内部已经把状态夹住，数值发散由 + /// [`crate::harness::simulate`] 的界检查负责（那条路径要产出错误码）。 + type Error = motor_control::Infallible; + + fn step(&mut self, input: f32, dt: f32) -> Result { + if dt > 0.0 && input.is_finite() { + // 与 harness 相同的物理帧路径：f32 → i16 → 双重量程钳位。 + let raw = input as i16; + let command = clamp_current( + self.limits.model, + raw.clamp(-self.limits.current_limit_raw, self.limits.current_limit_raw), + ); + let feedback = self.motor.step(command, dt); + self.steps += 1; + return Ok(f32::from(feedback.speed_rpm)); + } + Ok(self.motor.speed_rpm()) + } + + fn reset(&mut self) { + self.motor.reset(); + self.steps = 0; + } + + fn describe(&self) -> String { + let params = self.motor.params(); + format!( + "sim-plant(gain={:.4} rpm/raw, tau={}s, delay={}ticks, static={} raw, coulomb={} raw, limit={} raw, model={:?})", + params.gain_rpm_per_raw, + params.tau_s, + params.delay_ticks, + params.static_raw, + params.coulomb_raw, + self.limits.current_limit_raw, + self.limits.model + ) + } + } + + #[cfg(test)] + mod tests { + use super::*; + use motor_protocol::EscModel; + + #[test] + fn bench_plant_does_not_move_below_static_friction() { + let mut plant = SimPlant::bench(); + let static_raw = plant.motor.params().static_raw; + for _ in 0..1000 { + plant.step(static_raw * 0.5, 0.001).unwrap(); + } + assert!(plant.speed_rpm().abs() < 1e-6, "低于静摩擦不应产生运动（stick）"); + assert_eq!(plant.steps(), 1000); + } + + #[test] + fn bench_plant_turns_under_enough_effort() { + let mut plant = SimPlant::bench(); + for _ in 0..2000 { + plant.step(8000.0, 0.001).unwrap(); + } + assert!(plant.speed_rpm() > 100.0, "{}", plant.speed_rpm()); + } + + #[test] + fn ideal_plant_tracks_effort_at_steady_state() { + let mut plant = SimPlant::ideal(); + for _ in 0..5000 { + plant.step(1000.0, 0.001).unwrap(); + } + let expected = 1000.0 * plant.motor.params().gain_rpm_per_raw; + let error = (plant.speed_rpm() - expected).abs() / expected; + assert!(error < 0.05, "稳态 {} vs 期望 {expected}", plant.speed_rpm()); + } + + #[test] + fn command_is_clamped_by_the_configured_limit() { + let limits = LoopLimits { current_limit_raw: 500, model: EscModel::C620 }; + let mut plant = SimPlant::bench().with_limits(limits); + // 远超限幅的输入：转速不应超过"限幅对应的稳态转速"太多。 + for _ in 0..3000 { + plant.step(30_000.0, 0.001).unwrap(); + } + let ceiling = 500.0 * plant.motor.params().gain_rpm_per_raw * 1.2; + assert!(plant.speed_rpm() <= ceiling, "{} > {ceiling}", plant.speed_rpm()); + assert_eq!(plant.limits(), limits); + } + + #[test] + fn reset_returns_to_standstill() { + let mut plant = SimPlant::bench(); + for _ in 0..1000 { + plant.step(9000.0, 0.001).unwrap(); + } + assert!(plant.speed_rpm() != 0.0); + plant.reset(); + assert!((plant.speed_rpm()).abs() < 1e-6); + assert_eq!(plant.steps(), 0); + } + + #[test] + fn bad_inputs_do_not_advance() { + let mut plant = SimPlant::bench(); + assert!((plant.step(1000.0, 0.0).unwrap()).abs() < 1e-6); + assert!((plant.step(f32::NAN, 0.001).unwrap()).abs() < 1e-6); + assert_eq!(plant.steps(), 0); + } + + #[test] + fn feedback_mirrors_the_model_state() { + let mut plant = SimPlant::ideal(); + for _ in 0..2000 { + plant.step(2000.0, 0.001).unwrap(); + } + let feedback = plant.feedback(); + assert!(feedback.speed_rpm > 0); + assert_eq!(feedback.temperature_c, 25); + assert!((plant.speed_rpm() - f32::from(feedback.speed_rpm)).abs() < 1.0); + } + + #[test] + fn describe_mentions_the_physics() { + let text = SimPlant::bench().describe(); + assert!(text.contains("delay=2ticks"), "{text}"); + assert!(text.contains("limit=12000"), "{text}"); + assert!(SimPlant::ideal().describe().contains("delay=0ticks")); + } + } +} + +pub mod rig { + //! 异步 plant rig：把被控对象挂到一对 [`CanLink`] 通道上。 + //! + //! 与真实电调对调位置的"假电调"：消费命令帧（`0x200` / `0x1FF`），按 + //! `tick` 节拍产出反馈帧。`EscBus` 完全看不出区别，因此 + //! **协议 → 驱动 → 控制回路**的整条链路都能在无硬件环境下跑通 + //! （`chassisd --mock`、闭环集成测试）。 + + use std::collections::HashMap; + use std::time::Duration; + + use can::{CanFrame, CanLink}; + use motor_protocol::{ + COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, encode_feedback_frame, + }; + use tokio::task::JoinHandle; + use tokio::time::{MissedTickBehavior, interval}; + use tracing::trace; + + use motor_model::{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 motor_protocol::{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(); + } + } +} + +pub use bus_model::*; +pub use ensemble::*; +pub use harness::*; +pub use plant::*; +pub use rig::*; diff --git a/crates/dji-esc/Cargo.toml b/crates/slcan/Cargo.toml similarity index 52% rename from crates/dji-esc/Cargo.toml rename to crates/slcan/Cargo.toml index 2d0ce0e..246f7d7 100644 --- a/crates/dji-esc/Cargo.toml +++ b/crates/slcan/Cargo.toml @@ -1,20 +1,21 @@ [package] -name = "dji-esc" +name = "slcan" +description = "SLCAN serial transport bridge" version.workspace = true edition.workspace = true rust-version.workspace = true license.workspace = true -description = "DJI RoboMaster ESC (C620/C610) CAN protocol: codec, multi-turn tracking, and async bus driver" [lints] workspace = true [dependencies] -motor-core = { workspace = true } -can-link = { workspace = true } +can = { workspace = true } tokio = { workspace = true } thiserror = { workspace = true } tracing = { workspace = true } +slcan-proto = { workspace = true } +serial = { workspace = true } [dev-dependencies] -tokio = { workspace = true, features = ["test-util"] } +tokio = { workspace = true } diff --git a/crates/dji-esc-slcan/src/lib.rs b/crates/slcan/src/lib.rs similarity index 98% rename from crates/dji-esc-slcan/src/lib.rs rename to crates/slcan/src/lib.rs index 370bc43..84df050 100644 --- a/crates/dji-esc-slcan/src/lib.rs +++ b/crates/slcan/src/lib.rs @@ -1,8 +1,8 @@ -//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can_link::CanLink`]。 +//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can::CanLink`]。 //! //! 基于 [`slcan`] crate（Lawicel ASCII 协议）：`S8` = 1 Mbps（C620 标准），`O` 打开通道。 //! 阻塞串口 I/O 跑在专用线程上，与 SocketCAN 桥一样只暴露通道端点——对本 crate -//! 的调用方而言，`dji-esc` 看到的是一个普通 [`CanLink`]，与传输介质无关。 +//! 的调用方（`motor / esc-bus`）而言，看到的是一个普通 [`CanLink`]，与传输介质无关。 #![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] use std::io; @@ -11,9 +11,10 @@ use std::sync::atomic::{AtomicU64, Ordering}; use std::thread; use std::time::{Duration, Instant}; -use can_link::{CanFrame as LinkFrame, CanLink}; +use can::{CanFrame as LinkFrame, CanLink}; use serial::prelude::*; use slcan::{BitRate, CanSocket}; +use slcan_proto as slcan; use tokio::sync::mpsc; use tracing::{debug, error, info, warn}; diff --git a/crates/socketcan/Cargo.toml b/crates/socketcan/Cargo.toml new file mode 100644 index 0000000..d2a40ff --- /dev/null +++ b/crates/socketcan/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "socketcan" +description = "SocketCAN transport bridge" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +can = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } + +[dev-dependencies] +esc-bus = { workspace = true } +motor = { workspace = true } +motor-control = { workspace = true } +tokio = { workspace = true } +motor-protocol = { workspace = true } + +[target.'cfg(target_os = "linux")'.dependencies] +socketcan-rs = { workspace = true } diff --git a/crates/socketcan/src/lib.rs b/crates/socketcan/src/lib.rs new file mode 100644 index 0000000..ef7caf5 --- /dev/null +++ b/crates/socketcan/src/lib.rs @@ -0,0 +1,122 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! 真实总线桥接实现（Linux）。 + +#[cfg(target_os = "linux")] +mod imp { + use std::io; + use std::sync::Arc; + + use can::{CanFrame as LinkFrame, CanLink}; + use socketcan_rs::{ + CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, + SocketOptions as _, StandardId, tokio::CanSocket, + }; + use tokio::sync::mpsc; + use tracing::{debug, error}; + + /// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。 + const CHANNEL_BOUND: usize = 512; + + /// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。 + /// + /// 内部： + /// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208， + /// 非反馈帧再由协议层解码拒绝，双保险）； + /// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。 + /// + /// # Errors + /// + /// 接口不存在 / 权限不足等 `io::Error`。 + pub fn open_link(interface: &str) -> io::Result { + let socket = CanSocket::open(interface)?; + socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?; + let socket = Arc::new(socket); + + let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND); + let CanLink { outgoing_tx, incoming_rx } = bridge_side; + + tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx)); + tokio::spawn(tx_bridge(socket, incoming_rx)); + Ok(bus_side) + } + + /// socket → 通道：真实总线反馈进入协议 bus。 + async fn rx_bridge(socket: Arc, tx: mpsc::Sender) { + loop { + match socket.read_frame().await { + Ok(frame) => { + if let Some(frame) = to_link_frame(&frame) { + if tx.send(frame).await.is_err() { + debug!("bus side closed, rx bridge exiting"); + return; + } + } + } + Err(e) => { + // 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程， + // 由 systemd 拉起重连。 + error!(error = %e, "CAN read failed, rx bridge exiting"); + return; + } + } + } + } + + /// 通道 → socket：协议 bus 命令写入真实总线。 + async fn tx_bridge(socket: Arc, mut rx: mpsc::Receiver) { + while let Some(frame) = rx.recv().await { + match to_socketcan_frame(&frame) { + Some(sc_frame) => { + if let Err(e) = socket.write_frame(sc_frame).await { + error!(error = %e, "CAN write failed, tx bridge exiting"); + return; + } + } + None => { + debug!(id = frame.id, "drop frame with non-standard CAN id"); + } + } + } + debug!("bus side closed, tx bridge exiting"); + } + + /// socketcan 帧 → 链路帧（仅 8 字节数据帧）。 + fn to_link_frame(frame: &ScFrame) -> Option { + match frame { + ScFrame::Data(df) if df.data().len() == 8 => { + Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?)) + } + _ => None, + } + } + + /// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。 + fn to_socketcan_frame(frame: &LinkFrame) -> Option { + let id = StandardId::new(frame.id as u16)?; + let df = CanDataFrame::new(id, &frame.data)?; + Some(ScFrame::Data(df)) + } +} + +#[cfg(target_os = "linux")] +pub use imp::*; + +#[cfg(not(target_os = "linux"))] +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct Unsupported; + +#[cfg(not(target_os = "linux"))] +impl std::fmt::Display for Unsupported { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + f.write_str("SocketCAN is only available on Linux") + } +} + +#[cfg(not(target_os = "linux"))] +impl std::error::Error for Unsupported {} + +#[cfg(not(target_os = "linux"))] +pub fn open_link(_interface: &str) -> Result { + Err(Unsupported) +} diff --git a/crates/dji-esc-socketcan/tests/vcan.rs b/crates/socketcan/tests/vcan.rs similarity index 83% rename from crates/dji-esc-socketcan/tests/vcan.rs rename to crates/socketcan/tests/vcan.rs index 8d2bf8f..a031fc2 100644 --- a/crates/dji-esc-socketcan/tests/vcan.rs +++ b/crates/socketcan/tests/vcan.rs @@ -7,10 +7,11 @@ use std::time::Duration; -use dji_esc::{CanFrame, CanLink}; -use motor_core::Motor; -use socketcan::tokio::CanSocket; -use socketcan::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId}; +use can::{CanFrame, CanLink}; +use motor::{MotorFeedback, VelocityDrive}; +use motor_control::{MotorPolicy, VelocityGains}; +use socketcan_rs::tokio::CanSocket; +use socketcan_rs::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId}; use tokio::time::timeout; const IFACE: &str = "vcan0"; @@ -22,7 +23,7 @@ fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame { #[tokio::test] #[ignore = "requires vcan0 (see `just vcan-test`)"] async fn bridge_relays_command_to_bus() { - let link = dji_esc_socketcan::open_link(IFACE).unwrap(); + let link = socketcan::open_link(IFACE).unwrap(); let observer = CanSocket::open(IFACE).unwrap(); let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]); @@ -44,7 +45,7 @@ async fn bridge_relays_command_to_bus() { #[tokio::test] #[ignore = "requires vcan0 (see `just vcan-test`)"] async fn bridge_relays_feedback_from_bus() { - let CanLink { outgoing_tx: _, mut incoming_rx } = dji_esc_socketcan::open_link(IFACE).unwrap(); + let CanLink { outgoing_tx: _, mut incoming_rx } = socketcan::open_link(IFACE).unwrap(); let simulator = CanSocket::open(IFACE).unwrap(); // 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。 @@ -63,15 +64,14 @@ async fn bridge_relays_feedback_from_bus() { #[ignore = "requires vcan0 (see `just vcan-test`)"] async fn esc_bus_end_to_end_over_vcan() { // EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。 - let link = dji_esc_socketcan::open_link(IFACE).unwrap(); + let link = socketcan::open_link(IFACE).unwrap(); let mut esc = EscSim::new(IFACE, &[1]); - let bus = dji_esc::EscBus::spawn( - link, - vec![dji_esc::EscConfig::gm3508("m1", 1, 1.0)], - dji_esc::BusOptions::default(), - ) - .unwrap(); + let config = esc_bus::EscConfig::gm3508("m1", 1, 1.0); + let mut policy = MotorPolicy::new(); + policy.configure(1, config.current_limit, VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0)); + let bus = + esc_bus::EscBus::spawn(link, vec![config], esc_bus::BusOptions::default(), policy).unwrap(); bus.motor(1).unwrap().set_velocity(2000.0); // 仿真 2s：模拟电调按命令回反馈，速度环应收敛。 diff --git a/crates/spin/Cargo.toml b/crates/spin/Cargo.toml new file mode 100644 index 0000000..992aca4 --- /dev/null +++ b/crates/spin/Cargo.toml @@ -0,0 +1,27 @@ +[package] +name = "spin" +description = "Chassis and single-motor hardware bench tool" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +bench = { workspace = true } +motor = { workspace = true } +esc-bus = { workspace = true } +experiment = { workspace = true } +motor-control = { workspace = true } +mecanum = { workspace = true } +slcan = { workspace = true } +socketcan = { workspace = true } +tokio = { workspace = true } +clap = { workspace = true } +tracing = { workspace = true } +tracing-subscriber = { workspace = true } + +[dev-dependencies] +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/spin/src/main.rs b/crates/spin/src/main.rs new file mode 100644 index 0000000..51572e5 --- /dev/null +++ b/crates/spin/src/main.rs @@ -0,0 +1,722 @@ +mod chassis { + //! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 500 rpm）。 + //! + //! ```sh + //! # T153 板载 CANable2（slcan 固件）→ /dev/ttyACM0，四轮 500 rpm 转子，缓升 1s 保持 10s + //! cargo run --release -p chassis-bench --bin chassis-spin -- \ + //! --device /dev/ttyACM0 --rpm 500 + //! + //! # 只验证链路与反馈，不发任何非零电流（接线排查用） + //! cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run + //! ``` + //! + //! # 抽象边界 + //! + //! 本工具是**组合根**：`dji-esc-slcan`（链路桥接）→ `dji-esc`（C620 协议 + + //! 速度环）→ `motor-core`（`VelocityDrive` / `MotorFeedback` / `Disable` 能力 + //! trait）。激励场景与安全包络来自同 crate 的 `chassis-bench` 库，与 + //! `chassis-tune` 的真机后端**共用同一套实现**。 + //! + //! # 命令租约（重要） + //! + //! `EscBus` 的命令是**租约**：`command_timeout`（默认 50ms）内没有重新下发命令， + //! 驱动会强制 0 电流。所以控制循环必须**逐拍重发**同一个设定值——本工具在 1kHz + //! 节拍里重发（与 `chassisd` 的控制循环一致）。只在开头下发一次是**不行的**。 + //! + //! # 安全 + //! + //! 轮子必须架空或整车固定，急停可及。默认低限流、有限时长、ctrl-c 任意时刻 + //! 安全停机（先 0 电流再关总线）；未收到全部反馈前绝不下发速度。 + + use std::process::ExitCode; + use std::time::Duration; + + use bench::{SafetyEnvelope, SafetyMonitor}; + use clap::{Parser, ValueEnum}; + use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor}; + use experiment::Scenario; + use motor::{Disable, MotorFeedback, VelocityDrive}; + use motor_control::{MotorPolicy, VelocityGains}; + use slcan::{CanSpeed, SlcanOptions, open_link_with}; + use tokio::time::{Instant, MissedTickBehavior, interval, sleep}; + + /// 稳态判据：平均转子转速与目标的相对误差上限。 + const TOLERANCE: f32 = 0.05; + /// 状态打印周期。 + const REPORT_PERIOD: Duration = Duration::from_millis(200); + /// 反馈新鲜度阈值：`feedback_age` 超过它即视为该电调掉线。 + const FRESH: Duration = Duration::from_millis(100); + /// 过速需要持续这么久才判定为失控并停机（避免单帧噪声误触发）。 + const OVERSPEED_GRACE: Duration = Duration::from_millis(60); + /// 控制节拍（C620 期望 1kHz 命令）。 + const CONTROL_PERIOD: Duration = Duration::from_millis(1); + /// 四轮默认名（顺序固定 FL、FR、RL、RR）。 + const WHEEL_NAMES: [&str; 4] = ["front_left", "front_right", "rear_left", "rear_right"]; + + #[derive(Debug, Parser)] + #[command(name = "chassis-spin", version, about = "台架：四轮同时旋转（默认 500 rpm 转子）")] + struct Args { + /// slcan 串口设备（T153 板载 CANable2 为 /dev/ttyACM0）。 + #[arg(long, default_value = "/dev/ttyACM0")] + device: String, + + /// 串口波特率（CDC ACM 一般忽略；CH340 类适配器需与固件一致）。 + #[arg(long, default_value_t = 2_000_000)] + serial_baud: u32, + + /// 串口超时（毫秒）。读写共用：`serial` 的 `TTYPort::write` 也按它 poll(POLLOUT)。 + #[arg(long, default_value_t = 50)] + serial_timeout_ms: u64, + + /// CAN 波特率档位（C620 标准 = 1Mbps）。 + #[arg(long, value_enum, default_value_t = CanSpeedArg::M1)] + can_speed: CanSpeedArg, + + /// 电调 ID 列表，顺序 = FL、FR、RL、RR。 + #[arg(long, value_delimiter = ',', default_value = "1,2,3,4")] + esc_ids: Vec, + + /// 各轮安装方向 ±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 + }) + .collect() + } + + fn control_policy(&self) -> MotorPolicy { + let mut policy = MotorPolicy::new(); + let gains = VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward); + for id in &self.esc_ids { + policy.configure(*id, self.current_limit, gains); + } + policy + } + } + + fn bus_options() -> BusOptions { + BusOptions { + period: CONTROL_PERIOD, + feedback_timeout: Duration::from_millis(100), + // 命令租约 50ms；控制循环每 1ms 续租。 + command_timeout: Duration::from_millis(50), + } + } + + pub async fn main_with_args(parameters: Vec) -> ExitCode { + init_tracing(); + let args = + Args::parse_from(std::iter::once(String::from("spin-chassis")).chain(parameters)); + if let Err(problem) = args.validate() { + eprintln!("✗ 参数错误：{problem}"); + return ExitCode::FAILURE; + } + print_banner(&args); + + match run(&args).await { + Ok(true) => ExitCode::SUCCESS, + Ok(false) => ExitCode::FAILURE, + Err(e) => { + eprintln!("✗ 运行失败：{e}"); + ExitCode::FAILURE + } + } + } + + /// 初始化日志：链路桥的读写失败/静默告警必须可见， + /// 否则现场只剩一句"没有反馈"，无法区分适配器故障与整车接线问题。 + fn init_tracing() { + let filter = tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| { + tracing_subscriber::EnvFilter::new("warn,slcan=debug,socketcan=debug") + }); + let _ = tracing_subscriber::fmt().with_env_filter(filter).try_init(); + } + + fn print_banner(args: &Args) { + println!("=== 台架：四轮同时旋转（转子目标 {:.1} rpm）===", args.rpm); + println!( + "设备 {} | CAN {:?} | 电调 {:?} | 方向 {:?} | 减速比 {:.3}", + args.device, args.can_speed, args.esc_ids, args.directions, args.gear_ratio + ); + println!( + "转子 {:.1} rpm ÷ {:.3} = 轮端 {:.2} rpm | 电流限幅 {} raw | 缓升 {}ms | 保持 {:.1}s{}", + args.rpm, + args.gear_ratio, + args.rpm / args.gear_ratio, + args.current_limit, + args.ramp_ms, + args.hold_secs, + if args.dry_run { + " | dry-run" + } else if args.allow_partial { + " | allow-partial" + } else { + "" + } + ); + if args.dry_run { + println!("ℹ dry-run：只读反馈，不发出非零电流。\n"); + } else { + println!( + "速度环 kp={} ki={} kd={} ff={} | 过速保护 {:.0} rpm", + args.kp, + args.ki, + args.kd, + args.feedforward, + args.overspeed_limit() + ); + println!("⚠ 轮子必须架空或整车固定，急停可及；ctrl-c 随时安全停机。\n"); + } + } + + /// 打开链路 → 装配协议 bus → 执行 → **任何路径都先停电流再关总线**。 + async fn run(args: &Args) -> Result> { + let options = SlcanOptions { + serial_baud: args.serial_baud, + speed: args.can_speed.into(), + // 读写共用该超时（serial crate 的 TTYPort::write 也先 poll(POLLOUT)）： + // 设太小会把"这一拍写不出去"误判成写失败。总线繁忙时读立即返回， + // 放大它只影响总线完全静默时的下行延迟。 + read_timeout: Duration::from_millis(args.serial_timeout_ms), + }; + let link = open_link_with(&args.device, options).map_err(|e| { + format!("打开 slcan 设备 {} 失败：{e}（检查设备路径 / 权限 / 是否被占用）", args.device) + })?; + println!("✓ slcan 已打开：{}（CAN {:?}）", args.device, args.can_speed); + + let bus = EscBus::spawn(link, args.esc_configs(), bus_options(), args.control_policy())?; + let outcome = drive(args, &bus).await; + + // 安全收尾：先 0 电流并让它至少发出一拍，再关总线。 + for motor in bus.motors() { + motor.disable(); + } + sleep(Duration::from_millis(50)).await; + bus.shutdown().await; + println!("已停机（0 电流）并关闭总线"); + outcome + } + + async fn drive( + args: &Args, + bus: &EscBusHandle, + ) -> 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 + } +} + +mod motor_one { + //! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。 + //! + //! ```sh + //! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs] + //! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒 + //! cargo run --release -p bench --bin socketcan-spin -- can0 1 1000 10 + //! ``` + //! + //! 关键点在 `spin_one`：它只依赖 `motor::Motor` 抽象，完全不知道 + //! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码 + //! 原样可用。抽象（`can / imu`）、协议实现（`motor-protocol`）、设备层（`motor / esc-bus`）与总线后端（`driver crates`）在此汇合。 + //! + //! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。 + + #[cfg(target_os = "linux")] + use std::time::Duration; + + #[cfg(target_os = "linux")] + use esc_bus::{BusOptions, EscBus, EscConfig}; + #[cfg(target_os = "linux")] + use motor::Motor; + #[cfg(target_os = "linux")] + use motor_control::{MotorPolicy, VelocityGains}; + #[cfg(target_os = "linux")] + use socketcan::open_link; + #[cfg(target_os = "linux")] + use tokio::time::{MissedTickBehavior, interval}; + + #[cfg(target_os = "linux")] + pub async fn main_with_args(argv: Vec) { + let mut args = argv.into_iter(); + let interface = args.next().unwrap_or_else(|| "can0".into()); + let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1); + let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0); + let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0); + + println!("=== 单电机抽象验证 ==="); + println!( + "接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s" + ); + + let link = open_link(&interface).unwrap_or_else(|e| { + eprintln!( + "打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）" + ); + std::process::exit(1); + }); + let config = EscConfig::gm3508("spin_test", esc_id, 1.0); + println!( + "电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}", + config.model, config.gear_ratio, config.current_limit + ); + + let mut policy = MotorPolicy::new(); + policy.configure(esc_id, config.current_limit, VelocityGains::new(10.0, 0.5, 0.0, 1.5)); + let bus = EscBus::spawn(link, vec![config], BusOptions::default(), policy) + .expect("EscBus 配置非法"); + let motor = bus.motor(esc_id).expect("刚配置的电机必然存在"); + + spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await; + + bus.shutdown().await; + println!("已退出"); + } + + /// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。 + /// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。 + /// + /// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发 + /// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值 + /// （真实底盘的控制循环每 1ms 重发一次）。 + #[cfg(target_os = "linux")] + async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) { + const LEASE_PERIOD: Duration = Duration::from_millis(10); + const REPORT_PERIOD: Duration = Duration::from_millis(100); + + motor.set_velocity(target_rpm); + println!("已下发目标转速，开始观测（100ms 周期）……\n"); + + let mut lease = interval(LEASE_PERIOD); + lease.set_missed_tick_behavior(MissedTickBehavior::Skip); + let mut report = interval(REPORT_PERIOD); + report.set_missed_tick_behavior(MissedTickBehavior::Skip); + let deadline = tokio::time::Instant::now() + duration; + + loop { + tokio::select! { + _ = tokio::signal::ctrl_c() => { + println!("\n收到 ctrl-c"); + break; + } + _ = lease.tick() => motor.set_velocity(target_rpm), // 续租 + _ = report.tick() => { + let st = motor.state(); + if st.has_feedback() { + println!( + "转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧", + st.rotor_speed_rpm, + st.effort_raw, + st.temperature_c, + st.rotor_revolutions, + st.feedback_count + ); + } else { + println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线"); + } + } + } + if tokio::time::Instant::now() >= deadline { + break; + } + } + + motor.disable(); + // 让 0 电流帧至少发出一拍再关总线。 + tokio::time::sleep(Duration::from_millis(50)).await; + println!("已停机（0 电流）"); + } + + #[cfg(not(target_os = "linux"))] + #[allow(clippy::unused_async)] + pub async fn main_with_args(_argv: Vec) { + eprintln!("spin motor 仅支持 Linux（需要 SocketCAN）"); + } +} + +#[tokio::main] +async fn main() -> std::process::ExitCode { + let mut args = std::env::args().skip(1); + match args.next().as_deref() { + Some("chassis") => chassis::main_with_args(args.collect()).await, + Some("motor") => { + motor_one::main_with_args(args.collect()).await; + std::process::ExitCode::SUCCESS + } + Some("--help" | "-h") => { + println!("usage: spin [options]"); + std::process::ExitCode::SUCCESS + } + _ => { + eprintln!("usage: spin [options]"); + std::process::ExitCode::from(2) + } + } +} diff --git a/crates/tuning/Cargo.toml b/crates/tuning/Cargo.toml new file mode 100644 index 0000000..ad1e8b1 --- /dev/null +++ b/crates/tuning/Cargo.toml @@ -0,0 +1,31 @@ +[package] +name = "tuning" +description = "PID tuning algorithms, search, cost, and presets" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[lints] +workspace = true + +[dependencies] +pid = { workspace = true } +motor-control = { workspace = true } +experiment = { workspace = true } +sim-harness = { workspace = true } +serde = { workspace = true } +toml = { workspace = true } +rand = { workspace = true } +rayon = { workspace = true } +argmin = { workspace = true } +argmin-math = { workspace = true } +thiserror = { workspace = true } +indicatif = { workspace = true } +tracing = { workspace = true } +motor-protocol = { workspace = true } +motor-model = { workspace = true } + +[dev-dependencies] +approx = { workspace = true } +proptest = { workspace = true } diff --git a/crates/tuning/src/lib.rs b/crates/tuning/src/lib.rs new file mode 100644 index 0000000..8fd9cc9 --- /dev/null +++ b/crates/tuning/src/lib.rs @@ -0,0 +1,3538 @@ +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +//! PID tuning algorithms, search, scoring, and presets. + +/// Evaluation result shared by search backends. +pub type Evaluated = crate::cost::Score; + +/// Scores one candidate gain set. +pub trait Evaluator: Send + Sync { + /// Evaluate a gain set. + fn evaluate(&self, gains: &motor_control::VelocityGains) -> Evaluated; + /// Describe the experiment for reports. + fn describe(&self) -> String; +} + +pub mod relay { + //! 继电反馈（relay feedback）整定：自动找出临界增益 `Ku` 与临界周期 `Tu` + //! （ARCHITECTURE.md §5.5）。 + //! + //! # 原理 + //! + //! 把控制器换成**继电环节**（输出在 `±amplitude` 之间跳变，带一点滞环抑制 + //! 噪声误触发）。闭环在相频特性 −180° 的频率附近形成极限环： + //! + //! - 振荡**周期**即临界周期 `Tu`； + //! - 振荡**幅值** `a` 与继电幅值 `d` 通过描述函数联系：`Ku = 4d / (π·a)`。 + //! + //! 拿到 `(Ku, Tu)` 后交给 [`super::zn`] 换算成 PID 增益。 + //! + //! # 什么时候这个实验有意义（重要） + //! + //! 描述函数法成立的前提是**线性部分对高次谐波有足够衰减**，也就是相频要真正 + //! 穿过 −180°： + //! + //! - **有延迟 / 三阶以上**的被控对象（真实电调 + 机械、FOPDT）：穿越点唯一， + //! `Tu` 就是物理上的临界周期 ✅ + //! - **纯二阶**对象：相频只**渐近**趋于 −180°，没有有限穿越点，极限环由 + //! 滞环宽度决定（幅值 ≈ 滞环量级、周期远小于 `2π/wn`），此时算出的 `Ku` + //! 偏大 ⚠️ + //! - **纯一阶**对象：同样会振荡（由滞环决定），得到的是"滞环极限环"而非 + //! 相频信息 ⚠️ + //! + //! 结论：把结果当作**起点**而不是终点——台架上用它快速拿到一组可用增益， + //! 再用 `sim-harness` 的搜索式整定 / 阶跃复验确认。这也是 ARCHITECTURE.md §8.1 + //! 的整定流程（`auto_tune` → 阶跃验证 → 上车微调）的用意。 + //! + //! # 用法（与 ARCHITECTURE.md §5.5 / §8.3 的流程一致） + //! + //! ```ignore + //! let mut tuner = RelayTuner::new(amplitude, hysteresis); + //! let mut t = 0.0; + //! while t < max_time { + //! let measured = plant.step(tuner.last_output(), dt)?; + //! let output = tuner.step(measured, dt); + //! t += dt; + //! } + //! let result = tuner.result(); // None = 没形成稳定振荡 + //! ``` + + use crate::zn::{TuneResult, ZieglerNicholsRule}; + + /// 继电环节的当前状态。 + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum RelayState { + /// 输出 `+amplitude`。 + High, + /// 输出 `-amplitude`。 + Low, + } + + impl RelayState { + /// 取反。 + #[must_use] + pub const fn flipped(self) -> Self { + match self { + Self::High => Self::Low, + Self::Low => Self::High, + } + } + + /// 文本名。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::High => "high", + Self::Low => "low", + } + } + } + + /// 一个振荡半周期组合（峰 → 谷 → 峰）的观测。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct OscillationPoint { + /// 观测到的峰值。 + pub peak: f32, + /// 观测到的谷值。 + pub trough: f32, + /// 该次振荡的周期（秒）。 + pub period_s: f32, + } + + impl OscillationPoint { + /// 振荡幅值 `a = (peak − trough) / 2`。 + #[must_use] + pub fn amplitude(&self) -> f32 { + (self.peak - self.trough) / 2.0 + } + + /// 振荡中心（应是工作点；偏离说明有静差 / 漂移）。 + #[must_use] + pub fn midpoint(&self) -> f32 { + self.peak.midpoint(self.trough) + } + } + + /// 继电反馈整定器。 + #[derive(Debug, Clone)] + pub struct RelayTuner { + /// 继电输出幅值 `d`（控制量单位）。 + amplitude: f32, + /// 滞环半宽（测量量单位）：超过它才换向。 + hysteresis: f32, + /// 当前状态。 + state: RelayState, + /// 当前输出。 + output: f32, + /// 实验已进行的时间（秒）。 + elapsed_s: f32, + /// 上一次换向的时刻（秒）。 + last_switch_s: f32, + /// 上一次半周期长度（秒）。 + last_half_s: Option, + /// 本相内观测到的极值。 + extreme: f32, + /// 最近一次 High 相的峰值。 + peak: Option, + /// 最近一次 Low 相的谷值。 + trough: Option, + /// 已记录的振荡点。 + points: Vec, + /// 用于 `result()` 的规则。 + rule: ZieglerNicholsRule, + /// 参与平均的最近点数。 + window: usize, + } + + impl RelayTuner { + /// 构造。`amplitude` 必须为正；`hysteresis` 取绝对值。 + /// + /// `amplitude` 的选择：太小会被摩擦 / 量化淹没（台架空载建议 ≥ 静摩擦 + /// 量级的一倍），太大会让机械承受不必要的冲击。 + #[must_use] + pub fn new(amplitude: f32, hysteresis: f32) -> Self { + let amplitude = if amplitude.is_finite() && amplitude > 0.0 { amplitude } else { 1.0 }; + Self { + amplitude, + hysteresis: if hysteresis.is_finite() { hysteresis.abs() } else { 0.0 }, + state: RelayState::High, + output: amplitude, + elapsed_s: 0.0, + last_switch_s: 0.0, + last_half_s: None, + extreme: f32::NEG_INFINITY, + peak: None, + trough: None, + points: Vec::new(), + rule: ZieglerNicholsRule::default(), + window: 4, + } + } + + /// 换一条 ZN 规则（链式）。 + #[must_use] + pub fn with_rule(mut self, rule: ZieglerNicholsRule) -> Self { + self.rule = rule; + self + } + + /// 换平均窗口（参与 `Ku` / `Tu` 平均的最近点数，至少 1）。 + #[must_use] + pub fn with_window(mut self, window: usize) -> Self { + self.window = window.max(1); + self + } + + /// 当前继电输出（喂给被控对象）。 + #[must_use] + pub fn last_output(&self) -> f32 { + self.output + } + + /// 当前状态。 + #[must_use] + pub fn state(&self) -> RelayState { + self.state + } + + /// 已记录的振荡点。 + #[must_use] + pub fn points(&self) -> &[OscillationPoint] { + &self.points + } + + /// 复位（做下一次实验前调用）。 + pub fn reset(&mut self) { + self.state = RelayState::High; + self.output = self.amplitude; + self.elapsed_s = 0.0; + self.last_switch_s = 0.0; + self.last_half_s = None; + self.extreme = f32::NEG_INFINITY; + self.peak = None; + self.trough = None; + self.points.clear(); + } + + /// 推进一步：喂入测量值，取回继电输出。 + /// + /// 非有限测量值或非正 `dt` 时不推进状态（防御跳帧）。 + pub fn step(&mut self, measured: f32, dt: f32) -> f32 { + // 显式写 NaN 情形：`dt <= 0.0` 对 NaN 为假，会漏掉"跳帧 + NaN"的组合。 + if !dt.is_finite() || dt <= 0.0 || !measured.is_finite() { + return self.output; + } + self.elapsed_s += dt; + + // 本相内的极值 + self.extreme = match self.state { + RelayState::High => self.extreme.max(measured), + RelayState::Low => self.extreme.min(measured), + }; + + let switch = match self.state { + // 输出为正 → 测量值上升；越过滞环上沿说明该换向 + RelayState::High => measured >= self.hysteresis, + RelayState::Low => measured <= -self.hysteresis, + }; + if switch { + let half_s = self.elapsed_s - self.last_switch_s; + self.last_switch_s = self.elapsed_s; + match self.state { + RelayState::High => self.peak = Some(self.extreme), + RelayState::Low => self.trough = Some(self.extreme), + } + // 一个完整周期 = 相邻两个半周期；峰谷都有才能算幅值。 + if let (Some(peak), Some(trough), Some(previous_half)) = + (self.peak, self.trough, self.last_half_s) + && peak > trough + && half_s > 0.0 + { + self.points.push(OscillationPoint { + peak, + trough, + period_s: previous_half + half_s, + }); + } + self.last_half_s = Some(half_s); + self.state = self.state.flipped(); + self.output = match self.state { + RelayState::High => self.amplitude, + RelayState::Low => -self.amplitude, + }; + self.extreme = measured; + } + self.output + } + + /// 平均后的 `(Ku, Tu)`：`None` 表示还没形成足够多的振荡。 + #[must_use] + pub fn ultimate(&self) -> Option<(f32, f32)> { + if self.points.len() < 2 { + return None; + } + let window = &self.points[self.points.len().saturating_sub(self.window)..]; + let count = window.len() as f32; + let mean_amplitude = + window.iter().map(OscillationPoint::amplitude).sum::() / count; + let mean_period = window.iter().map(|point| point.period_s).sum::() / count; + let amplitude_ok = mean_amplitude.is_finite() && mean_amplitude > 0.0; + let period_ok = mean_period.is_finite() && mean_period > 0.0; + if !amplitude_ok || !period_ok { + return None; + } + // 描述函数：Ku = 4d / (π·a) + let ku = 4.0 * self.amplitude / (core::f32::consts::PI * mean_amplitude); + if ku.is_finite() && ku > 0.0 { Some((ku, mean_period)) } else { None } + } + + /// 整定结果（含 ZN 换算）；`None` = 实验没有形成可用振荡。 + #[must_use] + pub fn result(&self) -> Option { + let (ku, tu) = self.ultimate()?; + TuneResult::from_ultimate(ku, tu, self.rule) + } + } + + #[cfg(test)] + mod tests { + use super::*; + use motor_control::{ControlPlant, SecondOrderPlant}; + + /// 二阶对象在 `ω = wn` 处相移恰为 −180°，因此 `Tu ≈ 2π / wn`。 + fn run_relay(wn: f32, zeta: f32, seconds: f32) -> RelayTuner { + let dt = 0.001f32; + let mut plant = SecondOrderPlant::new(wn, zeta, 1.0); + let mut tuner = RelayTuner::new(1.0, 0.005); + let steps = (seconds / dt) as usize; + for _ in 0..steps { + let measured = plant.step(tuner.last_output(), dt).unwrap(); + tuner.step(measured, dt); + } + tuner + } + + #[test] + fn relay_reaches_a_limit_cycle_on_a_second_order_plant() { + // ARCHITECTURE.md §5.5 的验收标准：继电整定在标准二阶系统上能收敛。 + // 注意收敛的是"形成稳定极限环并给出有限 (Ku, Tu)"，而不是 Tu = 2π/wn + // （纯二阶没有有限的 −180° 穿越点，见模块文档）。 + let tuner = run_relay(20.0, 0.3, 4.0); + assert!(tuner.points().len() >= 3, "点数 = {}", tuner.points().len()); + let (ku, tu) = tuner.ultimate().unwrap(); + assert!(ku.is_finite() && ku > 0.0, "Ku = {ku}"); + assert!(tu.is_finite() && tu > 0.0, "Tu = {tu}"); + let result = tuner.result().unwrap(); + assert!(result.gains.is_finite()); + assert!(result.gains.kp > 0.0 && result.gains.ki > 0.0 && result.gains.kd > 0.0); + } + + #[test] + fn limit_cycle_is_repeatable_across_the_averaging_window() { + // "收敛"的可检验含义：进入极限环后，逐周期的幅值与周期不再漂移。 + let tuner = run_relay(20.0, 0.3, 4.0); + let points = tuner.points(); + assert!(points.len() >= 4, "点数 = {}", points.len()); + let tail = &points[points.len() - 3..]; + let first = tail[0]; + for point in tail { + let period_drift = (point.period_s - first.period_s).abs() / first.period_s; + let amplitude_drift = + (point.amplitude() - first.amplitude()).abs() / first.amplitude(); + assert!(period_drift < 0.1, "周期漂移 {period_drift}：{point:?}"); + assert!(amplitude_drift < 0.1, "幅值漂移 {amplitude_drift}：{point:?}"); + } + } + + #[test] + fn amplitude_and_midpoint_stay_physical() { + let tuner = run_relay(20.0, 0.3, 4.0); + for point in tuner.points() { + assert!(point.amplitude() > 0.0, "{point:?}"); + assert!(point.period_s > 0.0, "{point:?}"); + assert!(point.midpoint().abs() < 1.0, "中心应停在 0 附近：{point:?}"); + } + } + + #[test] + fn no_limit_cycle_means_no_result() { + // 没有实验 → 没有结果（而不是给一组乱数）。 + let tuner = RelayTuner::new(1.0, 0.01); + assert!(tuner.ultimate().is_none()); + assert!(tuner.result().is_none()); + + // 测量值卡死在正侧（如机械堵转 / 传感器饱和）：只换向一次，形不成振荡。 + let mut tuner = RelayTuner::new(1.0, 0.1); + for _ in 0..500 { + tuner.step(5.0, 0.001); + } + assert!(tuner.points().len() < 2, "堵转测量不应产生振荡点：{:?}", tuner.points()); + assert!(tuner.result().is_none(), "没有振荡就必须报失败"); + } + + #[test] + fn switching_respects_hysteresis_and_alternates() { + let mut tuner = RelayTuner::new(2.0, 0.5); + assert_eq!(tuner.state(), RelayState::High); + assert!((tuner.last_output() - 2.0).abs() < 1e-6); + // 未越过滞环：状态不变 + assert!((tuner.step(0.4, 0.001) - 2.0).abs() < 1e-6); + assert_eq!(tuner.state(), RelayState::High); + // 越过上沿：翻到 Low + assert!((tuner.step(0.6, 0.001) + 2.0).abs() < 1e-6); + assert_eq!(tuner.state(), RelayState::Low); + // 越过下沿：翻回 High + assert!((tuner.step(-0.6, 0.001) - 2.0).abs() < 1e-6); + assert_eq!(tuner.state(), RelayState::High); + } + + #[test] + fn bad_inputs_do_not_advance_state() { + let mut tuner = RelayTuner::new(1.0, 0.1); + let before = tuner.last_output(); + assert!((tuner.step(f32::NAN, 0.001) - before).abs() < 1e-6); + assert!((tuner.step(1.0, 0.0) - before).abs() < 1e-6); + assert!((tuner.step(1.0, -0.001) - before).abs() < 1e-6); + assert!(tuner.points().is_empty()); + } + + #[test] + fn reset_clears_the_experiment() { + let mut tuner = run_relay(20.0, 0.3, 4.0); + assert!(!tuner.points().is_empty()); + tuner.reset(); + assert!(tuner.points().is_empty()); + assert_eq!(tuner.state(), RelayState::High); + assert!(tuner.ultimate().is_none()); + } + + #[test] + fn degenerate_constructor_arguments_are_clamped() { + let tuner = RelayTuner::new(0.0, f32::NAN); + assert!((tuner.last_output() - 1.0).abs() < 1e-6, "非正幅值必须回落到安全默认"); + let tuner = RelayTuner::new(-5.0, -1.0); + assert!(tuner.last_output() > 0.0); + assert_eq!(tuner.with_window(0).window, 1); + } + } +} + +pub mod zn { + //! Ziegler-Nichols 整定公式（ARCHITECTURE.md §5.5）。 + //! + //! 输入是**临界增益** `Ku` 与**临界周期** `Tu`（由继电反馈实验测得，见 + //! [`super::relay`]），输出是一组 PID 增益： + //! + //! ```text + //! Ti = α · Tu Td = β · Tu + //! Kp = γ · Ku Ki = Kp / Ti Kd = Kp · Td + //! ``` + //! + //! 单位约定：`Ku` 与 `Kp` 是「控制量 / 测量量」，`Ki` 是「控制量 / (测量量·s)」， + //! `Kd` 是「控制量 / (测量量/s)」。速度环里测量量是 rpm、控制量是 raw， + //! 于是正好得到 [`motor_control::VelocityGains`] 的单位。 + //! + //! # 选哪条规则 + //! + //! ZN 的经典整定**以超调大著称**（约 25%），对底盘速度环偏激进； + //! [`ZieglerNicholsRule::NoOvershoot`] 更保守，是实车起步的稳妥选择。 + + use motor_control::VelocityGains; + + /// ZN 整定规则（系数来自 Ziegler-Nichols 1942 及后续的保守变体）。 + #[derive(Debug, Clone, Copy, PartialEq, Eq, Default)] + pub enum ZieglerNicholsRule { + /// 经典 PID：`Kp = 0.6·Ku`、`Ti = 0.5·Tu`、`Td = 0.125·Tu`（超调约 25%）。 + #[default] + Classic, + /// 少量超调：`Kp = 0.33·Ku`、`Ti = 0.5·Tu`、`Td = 0.33·Tu`。 + SomeOvershoot, + /// 无超调：`Kp = 0.2·Ku`、`Ti = 0.5·Tu`、`Td = 0.33·Tu`。 + NoOvershoot, + /// Pessen：`Kp = 0.7·Ku`、`Ti = 0.4·Tu`、`Td = 0.15·Tu`（更激进）。 + Pessen, + } + + impl ZieglerNicholsRule { + /// 规则系数 `(γ = Kp/Ku, α = Ti/Tu, β = Td/Tu)`。 + #[must_use] + pub const fn coefficients(self) -> (f32, f32, f32) { + match self { + Self::Classic => (0.6, 0.5, 0.125), + Self::SomeOvershoot => (0.33, 0.5, 0.33), + Self::NoOvershoot => (0.2, 0.5, 0.33), + Self::Pessen => (0.7, 0.4, 0.15), + } + } + + /// 文本名（报告 / CLI 用）。 + #[must_use] + pub const fn name(self) -> &'static str { + match self { + Self::Classic => "classic", + Self::SomeOvershoot => "some-overshoot", + Self::NoOvershoot => "no-overshoot", + Self::Pessen => "pessen", + } + } + } + + /// 由 `(Ku, Tu)` 与规则算出增益组。 + /// + /// # Errors + /// + /// `Ku` / `Tu` 非有限或非正时返回 `None`（宁可报"整定失败"，也不要发散的增益）。 + #[must_use] + pub fn gains(ku: f32, tu: f32, rule: ZieglerNicholsRule) -> Option { + if !(ku.is_finite() && tu.is_finite() && ku > 0.0 && tu > 0.0) { + return None; + } + let (gamma, alpha, beta) = rule.coefficients(); + let kp = gamma * ku; + let ti = alpha * tu; + let td = beta * tu; + // ti 由上式必然为正有限；仍做一次防御，避免 NaN 传染给 PID（Pid::new 会 panic）。 + if !(kp.is_finite() && ti.is_finite() && ti > 0.0 && td.is_finite()) { + return None; + } + let gains = VelocityGains::new(kp, kp / ti, kp * td, 0.0); + if gains.is_finite() { Some(gains) } else { None } + } + + /// 一次整定的结果：中间量 + 换算后的增益（便于写进报告复现）。 + #[derive(Debug, Clone, Copy, PartialEq)] + pub struct TuneResult { + /// 临界增益。 + pub ku: f32, + /// 临界周期（秒）。 + pub tu: f32, + /// 使用的规则。 + pub rule: ZieglerNicholsRule, + /// 换算后的速度环增益（前馈为 0：ZN 不产生前馈项）。 + pub gains: VelocityGains, + } + + impl TuneResult { + /// 按规则算出一组结果。 + #[must_use] + pub fn from_ultimate(ku: f32, tu: f32, rule: ZieglerNicholsRule) -> Option { + let gains = gains(ku, tu, rule)?; + Some(Self { ku, tu, rule, gains }) + } + + /// 人类可读描述。 + #[must_use] + pub fn describe(&self) -> String { + format!( + "ZN({})：Ku={:.4} Tu={:.4}s → kp={:.4} ki={:.4} kd={:.4}", + self.rule.name(), + self.ku, + self.tu, + self.gains.kp, + self.gains.ki, + self.gains.kd + ) + } + } + + #[cfg(test)] + mod tests { + use super::*; + + #[test] + fn classic_formulas_match_the_textbook() { + // Ku = 2, Tu = 0.5 → Kp = 1.2, Ti = 0.25, Td = 0.0625 + // Ki = Kp / Ti = 4.8, Kd = Kp · Td = 0.075 + let gains = gains(2.0, 0.5, ZieglerNicholsRule::Classic).unwrap(); + assert!((gains.kp - 1.2).abs() < 1e-6); + assert!((gains.ki - 4.8).abs() < 1e-6); + assert!((gains.kd - 0.075).abs() < 1e-6); + assert!(gains.feedforward.abs() < 1e-6, "ZN 不产生前馈"); + } + + #[test] + fn conservative_rules_give_smaller_kp() { + let classic = gains(2.0, 0.5, ZieglerNicholsRule::Classic).unwrap(); + let some = gains(2.0, 0.5, ZieglerNicholsRule::SomeOvershoot).unwrap(); + let none = gains(2.0, 0.5, ZieglerNicholsRule::NoOvershoot).unwrap(); + let pessen = gains(2.0, 0.5, ZieglerNicholsRule::Pessen).unwrap(); + assert!(pessen.kp > classic.kp); + assert!(classic.kp > some.kp); + assert!(some.kp > none.kp); + } + + #[test] + fn degenerate_ultimate_values_are_rejected() { + for (ku, tu) in [(0.0, 1.0), (1.0, 0.0), (-1.0, 1.0), (1.0, -1.0), (f32::NAN, 1.0)] { + assert!(gains(ku, tu, ZieglerNicholsRule::Classic).is_none(), "ku={ku} tu={tu}"); + } + } + + #[test] + fn result_carries_the_intermediate_values() { + let result = + TuneResult::from_ultimate(2.0, 0.5, ZieglerNicholsRule::NoOvershoot).unwrap(); + assert!((result.ku - 2.0).abs() < 1e-6); + assert!((result.tu - 0.5).abs() < 1e-6); + assert_eq!(result.rule, ZieglerNicholsRule::NoOvershoot); + let text = result.describe(); + assert!(text.contains("no-overshoot"), "{text}"); + assert!(text.contains("Ku=2.0000"), "{text}"); + } + + #[test] + fn rule_names_are_stable() { + for (rule, name) in [ + (ZieglerNicholsRule::Classic, "classic"), + (ZieglerNicholsRule::SomeOvershoot, "some-overshoot"), + (ZieglerNicholsRule::NoOvershoot, "no-overshoot"), + (ZieglerNicholsRule::Pessen, "pessen"), + ] { + assert_eq!(rule.name(), name); + } + assert_eq!(ZieglerNicholsRule::default(), ZieglerNicholsRule::Classic); + } + } +} + +pub mod cost { + //! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。 + //! + //! 两个原则： + //! + //! 1. **硬约束不用权重表达**。过速 / 长时间饱和 / 不收敛不是"差一点"， + //! 而是"不能用"——它们直接给罚值并记录判定，绝不允许被其他项的权重 + //! 抵消掉（否则搜索会主动挑一个会飞车的解）。 + //! 2. **各项归一化**。时间按轨迹时长归一、误差按 `|目标| × 时长` 归一、 + //! effort 按限幅归一，因此权重在不同场景 / 不同电机之间仍然可解释。 + + use experiment::{Scenario, Trace}; + use sim_harness::{LoopLimits, SimError}; + + use experiment::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 motor_control::VelocityGains; + use motor_model::PlantParams; + use sim_harness::simulate; + + 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: motor_protocol::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); + } + } +} + +pub mod gains { + //! 增益参数空间：轴定义、采样与**坐标空间**映射。 + //! + //! # 为什么要有"坐标空间" + //! + //! 增益的合法取值跨越数量级（`ki` 可能在 0.1 与 100 之间），线性空间的 + //! 单纯形步长对数量级不敏感；同时 `kp/ki/kd` 必须为正。把每个轴映射到 + //! **坐标空间**（对数轴取 `ln`，线性轴恒等）后： + //! + //! - 无约束优化器不会走出正增益区域； + //! - 步长在不同数量级上等价（±0.1 的坐标步长 = ×1.1 的比例步长）； + //! - `LogOrZero` 轴在**粗扫**阶段可以精确取 0（`ff = 0` 是台架常态）， + //! 在**精修**阶段退化为对数轴（零是不可导的拐点，本地搜索收益为负）。 + + use motor_control::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()); + } + } +} + +pub mod preset { + //! 整定**超参数**预设：把"搜什么、怎么打分、什么算达标"从 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 experiment::{Scenario, SetpointProfile}; + use motor_model::PlantParams; + use serde::{Deserialize, Serialize}; + use sim_harness::PlantTolerances; + 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); + } + } +} + +pub mod search { + //! 两阶段参数空间搜索：种子化分层粗扫 → `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 experiment::Scenario; + use indicatif::ProgressBar; + use motor_control::VelocityGains; + use rand::SeedableRng as _; + use rand::rngs::StdRng; + use rayon::prelude::*; + use thiserror::Error; + + use crate::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::TestSimEvaluator; + use crate::cost::{Objective, Weights}; + use motor_model::PlantParams; + use sim_harness::{LoopLimits, PlantEnsemble, PlantTolerances}; + + fn space() -> GainSpace { + GainSpace::default_gm3508() + } + + fn evaluator(plant: PlantParams, ensemble: usize, robust_weight: f64) -> TestSimEvaluator { + 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); + TestSimEvaluator::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}"); + } + } +} + +pub use cost::*; +pub use gains::*; +pub use preset::*; +pub use relay::*; +pub use search::*; +pub use zn::*; + +#[cfg(test)] +/// Test-only offline evaluator for search regression tests. +struct TestSimEvaluator { + ensemble: sim_harness::PlantEnsemble, + scenario: experiment::Scenario, + limits: sim_harness::LoopLimits, + objective: cost::Objective, + robust_weight: f64, +} + +#[cfg(test)] +impl TestSimEvaluator { + /// Build an offline evaluator. + #[must_use] + pub fn new( + ensemble: sim_harness::PlantEnsemble, + scenario: experiment::Scenario, + limits: sim_harness::LoopLimits, + objective: cost::Objective, + robust_weight: f64, + ) -> Self { + Self { ensemble, scenario, limits, objective, robust_weight } + } +} + +#[cfg(test)] +impl Evaluator for TestSimEvaluator { + fn evaluate(&self, gains: &motor_control::VelocityGains) -> Evaluated { + let mut costs = Vec::with_capacity(self.ensemble.len()); + let mut nominal = None; + let mut worst_verdict = cost::Verdict::Ok; + for (index, plant) in self.ensemble.iter().enumerate() { + let score = match sim_harness::simulate(*gains, plant, &self.scenario, self.limits) { + Ok(trace) => self.objective.score(&trace), + Err(error) => cost::Score::penalized(cost::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().max(1) as f64; + let mean = costs.iter().sum::() / count; + let worst = costs.iter().copied().fold(f64::NEG_INFINITY, f64::max); + let cost_value = (1.0 - self.robust_weight) * mean + self.robust_weight * worst; + let nominal = nominal.unwrap_or_else(|| cost::Score::penalized(cost::Verdict::Rejected)); + let verdict = if nominal.verdict.is_ok() { worst_verdict } else { nominal.verdict }; + cost::Score { cost: cost_value, verdict, metrics: nominal.metrics } + } + + fn describe(&self) -> String { + format!("sim(ensemble={}, robust_weight={:.2})", self.ensemble.len(), self.robust_weight) + } +} diff --git a/deploy/chassisd.service b/deploy/chassisd.service index 6f149f6..70dbed6 100644 --- a/deploy/chassisd.service +++ b/deploy/chassisd.service @@ -1,19 +1,19 @@ [Unit] Description=Sentry chassis daemon (mecanum, 4x GM3508/C620) After=network.target -# 若使用 USB 转 CAN，等待接口就绪： -# After=systemd-networkd.service -# RequiresMountsFor=/sys/class/net/can0 [Service] Type=simple ExecStart=/usr/local/bin/chassisd --interface can0 --config /etc/chassisd/chassis.toml Restart=always RestartSec=1 -# 实时性：提升调度优先级（按需调整） -# CPUSchedulingPolicy=fifo -# CPUSchedulingPriority=20 -# LimitRTPRIO=95 + +# 实时调度由 systemd 设置；应用不调用 libc FFI。 +CPUSchedulingPolicy=fifo +CPUSchedulingPriority=80 +LimitRTPRIO=95 +# 仅提高锁定额度；std 程序不调用 mlockall。 +LimitMEMLOCK=infinity [Install] WantedBy=multi-user.target diff --git a/docs/architecture.md b/docs/architecture.md index 398326f..12cd559 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -1,162 +1,6 @@ -# 架构设计 +# 架构文档入口 -## 分层 - -``` - ┌────────────────┐ ┌──────────────────────┐ ┌─────────────────────────┐ - │ 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` 提供两条路径：**异步假电调**（挂在同一对通道上，`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 手册） - -| 项目 | 值 | -| --- | --- | -| 控制帧 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 温度℃，均大端 | -| 反馈频率 | 每电调 1kHz | - -C620 内部**只闭电流环**：转速 / 位置环由主控侧 PID 实现（`motor-core::Pid`， -挂在 `EscBus` 的 1kHz 发送节拍上）。GM3508 减速比 3591 / 187 ≈ 19.203。 - -## 单位与方向约定 - -- 机器人坐标系：ISO 约定——x 前、y 左、ω 逆时针为正（见 `mecanum`）。 -- `Motor` 接口呈现的是**物理量**（安装方向修正后）： - `EscConfig::direction = -1` 时，驱动层在写命令与读反馈时统一翻转符号， - 上层永远以"轮子正转 = 底盘前进方向"的正向工作。 -- `MotorState::rotor_speed_rpm` 为电机转子转速；轮端转速 = 转子转速 / 减速比。 -- 电流一律以协议原始值（raw）为主，安培换算通过 `EscModel` 常量提供。 - -## 安全设计 - -1. **上电门控**：底盘循环在收到任一电机首帧反馈前不下发速度命令。 -2. **反馈离线**：任一电机反馈超时（默认 100ms）→ 该电机命令强制 0 电流并告警。 -3. **命令超时**：上层 twist 命令超时（默认 200ms）→ 目标速度归零。 -4. 所有超时可经 `chassis.toml` 配置。 - -## 测试策略 - -| 层级 | 内容 | 平台 | -| --- | --- | --- | -| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、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`。板端 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)——全系统分层与阶段计划 +架构规范的唯一来源是仓库根目录的 [`ARCHITECTURE.md`](../ARCHITECTURE.md)。 +本文件不再复制分层图或依赖规则，以免两份架构文档漂移。接线、标定、台架、 +自动整定和故障排查见 [`chassis.md`](chassis.md)。 diff --git a/docs/chassis.md b/docs/chassis.md index 59ff841..24fda1a 100644 --- a/docs/chassis.md +++ b/docs/chassis.md @@ -32,12 +32,11 @@ | 文档 | 回答的问题 | 读者 | | --- | --- | --- | -| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 整套 RoboMaster CAN 控制系统的**全系统分层规划**（含云台、发射机构、delta10 IMU、T153 实时性、9 电机） | 架构决策者 | -| [`docs/architecture.md`](architecture.md) | 本仓库**已实现**部分的解耦点：`CanLink` seam、被控对象模型、自动整定两阶段搜索、测试策略 | 改代码的人 | -| **本文** | **底盘这一块**怎么开发、怎么标定、怎么整定、怎么上车、出事怎么办 | 底盘开发 / 调试 / 上车的人 | +| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 职责型 workspace、允许依赖、std/unsafe 策略、实现边界 | 架构决策者 | +| [`docs/architecture.md`](architecture.md) | 架构文档入口，不复制规范 | 改代码的人 | +| **本文** | **底盘这一块**怎么开发、标定、整定、上车与排障 | 底盘开发 / 调试 / 上车的人 | -`ARCHITECTURE.md` 里的 `rm-*` crate 划分是**规划**；本仓库实际落地为下面这套 crate 命名 -（功能对应关系见第 5 节）。本文只覆盖底盘切片，云台 / 发射 / IMU 不在范围内。 +本文只覆盖四轮底盘切片；云台、发射机构与 DETA10 真实载荷布局不在当前范围内。 --- @@ -46,7 +45,7 @@ ### 2.1 无硬件（macOS / Linux / CI 均可） ```sh -cargo test --workspace # 单测 + 闭环仿真 + 整定，全部无需硬件 +cargo test --workspace --all-targets # 单测 + 闭环仿真 + 整定，全部无需硬件 cargo run -p chassisd -- --mock # 进程内电机模型的全链路演示 cargo run -p chassis-tune -- presets # 看有哪些整定超参数预设 just tune-smoke # 确定性整定冒烟（CI 同款） @@ -57,10 +56,10 @@ just lint # fmt + clippy（与 CI 同标准） ```sh # 1) 只验链路与反馈，绝不发非零电流（接线排查第一步） -cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run +cargo run --release -p spin -- chassis --device /dev/ttyACM0 --dry-run # 2) 四轮定性旋转（轮子必须架空或整车固定，急停可及） -cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --rpm 500 +cargo run --release -p spin -- chassis --device /dev/ttyACM0 --rpm 500 # 3) 先离线整定，再上台架复验（用版本化的超参数预设） cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name sim_bench_model @@ -89,8 +88,8 @@ T153 (Linux) Gm3508 × 4 + C620 | 路径 | 打开方式 | 适用 | | --- | --- | --- | -| **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；生产部署 | +| **slcan**（`slcan`） | 用户态直接读写 `/dev/ttyACM0`（Lawicel ASCII 协议） | 免 `ip link`、绕开 `gs_usb` 驱动；台架与初始联调首选 | +| **SocketCAN**（`socketcan`） | `ip link set can0 up type can bitrate 1000000 restart-ms 100` | 板载原生 CAN；生产部署 | ### 3.2 设备与协议常量（C620 手册） @@ -105,11 +104,11 @@ T153 (Linux) Gm3508 × 4 + C620 | 反馈频率 | 每电调 1 kHz | | 减速比 | GM3508 = 3591 / 187 ≈ **19.203**（转子 : 输出轴） | -C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-core::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。 +C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-control::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。 ### 3.3 电调 ID 与安装方向 -默认约定（`config/chassis.toml.example`、`chassis-spin` 默认值一致）： +默认约定（`config/chassis.toml.example`、`spin chassis` 默认值一致）： | 轮位 | 轮序下标 | 电调 ID | 安装方向 | | --- | --- | --- | --- | @@ -125,7 +124,7 @@ C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`moto | 场景 | 建议限幅（raw） | 说明 | | --- | --- | --- | -| 空载台架（`chassis-spin` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 | +| 空载台架（`spin chassis` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 | | 整车实车 | 6000 ~ 12000 | 视供电与机械而定；同时是 PID 抗饱和的钳位点 | > **限流不是"越大越安全"**：它同时是 PID 输出的钳位点。设得远大于实际所需， @@ -135,7 +134,7 @@ C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`moto - 电调供电与主控**共地**；CAN_H / CAN_L 不接反，两端 120Ω 终端电阻齐全。 - 轮子**架空**或整车固定；急停开关在手可及处。 -- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`chassis-spin` 会打印每个电调的 +- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`spin chassis` 会打印每个电调的 转速 / 温度 / 反馈年龄）。 --- @@ -161,7 +160,7 @@ v_RR = vx - vy + a·ω ``` 正解是逆解的左伪逆（JᵀJ 对角，可解析求平均）。实现见 -[`crates/mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` / +[`mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` / `wheel_rpms` / `twist_from_wheel_rpms`。 ### 4.3 单位与层级（最容易出错的地方） @@ -175,7 +174,7 @@ v_RR = vx - vy + a·ω | 控制量 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`）。 +- **减速比换算只发生在一个地方**：`motor / esc-bus::OutputShaft` 包装（`转子 = 输出轴 × gear_ratio`）。 `EscBus` / `EscMotor` 只认转子，绝不碰减速比；`EscConfig::gear_ratio` 仅作配置元数据。 - PID 的接线顺序：`ChassisController` 算出**轮端** rpm → `OutputShaft` 换算成转子 rpm → `EscMotor` 的速度环以转子 rpm 闭环。 @@ -184,77 +183,34 @@ v_RR = vx - vy + a·ω ## 5. 软件架构 -### 5.1 Crate 分层与职责 +### 5.1 职责边界 -**依赖只能向下**（上层可依赖下层，反之不行）： +完整 crate 清单和允许依赖见 [`ARCHITECTURE.md`](../ARCHITECTURE.md)。底盘路径上的关键职责是： -``` -第 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） -``` +| 职责 | crate | +| --- | --- | +| CAN 帧、总线接口、链路与 Mock | `can` | +| 电调协议 | `motor-protocol` | +| 设备契约、命令/状态/减速比/注册表 | `motor` | +| 帧泵、租约、反馈新鲜度、零电流 | `esc-bus` | +| PID、速度/位置环、组合策略 | `pid`、`motor-control` | +| 麦轮运动学 | `mecanum` | +| 配置 schema | `chassis-config` | +| 台架安全与采样 | `bench` | +| 应用入口 | `chassisd`、`spin`、`chassis-tune` | -`dji-esc-slcan` / `dji-esc-socketcan` 只实现 `can-link` 的通道语义，**不含任何应用逻辑**； -它们被第 4 层的工具与守护进程选用，而不是被协议层依赖。 +crate 内源码保持扁平，不建立 `src//` 子目录。 -| 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 依赖与组合 -### 5.2 依赖方向（硬约束） +`esc-bus` 不依赖 PID 或 `motor-control`。`motor::EffortPolicy` 是两者的组合点： +应用构造 `motor-control::MotorPolicy`，并把它传给 `EscBus::spawn`。 -``` -chassisd ─┬─► chassis-config ─► motor-core, mecanum - ├─► dji-esc ──► can-link, motor-core - ├─► dji-esc-sim ─► dji-esc（协议类型）, can-link - └─► dji-esc-socketcan ─► can-link +总线仍然在 1 kHz 节拍上运行策略，因此控制时序不变；变化的是控制律不再被封在总线驱动里。 +反馈离线、命令租约到期和模式切换会把对应策略状态复位并强制零输出。 -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）。 - ---- +`bench` 只产生试验轨迹和安全中止原因；指标、成本、搜索和报告适配由 +`tuning` 与 `chassis-tune` 完成。 ## 6. 关键机制（实车踩过的坑） @@ -292,7 +248,7 @@ tick (1ms) ## 7. 控制设计 -### 7.1 PID（`motor-core::Pid`） +### 7.1 PID（`motor-control::Pid`） 并联式 PID，显式时间步 `dt`（秒），特性： @@ -302,7 +258,7 @@ tick (1ms) 4. 可选**微分一阶低通**（`with_derivative_tau`），抑制编码器量化噪声； 5. `dt <= 0` 或非有限输入 → 返回上次输出且不更新内部状态（防御跳帧 / 首帧）。 -### 7.2 速度环（`motor-core::VelocityController`） +### 7.2 速度环（`motor-control::VelocityController`） ``` effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rpm, ±limit ) @@ -324,7 +280,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp | 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 | | 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 | -### 7.4 被控对象模型（`dji-esc-sim`） +### 7.4 被控对象模型（`sim-harness::motor_model`） 逐拍离散（`dt` 秒，电调坐标系：正电流 → 正转速）： @@ -394,9 +350,9 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程 | # | 步骤 | 命令 | 通过判据 | | --- | --- | --- | --- | -| 1 | **链路与反馈** | `chassis-spin --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID | +| 1 | **链路与反馈** | `spin chassis --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID | | 2 | **电调 ID 对应** | 同 1，逐个拔插 | 拔掉哪个电调，哪个 ID 的反馈消失 → ID 映射正确 | -| 3 | **方向** | `chassis-spin --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` | +| 3 | **方向** | `spin chassis --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` | | 4 | **减速比 / 轮径** | 手转一圈 + 读反馈角度 | 反馈角度 × 减速比 ≈ 轮子转过角度；不符则改 `gear_ratio` | | 5 | **几何参数** | 卷尺量 `half_length` / `half_width` / `wheel_radius` | 填入 `chassis.toml`（影响运动学精度，不影响稳定性） | | 6 | **限流** | 从 1500 raw 起，逐步加到"能可靠起动且不冲" | 越小越好；实车再按需上调 | @@ -485,7 +441,7 @@ chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预 | `field_updown` | bench | **up-down** 1200 rpm | 实战验收：升 / 降 / 回零双向，阈值收紧到 12% / 3% | **预设 schema**（`version = 1`，未知键报错，见 -[`crates/chassis-tune/src/preset.rs`](../crates/chassis-tune/src/preset.rs)）： +[`crates/tuning/src/lib.rs`](../crates/tuning/src/lib.rs)）： ```toml version = 1 @@ -556,7 +512,7 @@ feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜 ## 11. 台架工具 -### 11.1 `chassis-spin`（`chassis-bench`，slcan，全平台） +### 11.1 `spin chassis`（`spin`，slcan，全平台） 四轮同时以固定**转子**转速旋转。关键参数： @@ -586,13 +542,13 @@ feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜 - 上电门控：四个电调都上报新鲜反馈前绝不下发速度； - 空载的合适整定与带载**完全不同**，空载**不要**加前馈。 -### 11.2 `socketcan-spin`（`chassis-bench`，仅 Linux） +### 11.2 `spin motor`（`spin`，仅 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 +cargo run --release -p spin -- motor -- --interface can0 --dry-run ``` --- @@ -632,7 +588,7 @@ cargo run --release -p chassis-bench --bin socketcan-spin -- --interface can0 -- | 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 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（人工） | +| 实车台架 | `spin chassis` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） | | 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check；armv7 静态构建 `chassisd` | CI | ### 13.2 每个阶段完成后的门禁（`just lint` + 以下） @@ -660,7 +616,7 @@ just build-armv7 # cargo build --release -p chassisd --target armv7-unknown- just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2） ``` -目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。二进制静态链接，不依赖板端 libc。 +目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。发布二进制使用 musl 静态链接。 ### 14.2 板端拉起 CAN @@ -692,9 +648,8 @@ RestartSec=1 > （`ip -d link show can0` 可见），`chassisd` 侧表现为四电机反馈离线、强制零电流—— > **这正是安全设计的预期行为**，不是故障。 > -> 调度策略：`ARCHITECTURE.md` 规划了 `SCHED_FIFO` + `mlockall`（对应 `rm-app::realtime`）， -> 本仓库目前**未实现**实时优先级提升，当前只依赖 1 kHz tokio 定时器 + -> `MissedTickBehavior::Skip`。板端抖动实测与是否需要加固见第 15 节的抖动条目。 +> 调度策略：应用不调用 libc FFI。 +> 应用不调用 libc FFI。`SCHED_FIFO` 与资源限制由 systemd 服务单元设置；std 程序不调用 `mlockall`。 --- diff --git a/justfile b/justfile index ceab6be..a607f3f 100644 --- a/justfile +++ b/justfile @@ -1,103 +1,79 @@ -# 开发常用命令。未安装 just 时可直接使用 README 中的 cargo 命令。 - default: @just --list -# 格式化全部代码 fmt: cargo fmt --all -# 本地检查（与 CI 同标准） lint: cargo fmt --all -- --check cargo clippy --workspace --all-targets -- -D warnings -# 运行全部单测与集成测试（Linux 专属 vcan 测试自动跳过） test: - cargo test --workspace + cargo test --workspace --all-targets -# Linux：搭建 vcan0 并运行真实内核 CAN 栈集成测试 vcan-test: sudo modprobe vcan sudo ip link add dev vcan0 type vcan || true sudo ip link set up vcan0 - cargo test -p dji-esc-socketcan -- --ignored --test-threads 1 + cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1 -# 无硬件全链路演示（macOS / Linux 均可） run-mock: cargo run -p chassisd -- --mock -# 交叉检查目标板架构 -check-aarch64: - cargo check --workspace --target aarch64-unknown-linux-gnu +check-structure: + test -z "$(find crates/*/src -mindepth 2 -type d -print)" + ! rg -n '\bunsafe\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml' -# 交叉编译 armv7 静态二进制（目标板：创龙 T153 / Buildroot） build-armv7: rustup target add armv7-unknown-linux-musleabihf cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf + cargo build --release -p spin --target armv7-unknown-linux-musleabihf + cargo build --release -p chassis-tune --target armv7-unknown-linux-musleabihf -# 推送到板子并试运行（默认 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}}" -# 板端：拉起 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: +build-spin: rustup target add armv7-unknown-linux-musleabihf - cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf + cargo build --release -p 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}}" + just build-spin + scp -q target/armv7-unknown-linux-musleabihf/release/spin {{board}}:/tmp/spin + ssh {{board}} "/tmp/spin chassis --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" + just build-spin + scp -q target/armv7-unknown-linux-musleabihf/release/spin {{board}}:/tmp/spin + ssh {{board}} "/tmp/spin chassis --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