feat: 添加了自动超参数整定
This commit is contained in:
Horldsence
@@ -28,6 +28,19 @@ jobs:
|
||||
env:
|
||||
RUSTDOCFLAGS: -D warnings
|
||||
run: cargo doc --workspace --no-deps
|
||||
# 整定工具的可选 feature 不在默认 clippy/test 里，单独编译一次防止腐烂。
|
||||
- name: chassis-tune plot feature
|
||||
run: cargo check -p chassis-tune --features plot --all-targets
|
||||
# 自动整定的确定性冒烟：同 seed 必须逐位一致，且最优解必须通过验收阈值。
|
||||
- name: tune smoke (deterministic, no hardware)
|
||||
run: |
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \
|
||||
--out-dir target/tune-smoke
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \
|
||||
--out-dir target/tune-smoke-again
|
||||
diff target/tune-smoke/trials.csv target/tune-smoke-again/trials.csv
|
||||
|
||||
vcan-integration:
|
||||
name: vcan0 integration (real kernel CAN stack)
|
||||
@@ -65,6 +78,9 @@ jobs:
|
||||
targets: armv7-unknown-linux-musleabihf
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- run: cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
# 台架工具与整定工具同样要在板端跑（真机后端常直接跑在 T153 上）。
|
||||
- run: cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf
|
||||
- run: cargo build --release -p chassis-tune --target armv7-unknown-linux-musleabihf
|
||||
|
||||
coverage:
|
||||
name: coverage (llvm-cov)
|
||||
|
||||
+1172
File diff suppressed because it is too large
Load Diff
Generated
+837
-8
File diff suppressed because it is too large
Load Diff
+26
@@ -4,8 +4,13 @@ members = [
|
||||
"crates/motor-core",
|
||||
"crates/can-link",
|
||||
"crates/dji-esc",
|
||||
"crates/dji-esc-sim",
|
||||
"crates/dji-esc-socketcan",
|
||||
"crates/dji-esc-slcan",
|
||||
"crates/mecanum",
|
||||
"crates/chassis-config",
|
||||
"crates/chassis-bench",
|
||||
"crates/chassis-tune",
|
||||
"crates/chassisd",
|
||||
]
|
||||
|
||||
@@ -35,12 +40,33 @@ clap = { version = "4", features = ["derive"] }
|
||||
# 测试
|
||||
proptest = "1"
|
||||
approx = "0.5"
|
||||
# 自动整定工具（chassis-tune）
|
||||
#
|
||||
# `rand` 对齐 argmin 依赖的 0.9，避免依赖树里出现两个 rand 大版本；
|
||||
# `argmin-math` 必须显式开启 `vec`（argmin 自身只启 `primitives`），
|
||||
# 否则 `Vec<f64>` 参数向量没有 ArgminAdd/Sub/Mul 实现。
|
||||
argmin = { version = "0.11", default-features = false, features = ["rand"] }
|
||||
argmin-math = { version = "0.5", features = ["vec"] }
|
||||
rand = "0.9"
|
||||
rayon = "1"
|
||||
csv = "1.4"
|
||||
serde_json = "1"
|
||||
indicatif = "0.18"
|
||||
anyhow = "1"
|
||||
# 仅在 chassis-tune 的 `plot` feature 下引入：只要 SVG 后端与折线系列，
|
||||
# 但文字标签需要 `ttf`（字体加载），因此不能用纯 svg_backend 最小集。
|
||||
plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] }
|
||||
# workspace 内部 crate
|
||||
motor-core = { path = "crates/motor-core" }
|
||||
can-link = { path = "crates/can-link" }
|
||||
dji-esc = { path = "crates/dji-esc" }
|
||||
dji-esc-sim = { path = "crates/dji-esc-sim" }
|
||||
dji-esc-socketcan = { path = "crates/dji-esc-socketcan" }
|
||||
dji-esc-slcan = { path = "crates/dji-esc-slcan" }
|
||||
mecanum = { path = "crates/mecanum" }
|
||||
chassis-config = { path = "crates/chassis-config" }
|
||||
chassis-bench = { path = "crates/chassis-bench" }
|
||||
chassis-tune = { path = "crates/chassis-tune" }
|
||||
|
||||
[workspace.lints.rust]
|
||||
unsafe_code = "forbid"
|
||||
|
||||
@@ -1,19 +1,33 @@
|
||||
# sentry-chassis
|
||||
|
||||
机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），SocketCAN 接入，tokio 异步架构。
|
||||
机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），CAN 接入（SocketCAN 或串口 slcan），tokio 异步架构。
|
||||
|
||||
## 架构一览
|
||||
|
||||
| crate | 职责 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| [`motor-core`](crates/motor-core) | 电机抽象：`Motor` trait、`MotorState`、PID / 速度环控制器 | 纯逻辑，全平台 |
|
||||
| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动、仿真器 | 纯逻辑，全平台 |
|
||||
| [`motor-core`](crates/motor-core) | 电机抽象：能力 trait、`MotorState`、PID / 速度环 / 位置环 | 纯逻辑，全平台 |
|
||||
| [`can-link`](crates/can-link) | CAN 传输抽象：`CanFrame` + `CanLink` 通道对（无协议知识） | 纯逻辑，全平台 |
|
||||
| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-sim`](crates/dji-esc-sim) | 被控对象仿真：摩擦 / 延迟 / 负载模型、场景与**离线评估 harness**、异步假电调 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-socketcan`](crates/dji-esc-socketcan) | SocketCAN 后端：真实 CAN 接口 ↔ 内存通道桥接 | Linux |
|
||||
| [`dji-esc-slcan`](crates/dji-esc-slcan) | slcan 串口后端：CANable 等 USB 适配器 ↔ 内存通道桥接 | 全平台 |
|
||||
| [`mecanum`](crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 |
|
||||
| [`chassis-config`](crates/chassis-config) | `chassis.toml` schema：解析、校验、写回（守护进程与整定工具共用） | 全平台 |
|
||||
| [`chassis-bench`](crates/chassis-bench) | 台架：激励场景、安全包络、真机试验采样 + `chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） |
|
||||
| [`chassis-tune`](crates/chassis-tune) | **速度环 PID 自动整定**：参数空间搜索 + 指标 / 成本 + 报告（仿真 / 真机）+ 版本化**超参数预设** | 全平台 |
|
||||
| [`chassisd`](crates/chassisd) | 底盘守护进程：配置、1kHz 控制循环、安全逻辑、`--mock` 模式 | 全平台（实车需 Linux） |
|
||||
|
||||
分层原则：`motor-core` 与 `dji-esc` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道（`CanLink`），
|
||||
因此协议、运动学与控制逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层。
|
||||
分层原则：`motor-core`、`dji-esc`、`dji-esc-sim` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道
|
||||
（`CanLink`），因此协议、运动学、控制与整定逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层
|
||||
（`dji-esc-socketcan` / `dji-esc-slcan`，以及组合它们的 `chassis-bench` / `chassisd`）。
|
||||
|
||||
> 运输后端 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接，**不含应用**；
|
||||
> 台架程序住在 `chassis-bench`，自动整定住在 `chassis-tune`。
|
||||
|
||||
> **命令是租约**：`EscMotor::set_velocity` / `set_effort_raw` / `set_position` 每次调用即续租，
|
||||
> 超过 `BusOptions::command_timeout`（默认 50ms）未刷新，驱动强制 0 电流。
|
||||
> 控制循环必须**逐拍重发**同一个设定值（`chassisd` 的 1kHz 循环天然满足）。
|
||||
|
||||
## 开发
|
||||
|
||||
@@ -22,6 +36,7 @@
|
||||
```sh
|
||||
cargo test --workspace # 单测 + 闭环仿真测试（无需硬件）
|
||||
cargo run -p chassisd -- --mock # 无硬件全链路演示
|
||||
just tune-smoke # 自动整定冒烟（确定性，无需硬件，CI 同款）
|
||||
just lint # fmt + clippy（与 CI 同标准）
|
||||
```
|
||||
|
||||
@@ -31,31 +46,126 @@ just lint # fmt + clippy（与 CI 同标准）
|
||||
just vcan-test # 或见 justfile 内的 modprobe / ip link 命令
|
||||
```
|
||||
|
||||
### 部署到目标板（armv7，实测：创龙 T153 / Buildroot）
|
||||
## 台架：四轮 500 rpm 旋转测试（slcan）
|
||||
|
||||
实车台架验证工具 `chassis-spin`：四轮同时以固定**转子**转速旋转（默认 500 rpm，
|
||||
轮端 = 转子 ÷ 减速比 ≈ 26 rpm），带缓升、过速保护、上电门控与安全停机。
|
||||
|
||||
```sh
|
||||
# 本机直接跑（CANable2 在 /dev/ttyACM0）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0
|
||||
|
||||
# 只验证链路与反馈，不发非零电流（接线排查首选）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
|
||||
# 目标板（T153，armv7）：交叉编译 → 推送 → 运行
|
||||
just bench-spin # 默认 root@192.168.2.2 + /dev/ttyACM0 + 500 rpm
|
||||
just bench-check # 同上，但只做 dry-run
|
||||
```
|
||||
|
||||
安全与使用要点：
|
||||
|
||||
- **轮子必须架空或整车固定**；ctrl-c 随时安全停机（先 0 电流再关总线）。
|
||||
- 上电门控：四个电调都上报新鲜反馈前绝不下发速度；只接了部分电调时用 `--allow-partial` 降级。
|
||||
- 过速保护：任一电调超过 `--max-rpm`（默认 目标 × 2 + 200）持续 60ms 即立即停机。
|
||||
- 空载台架的合适整定与带载完全不同：默认 `kp=2 ki=10 ff=0 --current-limit 1500`，
|
||||
空载**不要**加前馈（会直接把轮子推到最高速度）。
|
||||
|
||||
## 自动整定（chassis-tune）
|
||||
|
||||
手动试凑 `kp/ki/kd/ff` 既慢又难复现。`chassis-tune` 把这件事变成**参数空间搜索**：
|
||||
种子化分层粗扫（`rayon` 并行）→ 以 top-k 为起点做多起点 `argmin` 局部精修（Nelder-Mead / PSO），
|
||||
每次评估都用同一套指标（上升时间、超调、调节时间、稳态误差、IAE、电流）和同一套硬约束
|
||||
（过速 / 长时间饱和 / 不收敛直接判罚，不参与权重折衷）。
|
||||
|
||||
```sh
|
||||
# 0) 看有哪些超参数预设（只读；预设文件见 config/tune.toml）
|
||||
cargo run -p chassis-tune -- presets
|
||||
|
||||
# 1) 仿真搜索（默认，不碰硬件；同 seed 结果逐位一致）
|
||||
cargo run --release -p chassis-tune -- sweep --samples 512 --out-dir target/tune
|
||||
|
||||
# 1b) 用预设跑：超参数（区间 / 场景 / 权重 / 阈值 / 预算 / 安全包络）进版本管理
|
||||
# 优先级：内置默认 < 预设文件 < 显式 CLI（逐字段覆盖）
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--preset config/tune.toml --preset-name bench_loaded
|
||||
|
||||
# 2) 只复算一组给定增益（CI / 回归用，按验收阈值给退出码：0 通过 / 1 未通过 / 2 参数错）
|
||||
cargo run -p chassis-tune -- validate --gains 2,10,0,0
|
||||
|
||||
# 3) 台架复验（**轮子会转**：必须架空或整车固定、急停可及）
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--preset config/tune.toml --preset-name bench_noload \
|
||||
--device /dev/ttyACM0 --esc-id 1 --i-know-the-wheel-spins
|
||||
```
|
||||
|
||||
随仓库提供的预设（`config/tune.toml`）：
|
||||
|
||||
| 预设 | 后端 | 用途 |
|
||||
| --- | --- | --- |
|
||||
| `sim_bench_model`（默认） | 仿真 | 台架量级模型全量搜索：评审 / CI / 代码回归基准 |
|
||||
| `sim_ideal` | 仿真 | 理想模型秒级回归（结果**不可直接上车**） |
|
||||
| `bench_noload` | 真机 | 空载台架，小样本局部搜索，前馈固定 0 |
|
||||
| `bench_loaded` | 真机 | 带载（整车落地），前馈区间上调 |
|
||||
| `field_updown` | 真机 | 实战验收：升 / 降 / 回零双向，阈值收紧 |
|
||||
|
||||
预设**不能**携带 `current_limit` / `direction` / `gear_ratio`（写了直接报错）——
|
||||
安全与机械约束只来自 `chassis.toml` 或显式 CLI。
|
||||
|
||||
产物（`--out-dir`）：`trials.csv`（全部候选与判定）、`best.toml`（可粘贴的增益片段）、
|
||||
`best-trace.csv`、`report.md`、`summary.json`；台架模式另有 `hil-validation.csv`（同一组增益的重复性复验）。
|
||||
加 `--features plot` 还会输出 `best.svg` 三联图。
|
||||
|
||||
整定工具与守护进程**共用同一份配置 schema**（`chassis-config`），因此可以直接拿实车配置当输入：
|
||||
|
||||
```sh
|
||||
# 用 chassis.toml 里的限流 / 电调 ID / 方向 / 减速比（这些**不参与搜索**），
|
||||
# 打印与现有增益的成本对比，并把结果写回一份新配置
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--config config/chassis.toml.example --motor front_left \
|
||||
--write-config target/chassis.tuned.toml
|
||||
```
|
||||
|
||||
要点：
|
||||
|
||||
- **仿真整定只保证在被控对象模型内最优**。模型里的摩擦 / 延迟是量级估计，且不含测量噪声，
|
||||
因此仿真最优解可能偏激进；交付前必须用同一 `--scenario` 跑一次 `--backend bench` 复验。
|
||||
- 最优解贴到参数空间边界时报告会明确提示——那是"约束在决定结果"，应放宽 `--kp` / `--ki` 再搜。
|
||||
- 台架搜索串行执行（只有一条总线），每次试验后强制 0 电流并静默 `--between-trials-ms`；
|
||||
过速 / 过温 / 掉线 / 超时 / ctrl-c 任一命中即停机并收工。
|
||||
- `current_limit`、`direction`、`gear_ratio` 属于安全与机械约束，**不参与搜索**。
|
||||
- 换一组工况先加**预设**（`config/tune.toml`）而不是复制一长串命令行：
|
||||
`chassis-tune presets` 查看、`--preset-name` 选用、CLI 只覆盖本次要变的项。
|
||||
|
||||
## 部署到目标板（armv7，实测：创龙 T153 / Ubuntu 22.04）
|
||||
|
||||
二进制为 musl 静态链接，不依赖板端 libc，无需交叉工具链：
|
||||
|
||||
```sh
|
||||
just deploy # 编译 + 推送 + mock 试运行（just deploy root@169.254.63.4）
|
||||
just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2）
|
||||
# 或手动：
|
||||
cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
scp target/armv7-unknown-linux-musleabihf/release/chassisd root@169.254.63.4:/tmp/
|
||||
scp target/armv7-unknown-linux-musleabihf/release/chassisd root@192.168.2.2:/tmp/
|
||||
```
|
||||
|
||||
板端拉起 CAN（C620 标准 1Mbps；`restart-ms` 让 bus-off 后自动恢复）：
|
||||
板端 CAN 有两条互斥路径：
|
||||
|
||||
```sh
|
||||
ip link set can0 up type can bitrate 1000000 restart-ms 100
|
||||
```
|
||||
- **SocketCAN**（板载原生 CAN 控制器，走 `can0`/`can1`）：
|
||||
`ip link set can0 up type can bitrate 1000000 restart-ms 100`，配 `--interface can0`。
|
||||
- **slcan**（USB 适配器，如 CANable2 在 CDC ACM 下枚举为 `/dev/ttyACM0`）：
|
||||
用 [`dji-esc-slcan`](crates/dji-esc-slcan)（`chassisd` / [`chassis-bench`](crates/chassis-bench) 均可），无需 `ip link`。
|
||||
|
||||
实车运行：`/tmp/chassisd --config /tmp/chassis.toml --interface can0`。板上运行样例见
|
||||
[`deploy/chassisd.service`](deploy/chassisd.service)，配置样例见
|
||||
[`config/chassis.toml.example`](config/chassis.toml.example)。
|
||||
|
||||
> 注意：电调未上电 / CAN 线未接时，控制器会因无 ACK 进入 bus-off（可用
|
||||
> 注意：电调未上电 / CAN 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off（可用
|
||||
> `ip -d link show can0` 查看），chassisd 侧表现为四电机反馈离线、强制零电流——
|
||||
> 这正是安全设计的预期行为。
|
||||
|
||||
## 文档
|
||||
|
||||
- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、测试策略
|
||||
- [**底盘开发文档**](docs/chassis.md)——硬件接线、坐标与单位约定、控制与安全设计、配置参考、
|
||||
**标定与上车流程**、自动整定与超参数预设、台架工具、故障排查（开发 / 调试 / 上车先看这份）
|
||||
- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、被控对象模型、参数空间搜索、测试策略
|
||||
- [整体架构规划](ARCHITECTURE.md)——全系统（含云台 / 发射 / IMU / 实时性）的 crate 划分与阶段计划
|
||||
|
||||
@@ -11,6 +11,9 @@ control_period_ms = 1
|
||||
command_timeout_ms = 200
|
||||
# 电机反馈离线判定（毫秒），离线后强制 0 电流。
|
||||
feedback_timeout_ms = 100
|
||||
# 驱动层命令租约（毫秒）：控制循环超过该时长未刷新电机命令 → 强制 0 电流
|
||||
# （防控制任务死亡后旧指令持续输出）。必须 >= control_period_ms。
|
||||
motor_command_timeout_ms = 50
|
||||
|
||||
[mecanum]
|
||||
# 轮轴线到底盘中心的纵向 / 横向半距（米）与轮半径（米）。
|
||||
|
||||
@@ -0,0 +1,334 @@
|
||||
# chassis-tune 超参数预设
|
||||
#
|
||||
# 一次整定的结果不只取决于被控对象与种子，还取决于"搜什么区间、用哪个场景、
|
||||
# 什么算好、什么算达标、搜多细"。这份文件把这些**实验条件**固化下来：
|
||||
# 评审一次、长期复用、任何机器上都能复现同一组默认行为。
|
||||
#
|
||||
# 用法：
|
||||
# chassis-tune presets # 列出本文件全部预设
|
||||
# chassis-tune sweep --preset config/tune.toml # 用 default_preset
|
||||
# chassis-tune sweep --preset config/tune.toml --preset-name bench_loaded
|
||||
# chassis-tune sweep --preset-name bench_loaded # 缺省路径即本文件
|
||||
# chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预设的单项
|
||||
#
|
||||
# 优先级：内置默认 < 本文件的预设 < 显式命令行参数（逐字段覆盖）。
|
||||
# 本文件里没写的项一律沿用内置默认，因此预设只需写"与默认不同"的部分——
|
||||
# 但为了让评审者一眼看全工况，下面每个预设都写全了关键项。
|
||||
#
|
||||
# **安全边界**：current_limit / direction / gear_ratio / 电调 ID / 串口设备名
|
||||
# 不属于超参数，本文件**不接受**这些键（写了会直接报错）。它们只来自
|
||||
# chassis.toml 或显式 CLI——否则预设文件就成了绕过限流的口子。
|
||||
#
|
||||
# schema：见 crates/chassis-tune/src/preset.rs（version = 1，未知键报错）。
|
||||
|
||||
version = 1
|
||||
default_preset = "sim_bench_model"
|
||||
|
||||
# ---------------------------------------------------------------- 离线：台架量级模型
|
||||
# 默认预设。评审、CI 冒烟、改完代码回归都用它：不碰硬件，结果可复现。
|
||||
[preset.sim_bench_model]
|
||||
description = "离线 · 台架量级模型（静/库仑摩擦 + 2 拍延迟）：全量搜索，评审与回归的基准"
|
||||
|
||||
[preset.sim_bench_model.space]
|
||||
# kp 覆盖手调基线 2 与仿真最优 8 两侧；对数刻度保证小增益也被采样到。
|
||||
kp = { min = 0.5, max = 30.0, scale = "log" }
|
||||
# ki 允许精确取 0（纯比例在某些工况已经够用）。
|
||||
ki = { min = 0.1, max = 100.0, scale = "log0" }
|
||||
# kd 半概率取 0：速度环加微分对量化噪声敏感，先让搜索自己决定要不要。
|
||||
kd = { min = 0.01, max = 3.0, scale = "log0" }
|
||||
# 前馈上限 2.5 覆盖带载所需的 ≈1.77（raw/rpm）。
|
||||
feedforward = { min = 0.01, max = 2.5, scale = "log0" }
|
||||
|
||||
[preset.sim_bench_model.scenario]
|
||||
profile = "ramp-and-hold"
|
||||
target_rpm = 1500.0
|
||||
ramp_ms = 200.0
|
||||
hold_ms = 1800.0
|
||||
dt_us = 1000.0
|
||||
|
||||
[preset.sim_bench_model.cost]
|
||||
# 跟得上、不过冲最重要；电流只作次要惩罚（省电流不是目的，避免烧电调才是）。
|
||||
settle = 1.0
|
||||
overshoot = 1.0
|
||||
iae = 0.5
|
||||
effort = 0.1
|
||||
|
||||
[preset.sim_bench_model.acceptance]
|
||||
max_overshoot_pct = 20.0
|
||||
max_ss_error_pct = 5.0
|
||||
|
||||
[preset.sim_bench_model.search]
|
||||
seed = 20260101
|
||||
samples = 512
|
||||
top_k = 8
|
||||
solver = "nelder-mead"
|
||||
max_iters = 200
|
||||
particles = 24
|
||||
max_evals = 4000
|
||||
threads = 0
|
||||
|
||||
[preset.sim_bench_model.sim]
|
||||
plant = "gm3508-bench"
|
||||
# 域随机化：单点模型调出来的增益对增益/时间常数/延迟高度敏感，
|
||||
# 因此默认在 8 个成员的集成上做鲁棒优化，而不是对着一条理想曲线最优化。
|
||||
ensemble = 8
|
||||
robust_weight = 0.5
|
||||
gain_pct = 0.2
|
||||
tau_pct = 0.3
|
||||
delay_ticks_max = 3
|
||||
friction_pct = 0.5
|
||||
|
||||
[preset.sim_bench_model.bench]
|
||||
# 离线预设不用台架，但安全包络仍写全，方便临时切 --backend bench 预演。
|
||||
max_rpm = 3000.0
|
||||
max_temp_c = 70
|
||||
trial_timeout_ms = 6000
|
||||
between_trials_ms = 500
|
||||
startup_timeout_ms = 5000
|
||||
continue_on_abort = false
|
||||
validate_runs = 3
|
||||
|
||||
# ---------------------------------------------------------------- 离线：理想模型
|
||||
# 无摩擦、无延迟的一阶模型：跑得快，适合改完控制逻辑后的秒级回归，
|
||||
# **不要**拿它的结果直接上车（模型里没有摩擦与延迟，最优解会偏激进）。
|
||||
[preset.sim_ideal]
|
||||
description = "离线 · 理想一阶模型（无摩擦无延迟）：秒级回归用，结果不可直接上车"
|
||||
|
||||
[preset.sim_ideal.space]
|
||||
kp = { min = 0.5, max = 30.0, scale = "log" }
|
||||
ki = { min = 0.1, max = 100.0, scale = "log0" }
|
||||
kd = { min = 0.01, max = 3.0, scale = "log0" }
|
||||
feedforward = { min = 0.01, max = 2.5, scale = "log0" }
|
||||
|
||||
[preset.sim_ideal.scenario]
|
||||
profile = "ramp-and-hold"
|
||||
target_rpm = 1500.0
|
||||
ramp_ms = 200.0
|
||||
hold_ms = 1800.0
|
||||
dt_us = 1000.0
|
||||
|
||||
[preset.sim_ideal.cost]
|
||||
settle = 1.0
|
||||
overshoot = 1.0
|
||||
iae = 0.5
|
||||
effort = 0.1
|
||||
|
||||
[preset.sim_ideal.acceptance]
|
||||
max_overshoot_pct = 20.0
|
||||
max_ss_error_pct = 5.0
|
||||
|
||||
[preset.sim_ideal.search]
|
||||
seed = 20260101
|
||||
samples = 128
|
||||
top_k = 4
|
||||
solver = "nelder-mead"
|
||||
max_iters = 100
|
||||
particles = 16
|
||||
max_evals = 1000
|
||||
threads = 0
|
||||
|
||||
[preset.sim_ideal.sim]
|
||||
plant = "gm3508"
|
||||
# 理想模型本身就是确定的，单成员即可。
|
||||
ensemble = 1
|
||||
robust_weight = 0.0
|
||||
gain_pct = 0.0
|
||||
tau_pct = 0.0
|
||||
delay_ticks_max = 0
|
||||
friction_pct = 0.0
|
||||
|
||||
[preset.sim_ideal.bench]
|
||||
max_rpm = 3000.0
|
||||
max_temp_c = 70
|
||||
trial_timeout_ms = 6000
|
||||
between_trials_ms = 500
|
||||
startup_timeout_ms = 5000
|
||||
continue_on_abort = false
|
||||
validate_runs = 3
|
||||
|
||||
# ---------------------------------------------------------------- 台架：空载
|
||||
# 轮子架空（无地面载荷）。摩擦小、静差靠积分压制，前馈**必须为 0**：
|
||||
# 空载加前馈会把轮子直接推到最高转速。目标转速也压低，便于观察与急停。
|
||||
[preset.bench_noload]
|
||||
description = "真机台架 · 空载（轮子架空）：小样本局部搜索，禁用前馈，目标 800 rpm"
|
||||
# 真机后端：仍然需要 --i-know-the-wheel-spins 才会真的驱动电机。
|
||||
backend = "bench"
|
||||
|
||||
[preset.bench_noload.space]
|
||||
# 空载好解在手调基线（kp≈2, ki≈10）附近，区间收紧以提高样本密度。
|
||||
kp = { min = 0.2, max = 12.0, scale = "log" }
|
||||
ki = { min = 0.1, max = 50.0, scale = "log0" }
|
||||
kd = { min = 0.01, max = 1.0, scale = "log0" }
|
||||
# 固定为 0：单点区间 + 线性刻度 = 搜索时该轴恒定。
|
||||
feedforward = { min = 0.0, max = 0.0, scale = "linear" }
|
||||
|
||||
[preset.bench_noload.scenario]
|
||||
profile = "ramp-and-hold"
|
||||
target_rpm = 800.0
|
||||
ramp_ms = 300.0
|
||||
hold_ms = 1700.0
|
||||
dt_us = 1000.0
|
||||
|
||||
[preset.bench_noload.cost]
|
||||
settle = 1.0
|
||||
overshoot = 1.0
|
||||
iae = 0.5
|
||||
effort = 0.1
|
||||
|
||||
[preset.bench_noload.acceptance]
|
||||
# 空载量化噪声相对目标更大，放宽到 25%，避免"为了 0.5% 的稳态误差过冲"。
|
||||
max_overshoot_pct = 25.0
|
||||
max_ss_error_pct = 8.0
|
||||
|
||||
[preset.bench_noload.search]
|
||||
seed = 20260101
|
||||
# 只有一条总线，每次试验 2s，24 次 ≈ 1 分钟：够做局部精修，不够铺满空间。
|
||||
samples = 24
|
||||
top_k = 3
|
||||
solver = "nelder-mead"
|
||||
max_iters = 40
|
||||
particles = 12
|
||||
max_evals = 40
|
||||
threads = 1
|
||||
|
||||
[preset.bench_noload.sim]
|
||||
# 真机后端不用这些，但保留"最接近台架的模型"，方便先离线预演再上车。
|
||||
plant = "gm3508-bench"
|
||||
ensemble = 8
|
||||
robust_weight = 0.5
|
||||
gain_pct = 0.2
|
||||
tau_pct = 0.3
|
||||
delay_ticks_max = 3
|
||||
friction_pct = 0.5
|
||||
|
||||
[preset.bench_noload.bench]
|
||||
# 目标是 800，过速保护留足裕量但远低于电调上限。
|
||||
max_rpm = 2000.0
|
||||
max_temp_c = 60
|
||||
trial_timeout_ms = 6000
|
||||
between_trials_ms = 500
|
||||
startup_timeout_ms = 5000
|
||||
continue_on_abort = false
|
||||
# 同一组增益跑 3 次看重复性；离散度大说明参数贴着噪声，不该上车。
|
||||
validate_runs = 3
|
||||
|
||||
# ---------------------------------------------------------------- 台架：带载
|
||||
# 整车落地 / 配重（有地面摩擦与惯量）。静差明显，前馈开始有意义（覆盖 ≈1.77）。
|
||||
[preset.bench_loaded]
|
||||
description = "真机台架 · 带载（整车落地/配重）：前馈区间上调，目标 1200 rpm，超调收紧"
|
||||
backend = "bench"
|
||||
|
||||
[preset.bench_loaded.space]
|
||||
kp = { min = 0.5, max = 30.0, scale = "log" }
|
||||
ki = { min = 0.5, max = 100.0, scale = "log0" }
|
||||
kd = { min = 0.01, max = 3.0, scale = "log0" }
|
||||
# 带载需要前馈抵消摩擦：下限不为 0，让搜索必须考虑它。
|
||||
feedforward = { min = 0.5, max = 2.5, scale = "log" }
|
||||
|
||||
[preset.bench_loaded.scenario]
|
||||
profile = "ramp-and-hold"
|
||||
target_rpm = 1200.0
|
||||
ramp_ms = 300.0
|
||||
hold_ms = 2200.0
|
||||
dt_us = 1000.0
|
||||
|
||||
[preset.bench_loaded.cost]
|
||||
settle = 1.0
|
||||
overshoot = 1.0
|
||||
iae = 0.5
|
||||
effort = 0.1
|
||||
|
||||
[preset.bench_loaded.acceptance]
|
||||
# 带载下超调意味着实际位移过冲，收紧到 15%。
|
||||
max_overshoot_pct = 15.0
|
||||
max_ss_error_pct = 5.0
|
||||
|
||||
[preset.bench_loaded.search]
|
||||
seed = 20260101
|
||||
samples = 32
|
||||
top_k = 4
|
||||
solver = "nelder-mead"
|
||||
max_iters = 60
|
||||
particles = 16
|
||||
max_evals = 80
|
||||
threads = 1
|
||||
|
||||
[preset.bench_loaded.sim]
|
||||
plant = "gm3508-bench"
|
||||
ensemble = 8
|
||||
robust_weight = 0.5
|
||||
gain_pct = 0.2
|
||||
tau_pct = 0.3
|
||||
delay_ticks_max = 3
|
||||
friction_pct = 0.5
|
||||
|
||||
[preset.bench_loaded.bench]
|
||||
max_rpm = 2500.0
|
||||
max_temp_c = 70
|
||||
trial_timeout_ms = 8000
|
||||
between_trials_ms = 800
|
||||
startup_timeout_ms = 5000
|
||||
continue_on_abort = false
|
||||
validate_runs = 3
|
||||
|
||||
# ---------------------------------------------------------------- 实战验收：双向
|
||||
# 上车前的最后一道关：升速 → 保持 → 降速 → 保持（回零），验的是双向跟随与
|
||||
# 回零能力，比单向斜坡更接近实战。样本很少（围绕现有增益做局部复算），
|
||||
# 因为此时**不该**再大改增益，只想确认现有一组在两个方向上都不出问题。
|
||||
[preset.field_updown]
|
||||
description = "真机实战验收 · 升降速双向（回零）：小样本复验，超调与稳态误差收紧"
|
||||
backend = "bench"
|
||||
|
||||
[preset.field_updown.space]
|
||||
kp = { min = 0.5, max = 30.0, scale = "log" }
|
||||
ki = { min = 0.5, max = 100.0, scale = "log0" }
|
||||
kd = { min = 0.01, max = 3.0, scale = "log0" }
|
||||
feedforward = { min = 0.5, max = 2.5, scale = "log" }
|
||||
|
||||
[preset.field_updown.scenario]
|
||||
profile = "up-down"
|
||||
target_rpm = 1200.0
|
||||
ramp_ms = 400.0
|
||||
hold_ms = 800.0
|
||||
dt_us = 1000.0
|
||||
|
||||
[preset.field_updown.cost]
|
||||
# 双向场景里稳态误差（尤其回零）比调节时间更重要。
|
||||
settle = 1.0
|
||||
overshoot = 1.0
|
||||
iae = 1.0
|
||||
effort = 0.1
|
||||
|
||||
[preset.field_updown.acceptance]
|
||||
max_overshoot_pct = 12.0
|
||||
max_ss_error_pct = 3.0
|
||||
|
||||
[preset.field_updown.search]
|
||||
seed = 20260101
|
||||
samples = 16
|
||||
top_k = 2
|
||||
solver = "nelder-mead"
|
||||
max_iters = 30
|
||||
particles = 12
|
||||
max_evals = 40
|
||||
threads = 1
|
||||
|
||||
[preset.field_updown.sim]
|
||||
plant = "gm3508-bench"
|
||||
ensemble = 8
|
||||
robust_weight = 0.5
|
||||
gain_pct = 0.2
|
||||
tau_pct = 0.3
|
||||
delay_ticks_max = 3
|
||||
friction_pct = 0.5
|
||||
|
||||
[preset.field_updown.bench]
|
||||
max_rpm = 2500.0
|
||||
max_temp_c = 70
|
||||
# 双向场景总时长翻倍，超时也翻倍。
|
||||
trial_timeout_ms = 10000
|
||||
between_trials_ms = 1000
|
||||
startup_timeout_ms = 5000
|
||||
continue_on_abort = false
|
||||
# 验收阶段多跑几次，重复性不过关就不签字。
|
||||
validate_runs = 5
|
||||
@@ -0,0 +1,32 @@
|
||||
[package]
|
||||
name = "chassis-bench"
|
||||
description = "Bench harness: stimulus, safety envelope, trial sampling, and the bench binaries"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
# 二进制从 `src/bin/` 自动发现：
|
||||
# - `chassis-spin`：四轮定速旋转（slcan，全平台）
|
||||
# - `socketcan-spin`：单电机验证（SocketCAN，仅 Linux）
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
dji-esc-slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
dji-esc-socketcan = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,554 @@
|
||||
//! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 500 rpm）。
|
||||
//!
|
||||
//! ```sh
|
||||
//! # T153 板载 CANable2（slcan 固件）→ /dev/ttyACM0，四轮 500 rpm 转子，缓升 1s 保持 10s
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- \
|
||||
//! --device /dev/ttyACM0 --rpm 500
|
||||
//!
|
||||
//! # 只验证链路与反馈，不发任何非零电流（接线排查用）
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
//! ```
|
||||
//!
|
||||
//! # 抽象边界
|
||||
//!
|
||||
//! 本工具是**组合根**：`dji-esc-slcan`（链路桥接）→ `dji-esc`（C620 协议 +
|
||||
//! 速度环）→ `motor-core`（`VelocityDrive` / `MotorFeedback` / `Disable` 能力
|
||||
//! trait）。激励场景与安全包络来自同 crate 的 `chassis-bench` 库，与
|
||||
//! `chassis-tune` 的真机后端**共用同一套实现**。
|
||||
//!
|
||||
//! # 命令租约（重要）
|
||||
//!
|
||||
//! `EscBus` 的命令是**租约**：`command_timeout`（默认 50ms）内没有重新下发命令，
|
||||
//! 驱动会强制 0 电流。所以控制循环必须**逐拍重发**同一个设定值——本工具在 1kHz
|
||||
//! 节拍里重发（与 `chassisd` 的控制循环一致）。只在开头下发一次是**不行的**。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 轮子必须架空或整车固定，急停可及。默认低限流、有限时长、ctrl-c 任意时刻
|
||||
//! 安全停机（先 0 电流再关总线）；未收到全部反馈前绝不下发速度。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::process::ExitCode;
|
||||
use std::time::Duration;
|
||||
|
||||
use chassis_bench::{SafetyEnvelope, SafetyMonitor, Scenario};
|
||||
use clap::{Parser, ValueEnum};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_slcan::{CanSpeed, SlcanOptions, open_link_with};
|
||||
use motor_core::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval, sleep};
|
||||
|
||||
/// 稳态判据：平均转子转速与目标的相对误差上限。
|
||||
const TOLERANCE: f32 = 0.05;
|
||||
/// 状态打印周期。
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(200);
|
||||
/// 反馈新鲜度阈值：`feedback_age` 超过它即视为该电调掉线。
|
||||
const FRESH: Duration = Duration::from_millis(100);
|
||||
/// 过速需要持续这么久才判定为失控并停机（避免单帧噪声误触发）。
|
||||
const OVERSPEED_GRACE: Duration = Duration::from_millis(60);
|
||||
/// 控制节拍（C620 期望 1kHz 命令）。
|
||||
const CONTROL_PERIOD: Duration = Duration::from_millis(1);
|
||||
/// 四轮默认名（顺序固定 FL、FR、RL、RR）。
|
||||
const WHEEL_NAMES: [&str; 4] = ["front_left", "front_right", "rear_left", "rear_right"];
|
||||
|
||||
#[derive(Debug, Parser)]
|
||||
#[command(name = "chassis-spin", version, about = "台架：四轮同时旋转（默认 500 rpm 转子）")]
|
||||
struct Args {
|
||||
/// slcan 串口设备（T153 板载 CANable2 为 /dev/ttyACM0）。
|
||||
#[arg(long, default_value = "/dev/ttyACM0")]
|
||||
device: String,
|
||||
|
||||
/// 串口波特率（CDC ACM 一般忽略；CH340 类适配器需与固件一致）。
|
||||
#[arg(long, default_value_t = 2_000_000)]
|
||||
serial_baud: u32,
|
||||
|
||||
/// 串口超时（毫秒）。读写共用：`serial` 的 `TTYPort::write` 也按它 poll(POLLOUT)。
|
||||
#[arg(long, default_value_t = 50)]
|
||||
serial_timeout_ms: u64,
|
||||
|
||||
/// CAN 波特率档位（C620 标准 = 1Mbps）。
|
||||
#[arg(long, value_enum, default_value_t = CanSpeedArg::M1)]
|
||||
can_speed: CanSpeedArg,
|
||||
|
||||
/// 电调 ID 列表，顺序 = FL、FR、RL、RR。
|
||||
#[arg(long, value_delimiter = ',', default_value = "1,2,3,4")]
|
||||
esc_ids: Vec<u8>,
|
||||
|
||||
/// 各轮安装方向 ±1（默认左右镜像 → 四轮物理同向）。
|
||||
#[arg(long, value_delimiter = ',', allow_hyphen_values = true, default_value = "1,-1,1,-1")]
|
||||
directions: Vec<f32>,
|
||||
|
||||
/// 目标**转子**转速（rpm）；轮端 = 转子 ÷ 减速比。
|
||||
#[arg(long, default_value_t = 500.0)]
|
||||
rpm: f32,
|
||||
|
||||
/// 减速比（转子 : 输出轴，GM3508 ≈ 19.203）。
|
||||
#[arg(long, default_value_t = 19.203)]
|
||||
gear_ratio: f32,
|
||||
|
||||
/// 电流限幅（raw，C620 满量程 16384）。
|
||||
///
|
||||
/// 默认 1500 raw（≈1.8A）：要越过静摩擦（实测约 400~600 raw）且留出整定余量。
|
||||
/// 上限同时也是 PID 抗饱和的钳位点——**必须贴近实际所需电流**，
|
||||
/// 否则积分会在堵转期间积累，一旦起动就冲过头。
|
||||
#[arg(long, default_value_t = 1500)]
|
||||
current_limit: i16,
|
||||
|
||||
/// 缓升时间：0 → 目标转速（毫秒）。
|
||||
///
|
||||
/// 默认 3000：空载 GM3508 的静摩擦需要约 400~600 raw 才能起动（stick-slip），
|
||||
/// 缓升让积分在起动前平滑建立，避免"先憋住、一动就飞"。
|
||||
#[arg(long, default_value_t = 3000)]
|
||||
ramp_ms: u64,
|
||||
|
||||
/// 目标转速保持时长（秒）。
|
||||
#[arg(long, default_value_t = 10.0)]
|
||||
hold_secs: f32,
|
||||
|
||||
/// 等待全部电调反馈的超时（毫秒）。
|
||||
#[arg(long, default_value_t = 3000)]
|
||||
startup_timeout_ms: u64,
|
||||
|
||||
/// 只验证链路与反馈，不发出任何非零电流。
|
||||
#[arg(long)]
|
||||
dry_run: bool,
|
||||
|
||||
/// 允许缺失部分电调：仅驱动有反馈的电调继续测试（默认要求四个电调全部在线）。
|
||||
#[arg(long)]
|
||||
allow_partial: bool,
|
||||
|
||||
/// 速度环 kp（raw/rpm）。
|
||||
///
|
||||
/// 空载台架默认 2.0：被控对象近似积分器（≈12 rpm/s per raw），
|
||||
/// 高 kp（如 8~18）会因反馈延迟震荡；低 kp 配合大 ki 才稳。
|
||||
#[arg(long, default_value_t = 2.0)]
|
||||
kp: f32,
|
||||
/// 速度环 ki（raw/(rpm·s)）。
|
||||
///
|
||||
/// 空载台架默认 10：静摩擦需要积分项缓慢"顶"到起动电流，ki 太小会长期
|
||||
/// 停在 300~400 rpm 达不到目标；抗饱和由 `current_limit` 钳位兜底，
|
||||
/// 所以大 ki 不会跑飞。
|
||||
#[arg(long, default_value_t = 10.0)]
|
||||
ki: f32,
|
||||
/// 速度环 kd。
|
||||
#[arg(long, default_value_t = 0.0)]
|
||||
kd: f32,
|
||||
/// 速度前馈（raw/rpm）。
|
||||
///
|
||||
/// **空载台架默认 0**：轮子架空时维持转速只需要克服摩擦的极小电流，
|
||||
/// 任何接近 1/K（GM3508 空载 K ≈ 0.57 rpm/raw → 1/K ≈ 1.77）的前馈都会把
|
||||
/// 空载轮子直接推到最高转速。只有带载工况才需要正前馈。
|
||||
#[arg(long, default_value_t = 0.0)]
|
||||
feedforward: f32,
|
||||
|
||||
/// 过速保护上限（rpm）：任一参与测试的电调超过它即立即停机。
|
||||
/// 默认 = 目标 × 2 + 200。
|
||||
#[arg(long)]
|
||||
max_rpm: Option<f32>,
|
||||
}
|
||||
|
||||
/// CAN 波特率档位（对应 slcan 的 `Sn` 命令）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)]
|
||||
enum CanSpeedArg {
|
||||
/// `S8`：1 Mbps。
|
||||
#[value(name = "1m")]
|
||||
M1,
|
||||
/// `S6`：500 kbps。
|
||||
#[value(name = "500k")]
|
||||
K500,
|
||||
}
|
||||
|
||||
impl From<CanSpeedArg> for CanSpeed {
|
||||
fn from(value: CanSpeedArg) -> Self {
|
||||
match value {
|
||||
CanSpeedArg::M1 => Self::M1,
|
||||
CanSpeedArg::K500 => Self::K500,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Args {
|
||||
/// 过速保护上限：显式 `--max-rpm` 优先，否则目标 × 2 + 200。
|
||||
fn overspeed_limit(&self) -> f32 {
|
||||
self.max_rpm.unwrap_or(self.rpm * 2.0 + 200.0)
|
||||
}
|
||||
|
||||
/// 参数自检：越界参数在碰硬件之前就拒绝。
|
||||
fn validate(&self) -> Result<(), String> {
|
||||
if self.esc_ids.is_empty() {
|
||||
return Err("--esc-ids 不能为空".into());
|
||||
}
|
||||
if self.esc_ids.len() != self.directions.len() {
|
||||
return Err(format!(
|
||||
"--esc-ids（{} 个）与 --directions（{} 个）数量必须一致",
|
||||
self.esc_ids.len(),
|
||||
self.directions.len()
|
||||
));
|
||||
}
|
||||
for &id in &self.esc_ids {
|
||||
if !(1..=8).contains(&id) {
|
||||
return Err(format!("电调 ID {id} 超出 1..=8"));
|
||||
}
|
||||
}
|
||||
for &direction in &self.directions {
|
||||
let bits = direction.to_bits();
|
||||
if bits != 1.0f32.to_bits() && bits != (-1.0f32).to_bits() {
|
||||
return Err(format!("方向必须是 +1 或 -1，得到 {direction}"));
|
||||
}
|
||||
}
|
||||
if !(self.rpm.is_finite() && self.rpm >= 0.0) {
|
||||
return Err(format!("--rpm 必须 ≥ 0，得到 {}", self.rpm));
|
||||
}
|
||||
if let Some(max) = self.max_rpm
|
||||
&& !(max.is_finite() && max > self.rpm)
|
||||
{
|
||||
return Err(format!("--max-rpm 必须大于目标转速 {}，得到 {max}", self.rpm));
|
||||
}
|
||||
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
|
||||
return Err(format!("--gear-ratio 必须 > 0，得到 {}", self.gear_ratio));
|
||||
}
|
||||
if !(1..=16384).contains(&self.current_limit) {
|
||||
return Err(format!("--current-limit 必须在 1..=16384，得到 {}", self.current_limit));
|
||||
}
|
||||
if self.hold_secs < 0.0 {
|
||||
return Err(format!("--hold-secs 不能为负，得到 {}", self.hold_secs));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 四个电调配置（方向、减速比、电流限幅、PID 全部显式落地）。
|
||||
fn esc_configs(&self) -> Vec<EscConfig> {
|
||||
self.esc_ids
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(index, &id)| {
|
||||
let name = WHEEL_NAMES
|
||||
.get(index)
|
||||
.map_or_else(|| format!("wheel{id}"), |name| (*name).to_owned());
|
||||
let mut cfg = EscConfig::gm3508(name, id, self.directions[index]);
|
||||
cfg.gear_ratio = self.gear_ratio;
|
||||
cfg.current_limit = self.current_limit;
|
||||
cfg.kp = self.kp;
|
||||
cfg.ki = self.ki;
|
||||
cfg.kd = self.kd;
|
||||
cfg.feedforward = self.feedforward;
|
||||
cfg
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
fn bus_options() -> BusOptions {
|
||||
BusOptions {
|
||||
period: CONTROL_PERIOD,
|
||||
feedback_timeout: Duration::from_millis(100),
|
||||
// 命令租约 50ms；控制循环每 1ms 续租。
|
||||
command_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> ExitCode {
|
||||
init_tracing();
|
||||
let args = Args::parse();
|
||||
if let Err(problem) = args.validate() {
|
||||
eprintln!("✗ 参数错误：{problem}");
|
||||
return ExitCode::FAILURE;
|
||||
}
|
||||
print_banner(&args);
|
||||
|
||||
match run(&args).await {
|
||||
Ok(true) => ExitCode::SUCCESS,
|
||||
Ok(false) => ExitCode::FAILURE,
|
||||
Err(e) => {
|
||||
eprintln!("✗ 运行失败：{e}");
|
||||
ExitCode::FAILURE
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 初始化日志：链路桥的读写失败/静默告警必须可见，
|
||||
/// 否则现场只剩一句"没有反馈"，无法区分适配器故障与整车接线问题。
|
||||
fn init_tracing() {
|
||||
let filter = tracing_subscriber::EnvFilter::try_from_default_env()
|
||||
.unwrap_or_else(|_| tracing_subscriber::EnvFilter::new("warn,dji_esc_slcan=debug"));
|
||||
let _ = tracing_subscriber::fmt().with_env_filter(filter).try_init();
|
||||
}
|
||||
|
||||
fn print_banner(args: &Args) {
|
||||
println!("=== 台架：四轮同时旋转（转子目标 {:.1} rpm）===", args.rpm);
|
||||
println!(
|
||||
"设备 {} | CAN {:?} | 电调 {:?} | 方向 {:?} | 减速比 {:.3}",
|
||||
args.device, args.can_speed, args.esc_ids, args.directions, args.gear_ratio
|
||||
);
|
||||
println!(
|
||||
"转子 {:.1} rpm ÷ {:.3} = 轮端 {:.2} rpm | 电流限幅 {} raw | 缓升 {}ms | 保持 {:.1}s{}",
|
||||
args.rpm,
|
||||
args.gear_ratio,
|
||||
args.rpm / args.gear_ratio,
|
||||
args.current_limit,
|
||||
args.ramp_ms,
|
||||
args.hold_secs,
|
||||
if args.dry_run {
|
||||
" | dry-run"
|
||||
} else if args.allow_partial {
|
||||
" | allow-partial"
|
||||
} else {
|
||||
""
|
||||
}
|
||||
);
|
||||
if args.dry_run {
|
||||
println!("ℹ dry-run：只读反馈，不发出非零电流。\n");
|
||||
} else {
|
||||
println!(
|
||||
"速度环 kp={} ki={} kd={} ff={} | 过速保护 {:.0} rpm",
|
||||
args.kp,
|
||||
args.ki,
|
||||
args.kd,
|
||||
args.feedforward,
|
||||
args.overspeed_limit()
|
||||
);
|
||||
println!("⚠ 轮子必须架空或整车固定，急停可及；ctrl-c 随时安全停机。\n");
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开链路 → 装配协议 bus → 执行 → **任何路径都先停电流再关总线**。
|
||||
async fn run(args: &Args) -> Result<bool, Box<dyn std::error::Error>> {
|
||||
let options = SlcanOptions {
|
||||
serial_baud: args.serial_baud,
|
||||
speed: args.can_speed.into(),
|
||||
// 读写共用该超时（serial crate 的 TTYPort::write 也先 poll(POLLOUT)）：
|
||||
// 设太小会把"这一拍写不出去"误判成写失败。总线繁忙时读立即返回，
|
||||
// 放大它只影响总线完全静默时的下行延迟。
|
||||
read_timeout: Duration::from_millis(args.serial_timeout_ms),
|
||||
};
|
||||
let link = open_link_with(&args.device, options).map_err(|e| {
|
||||
format!("打开 slcan 设备 {} 失败：{e}（检查设备路径 / 权限 / 是否被占用）", args.device)
|
||||
})?;
|
||||
println!("✓ slcan 已打开：{}（CAN {:?}）", args.device, args.can_speed);
|
||||
|
||||
let bus = EscBus::spawn(link, args.esc_configs(), bus_options())?;
|
||||
let outcome = drive(args, &bus).await;
|
||||
|
||||
// 安全收尾：先 0 电流并让它至少发出一拍，再关总线。
|
||||
for motor in bus.motors() {
|
||||
motor.disable();
|
||||
}
|
||||
sleep(Duration::from_millis(50)).await;
|
||||
bus.shutdown().await;
|
||||
println!("已停机（0 电流）并关闭总线");
|
||||
outcome
|
||||
}
|
||||
|
||||
async fn drive(args: &Args, bus: &EscBusHandle) -> Result<bool, Box<dyn std::error::Error>> {
|
||||
let motors = bus.motors();
|
||||
wait_for_feedback(motors, Duration::from_millis(args.startup_timeout_ms)).await;
|
||||
|
||||
let targets: Vec<EscMotor> = motors.iter().filter(|m| is_fresh(m)).cloned().collect();
|
||||
let missing: Vec<u8> = motors.iter().filter(|m| !is_fresh(m)).map(MotorFeedback::id).collect();
|
||||
|
||||
if !missing.is_empty() {
|
||||
eprintln!(
|
||||
"✗ 以下电调没有新鲜反馈：{missing:?}（收到 {:?}）",
|
||||
motors.iter().map(MotorFeedback::feedback_age).collect::<Vec<_>>()
|
||||
);
|
||||
eprintln!(
|
||||
" 排查：CAN 波特率（C620 = 1Mbps = S8）| CANH/CANL 是否接反 | 120Ω 终端电阻 | 电调供电 | 电调 CAN ID 拨码 | 设备 {}",
|
||||
args.device
|
||||
);
|
||||
if !args.allow_partial {
|
||||
eprintln!(" 若确认台架只接了部分电调，可加 --allow-partial 降级测试。");
|
||||
return Ok(false);
|
||||
}
|
||||
eprintln!("⚠ --allow-partial：仅驱动有反馈的电调继续测试。\n");
|
||||
}
|
||||
if targets.is_empty() {
|
||||
eprintln!("✗ 没有任何电调在反馈，无法测试。");
|
||||
return Ok(false);
|
||||
}
|
||||
println!(
|
||||
"✓ 参与测试的电调：{:?}（新鲜度 < {FRESH:?}）\n",
|
||||
targets.iter().map(MotorFeedback::id).collect::<Vec<_>>()
|
||||
);
|
||||
|
||||
if args.dry_run {
|
||||
return Ok(dry_run(&targets).await);
|
||||
}
|
||||
Ok(spin(args, &targets).await)
|
||||
}
|
||||
|
||||
/// 该电调是否有**新鲜**反馈（而非历史上有过反馈）。
|
||||
fn is_fresh(motor: &EscMotor) -> bool {
|
||||
motor.feedback_age().is_some_and(|age| age <= FRESH)
|
||||
}
|
||||
|
||||
/// 上电门控：等到每个电调都有一帧**新鲜**反馈；超时即返回 `false`。
|
||||
async fn wait_for_feedback(motors: &[EscMotor], timeout: Duration) -> bool {
|
||||
let deadline = Instant::now() + timeout;
|
||||
loop {
|
||||
if motors.iter().all(is_fresh) {
|
||||
return true;
|
||||
}
|
||||
if Instant::now() >= deadline {
|
||||
return false;
|
||||
}
|
||||
sleep(Duration::from_millis(20)).await;
|
||||
}
|
||||
}
|
||||
|
||||
/// dry-run：只观察反馈，不下发任何非零电流（总线仍在发 0 电流帧）。
|
||||
async fn dry_run(targets: &[EscMotor]) -> bool {
|
||||
let mut ticker = interval(REPORT_PERIOD);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = Instant::now() + Duration::from_secs(3);
|
||||
while Instant::now() < deadline {
|
||||
ticker.tick().await;
|
||||
for motor in targets {
|
||||
let st = motor.state();
|
||||
println!(
|
||||
" {:<11} esc{} | 转子 {:>7.1} rpm | 温度 {:>3}℃ | 反馈 {:>6} 帧 | 新鲜度 {:?}",
|
||||
motor.name(),
|
||||
motor.id(),
|
||||
st.rotor_speed_rpm,
|
||||
st.temperature_c,
|
||||
st.feedback_count,
|
||||
motor.feedback_age().map(|age| age.as_millis())
|
||||
);
|
||||
}
|
||||
println!();
|
||||
}
|
||||
println!("✓ 链路与反馈验证完成（未发出非零电流）");
|
||||
true
|
||||
}
|
||||
|
||||
/// 本次台架的安全包络：只开过速保护（`chassis-spin` 的历史行为）。
|
||||
fn envelope(args: &Args) -> SafetyEnvelope {
|
||||
SafetyEnvelope::overspeed_only(args.overspeed_limit(), OVERSPEED_GRACE)
|
||||
}
|
||||
|
||||
/// 缓升 → 保持 → 采样；返回稳态是否达标。
|
||||
async fn spin(args: &Args, motors: &[EscMotor]) -> bool {
|
||||
let ramp = Duration::from_millis(args.ramp_ms);
|
||||
let hold = Duration::from_secs_f32(args.hold_secs);
|
||||
let overspeed_limit = args.overspeed_limit();
|
||||
// 设定值曲线与安全判定都来自 chassis-bench 库：与 chassis-tune 的真机
|
||||
// 后端共用同一套实现，避免"台架测的行为"和"整定优化的行为"分叉。
|
||||
let scenario = Scenario::ramp_and_hold(args.rpm, ramp.as_secs_f32(), hold.as_secs_f32());
|
||||
let mut ticker = interval(CONTROL_PERIOD);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
let start = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope(args), start);
|
||||
let mut next_report = REPORT_PERIOD;
|
||||
let mut samples: Vec<Vec<f32>> = vec![Vec::new(); motors.len()];
|
||||
let mut overspeed_warned = false;
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = tokio::signal::ctrl_c() => {
|
||||
println!("\n收到 ctrl-c，中断测试");
|
||||
return false;
|
||||
}
|
||||
_ = ticker.tick() => {}
|
||||
}
|
||||
let elapsed = start.elapsed();
|
||||
if elapsed >= ramp + hold {
|
||||
break;
|
||||
}
|
||||
// 逐拍重发（= 命令租约续期），斜坡段目标线性上升。
|
||||
let setpoint = scenario.setpoint_at(elapsed.as_secs_f32());
|
||||
for motor in motors {
|
||||
motor.set_velocity(setpoint);
|
||||
}
|
||||
// 过速保护：控制器失稳时立刻停机，别让轮子飞到最高转速。
|
||||
let peak = motors.iter().map(|m| m.state().rotor_speed_rpm.abs()).fold(0.0f32, f32::max);
|
||||
if peak > overspeed_limit && !overspeed_warned {
|
||||
overspeed_warned = true;
|
||||
eprintln!("⚠ 过速 {peak:.0} rpm > 上限 {overspeed_limit:.0} rpm，观察中……");
|
||||
}
|
||||
if let Some(abort) = monitor.check(motors, Instant::now()) {
|
||||
eprintln!(
|
||||
"✗ 过速保护触发：{peak:.0} rpm 超过上限 {overspeed_limit:.0} rpm，立即停机（{abort}）。\n 速度环整定不当（kp/ki/前馈过大）——空载台架前馈应为 0。"
|
||||
);
|
||||
return false;
|
||||
}
|
||||
if elapsed >= ramp {
|
||||
for (slot, motor) in samples.iter_mut().zip(motors) {
|
||||
slot.push(motor.state().rotor_speed_rpm);
|
||||
}
|
||||
}
|
||||
if elapsed >= next_report {
|
||||
report_line(motors, setpoint, elapsed);
|
||||
next_report += REPORT_PERIOD;
|
||||
}
|
||||
}
|
||||
summarize(args, motors, &samples)
|
||||
}
|
||||
|
||||
fn report_line(motors: &[EscMotor], setpoint: f32, elapsed: Duration) {
|
||||
let rotor: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>7.1}", m.state().rotor_speed_rpm)).collect();
|
||||
let current: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>6}", m.state().effort_raw)).collect();
|
||||
let temp: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>3}", m.state().temperature_c)).collect();
|
||||
println!(
|
||||
"[t={:>5.1}s] 目标 {:>7.1} | 转子 {} | 电流 {} | 温度 {}",
|
||||
elapsed.as_secs_f32(),
|
||||
setpoint,
|
||||
rotor.join(" "),
|
||||
current.join(" "),
|
||||
temp.join(" ")
|
||||
);
|
||||
}
|
||||
|
||||
fn summarize(args: &Args, motors: &[EscMotor], samples: &[Vec<f32>]) -> bool {
|
||||
if samples.iter().all(Vec::is_empty) {
|
||||
println!("保持时长为 0，未采集到稳态样本");
|
||||
return true;
|
||||
}
|
||||
println!(
|
||||
"\n=== 稳态结论（目标转子 {:.1} rpm ≈ 轮端 {:.2} rpm）===",
|
||||
args.rpm,
|
||||
args.rpm / args.gear_ratio
|
||||
);
|
||||
let mut all_ok = true;
|
||||
for (index, motor) in motors.iter().enumerate() {
|
||||
let mean = mean(&samples[index]);
|
||||
let error = if args.rpm.abs() > f32::EPSILON {
|
||||
(mean - args.rpm).abs() / args.rpm.abs()
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
let ok = error <= TOLERANCE;
|
||||
all_ok &= ok;
|
||||
println!(
|
||||
" {:<11} esc{} | 稳态 {:>7.1} rpm | 轮端 {:>6.2} rpm | 误差 {:>5.1}% | 电流 {:>6} raw | 温度 {:>3}℃ | {}",
|
||||
motor.name(),
|
||||
motor.id(),
|
||||
mean,
|
||||
mean / args.gear_ratio,
|
||||
error * 100.0,
|
||||
motor.state().effort_raw,
|
||||
motor.state().temperature_c,
|
||||
if ok { "✓" } else { "✗" }
|
||||
);
|
||||
}
|
||||
if all_ok {
|
||||
println!("\n✓ 通过：{} 个轮子均达到目标转速 ±{:.0}%", motors.len(), TOLERANCE * 100.0);
|
||||
} else {
|
||||
println!(
|
||||
"\n✗ 未通过：存在轮速偏差 > {:.0}%，检查 PID / 机械阻力 / 电流限幅 / 方向配置",
|
||||
TOLERANCE * 100.0
|
||||
);
|
||||
}
|
||||
all_ok
|
||||
}
|
||||
|
||||
fn mean(samples: &[f32]) -> f32 {
|
||||
if samples.is_empty() {
|
||||
return 0.0;
|
||||
}
|
||||
samples.iter().sum::<f32>() / samples.len() as f32
|
||||
}
|
||||
+32
-21
@@ -1,14 +1,14 @@
|
||||
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||||
//!
|
||||
//! ```sh
|
||||
//! spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||||
//! cargo run --release -p dji-esc-socketcan --example spin -- can0 1 1000 10
|
||||
//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10
|
||||
//! ```
|
||||
//!
|
||||
//! 关键点在 [`spin_one`]：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||||
//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（本 crate）在此汇合。
|
||||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。
|
||||
//!
|
||||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||||
|
||||
@@ -22,7 +22,7 @@ use dji_esc::{BusOptions, EscBus, EscConfig};
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc_socketcan::open_link;
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor_core::{Disable, EffortDrive, Motor, MotorFeedback, VelocityDrive};
|
||||
use motor_core::Motor;
|
||||
#[cfg(target_os = "linux")]
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
|
||||
@@ -63,13 +63,22 @@ async fn main() {
|
||||
|
||||
/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
|
||||
/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
|
||||
///
|
||||
/// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发
|
||||
/// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值
|
||||
/// （真实底盘的控制循环每 1ms 重发一次）。
|
||||
#[cfg(target_os = "linux")]
|
||||
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||||
const LEASE_PERIOD: Duration = Duration::from_millis(10);
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(100);
|
||||
|
||||
motor.set_velocity(target_rpm);
|
||||
println!("已下发目标转速，开始观测（100ms 周期）……\n");
|
||||
|
||||
let mut ticker = interval(Duration::from_millis(100));
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut lease = interval(LEASE_PERIOD);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut report = interval(REPORT_PERIOD);
|
||||
report.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = tokio::time::Instant::now() + duration;
|
||||
|
||||
loop {
|
||||
@@ -78,24 +87,26 @@ async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||||
println!("\n收到 ctrl-c");
|
||||
break;
|
||||
}
|
||||
_ = ticker.tick() => {}
|
||||
_ = lease.tick() => motor.set_velocity(target_rpm), // 续租
|
||||
_ = report.tick() => {
|
||||
let st = motor.state();
|
||||
if st.has_feedback() {
|
||||
println!(
|
||||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||||
st.rotor_speed_rpm,
|
||||
st.effort_raw,
|
||||
st.temperature_c,
|
||||
st.rotor_revolutions,
|
||||
st.feedback_count
|
||||
);
|
||||
} else {
|
||||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||||
}
|
||||
}
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
break;
|
||||
}
|
||||
let st = motor.state();
|
||||
if st.has_feedback() {
|
||||
println!(
|
||||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||||
st.rotor_speed_rpm,
|
||||
st.effort_raw,
|
||||
st.temperature_c,
|
||||
st.rotor_revolutions,
|
||||
st.feedback_count
|
||||
);
|
||||
} else {
|
||||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||||
}
|
||||
}
|
||||
|
||||
motor.disable();
|
||||
@@ -0,0 +1,27 @@
|
||||
//! 台架工具库：激励场景、安全包络与真机试验采样。
|
||||
//!
|
||||
//! 本 crate 是**台架相关一切的唯一归属**：以前 `chassis-spin` 是
|
||||
//! `dji-esc-slcan` 的一个 example、`spin` 是 `dji-esc-socketcan` 的一个
|
||||
//! example——运输后端 crate 却装着应用级工具。现在：
|
||||
//!
|
||||
//! - 运输 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接；
|
||||
//! - 台架程序与可复用的安全 / 采样逻辑住在这里；
|
||||
//! - `chassis-tune` 的真机后端复用 [`sampler`] 与 [`safety`]，不重复实现。
|
||||
//!
|
||||
//! 二进制：
|
||||
//!
|
||||
//! - `chassis-spin`：四轮定速旋转（slcan，全平台）；
|
||||
//! - `socketcan-spin`：单电机链路验证（SocketCAN，仅 Linux）；
|
||||
//! - [`link`]：两种运输后端的统一门面（台架与真机整定共用）。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod link;
|
||||
pub mod safety;
|
||||
pub mod sampler;
|
||||
|
||||
pub use link::{CanSpeed, SlcanOptions, default_slcan_options, open_slcan, open_socketcan};
|
||||
pub use safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
pub use sampler::{TrialOutcome, run_step_trial, run_step_trial_until_ctrl_c};
|
||||
|
||||
/// 与离线 harness 共用的场景 / 轨迹类型（同一套实验词汇）。
|
||||
pub use dji_esc_sim::{Scenario, SetpointProfile, Trace};
|
||||
@@ -0,0 +1,87 @@
|
||||
//! 链路打开：把两种运输后端收敛成一个门面。
|
||||
//!
|
||||
//! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`CanLink`]"，
|
||||
//! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处，上层就不必
|
||||
//! 同时依赖两个运输 crate。
|
||||
|
||||
use std::io::Result;
|
||||
use std::time::Duration;
|
||||
|
||||
use can_link::CanLink;
|
||||
|
||||
/// slcan 波特率档位（对应协议 `Sn` 命令）。
|
||||
pub use dji_esc_slcan::CanSpeed;
|
||||
/// slcan 打开参数。
|
||||
pub use dji_esc_slcan::SlcanOptions;
|
||||
|
||||
/// 打开 slcan 串口设备（CANable 等），桥接为链路端点。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。
|
||||
pub fn open_slcan(device: &str, options: SlcanOptions) -> Result<CanLink> {
|
||||
dji_esc_slcan::open_link_with(device, options)
|
||||
}
|
||||
|
||||
/// 打开 SocketCAN 接口（Linux）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 未 up / 权限不足时的 `io::Error`；非 Linux 平台恒为错误。
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
dji_esc_socketcan::open_link(interface)
|
||||
}
|
||||
|
||||
/// 非 Linux 平台没有 SocketCAN。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 恒返回 `Unsupported`。
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
Err(std::io::Error::new(
|
||||
std::io::ErrorKind::Unsupported,
|
||||
format!("SocketCAN ({interface}) 仅支持 Linux"),
|
||||
))
|
||||
}
|
||||
|
||||
/// 默认 slcan 参数（1 Mbps、串口超时 50 ms）。
|
||||
#[must_use]
|
||||
pub fn default_slcan_options(serial_baud: u32, speed: CanSpeed) -> SlcanOptions {
|
||||
SlcanOptions { serial_baud, speed, read_timeout: Duration::from_millis(50) }
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn socketcan_is_unavailable_off_linux() {
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
{
|
||||
let err = open_socketcan("can0").expect_err("非 Linux 必须报错");
|
||||
assert_eq!(err.kind(), std::io::ErrorKind::Unsupported);
|
||||
}
|
||||
#[cfg(target_os = "linux")]
|
||||
{
|
||||
// Linux 上接口名不存在时也必须是 Err，而不是 panic。
|
||||
let _ = open_socketcan("definitely-not-a-can-interface");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn slcan_failure_is_an_error_not_a_panic() {
|
||||
let result =
|
||||
open_slcan("/definitely/not/a/device", default_slcan_options(2_000_000, CanSpeed::M1));
|
||||
assert!(result.is_err());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_options_carry_the_requested_baud() {
|
||||
let options = default_slcan_options(115_200, CanSpeed::K500);
|
||||
assert_eq!(options.serial_baud, 115_200);
|
||||
assert_eq!(options.speed, CanSpeed::K500);
|
||||
assert_eq!(options.read_timeout, Duration::from_millis(50));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,322 @@
|
||||
//! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。
|
||||
//!
|
||||
//! 台架工具（`chassis-spin`）与自动整定的真机后端（`chassis-tune --backend bench`）
|
||||
//! 共用同一套判定，避免安全逻辑在两个地方各写一遍、然后慢慢漂移。
|
||||
//!
|
||||
//! 所有阈值都是 `Option`：只开需要的那个。`chassis-spin` 只开过速保护
|
||||
//! （保持历史行为），自动整定则开启全部——因为搜索会主动试错，必须由
|
||||
//! 包络兜住"试错到失控"。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use motor_core::MotorFeedback;
|
||||
// 用 tokio 的时钟：`start_paused` 下虚拟时间可控，安全包络（宽限期 / 超时）
|
||||
// 才能被确定性地测试；生产构建里它就是 `std::time::Instant`。
|
||||
use thiserror::Error;
|
||||
use tokio::time::Instant;
|
||||
|
||||
/// 触发停机的原因。
|
||||
#[derive(Debug, Clone, PartialEq, Error)]
|
||||
pub enum Abort {
|
||||
/// 转速超过上限并持续超过宽限期。
|
||||
#[error("overspeed: {peak_rpm:.0} rpm > {limit_rpm:.0} rpm for over {grace_ms}ms")]
|
||||
Overspeed {
|
||||
/// 观测到的峰值转速（rpm）。
|
||||
peak_rpm: f32,
|
||||
/// 配置的上限（rpm）。
|
||||
limit_rpm: f32,
|
||||
/// 宽限期（毫秒）。
|
||||
grace_ms: u64,
|
||||
},
|
||||
/// 温度超过上限。
|
||||
#[error("overtemperature: {temperature_c}℃ > {limit_c}℃")]
|
||||
OverTemperature {
|
||||
/// 观测温度（℃）。
|
||||
temperature_c: i8,
|
||||
/// 上限（℃）。
|
||||
limit_c: i8,
|
||||
},
|
||||
/// 反馈超出新鲜度阈值（掉线）。
|
||||
#[error("esc {esc_id} feedback lost: age {age_ms:?}ms > {timeout_ms}ms")]
|
||||
FeedbackLost {
|
||||
/// 电调 ID。
|
||||
esc_id: u8,
|
||||
/// 实测反馈年龄（毫秒）。
|
||||
age_ms: Option<u128>,
|
||||
/// 阈值（毫秒）。
|
||||
timeout_ms: u64,
|
||||
},
|
||||
/// 整个试验超时。
|
||||
#[error("trial timeout after {elapsed_ms}ms")]
|
||||
Timeout {
|
||||
/// 已耗时（毫秒）。
|
||||
elapsed_ms: u128,
|
||||
},
|
||||
/// 上层主动取消（ctrl-c）。
|
||||
#[error("cancelled by operator")]
|
||||
Cancelled,
|
||||
}
|
||||
|
||||
/// 安全包络（阈值 + 宽限期）。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct SafetyEnvelope {
|
||||
/// 转速上限（rpm）。
|
||||
pub max_rpm: Option<f32>,
|
||||
/// 温度上限（℃）。
|
||||
pub max_temperature_c: Option<i8>,
|
||||
/// 反馈新鲜度阈值：超过它即视为掉线。
|
||||
pub feedback_timeout: Option<Duration>,
|
||||
/// 过速需要持续多久才判定为失控（抑制单帧噪声）。
|
||||
pub overspeed_grace: Duration,
|
||||
/// 单次试验的总时长上限。
|
||||
pub trial_timeout: Option<Duration>,
|
||||
}
|
||||
|
||||
impl Default for SafetyEnvelope {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_rpm: None,
|
||||
max_temperature_c: None,
|
||||
feedback_timeout: None,
|
||||
overspeed_grace: Duration::from_millis(60),
|
||||
trial_timeout: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SafetyEnvelope {
|
||||
/// 空包络（不判定任何条件）。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 只开过速保护（`chassis-spin` 的历史行为）。
|
||||
#[must_use]
|
||||
pub fn overspeed_only(max_rpm: f32, grace: Duration) -> Self {
|
||||
Self { max_rpm: Some(max_rpm), overspeed_grace: grace, ..Self::default() }
|
||||
}
|
||||
|
||||
/// 开过速保护。
|
||||
#[must_use]
|
||||
pub fn with_overspeed(mut self, max_rpm: f32, grace: Duration) -> Self {
|
||||
self.max_rpm = Some(max_rpm);
|
||||
self.overspeed_grace = grace;
|
||||
self
|
||||
}
|
||||
|
||||
/// 开温度保护。
|
||||
#[must_use]
|
||||
pub fn with_temperature_max(mut self, max_temperature_c: i8) -> Self {
|
||||
self.max_temperature_c = Some(max_temperature_c);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开掉线保护。
|
||||
#[must_use]
|
||||
pub fn with_feedback_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.feedback_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开试验总时长保护。
|
||||
#[must_use]
|
||||
pub fn with_trial_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.trial_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 是否需要任何判定（空包络时调用方可以跳过整段检查）。
|
||||
#[must_use]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.max_rpm.is_none()
|
||||
&& self.max_temperature_c.is_none()
|
||||
&& self.feedback_timeout.is_none()
|
||||
&& self.trial_timeout.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 带状态的安全监视器（过速宽限与总时长需要记忆）。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SafetyMonitor {
|
||||
envelope: SafetyEnvelope,
|
||||
started: Instant,
|
||||
overspeed_since: Option<Instant>,
|
||||
}
|
||||
|
||||
impl SafetyMonitor {
|
||||
/// 创建监视器并记录起点。
|
||||
#[must_use]
|
||||
pub fn new(envelope: SafetyEnvelope, now: Instant) -> Self {
|
||||
Self { envelope, started: now, overspeed_since: None }
|
||||
}
|
||||
|
||||
/// 当前包络。
|
||||
#[must_use]
|
||||
pub const fn envelope(&self) -> &SafetyEnvelope {
|
||||
&self.envelope
|
||||
}
|
||||
|
||||
/// 检查一组电机在当前时刻是否触碰包络。
|
||||
///
|
||||
/// 返回 `Some(Abort)` 即调用方**必须立刻停电流并结束本次试验**。
|
||||
/// 每次调用**不分配**（1 kHz 台架循环里每拍都要过一遍）。
|
||||
pub fn check<M: MotorFeedback>(&mut self, motors: &[M], now: Instant) -> Option<Abort> {
|
||||
if let Some(timeout) = self.envelope.trial_timeout {
|
||||
let elapsed = now.saturating_duration_since(self.started);
|
||||
if elapsed > timeout {
|
||||
return Some(Abort::Timeout { elapsed_ms: elapsed.as_millis() });
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(timeout) = self.envelope.feedback_timeout {
|
||||
for motor in motors {
|
||||
let age = motor.feedback_age();
|
||||
if age.is_none_or(|age| age > timeout) {
|
||||
return Some(Abort::FeedbackLost {
|
||||
esc_id: motor.id(),
|
||||
age_ms: age.map(|age| age.as_millis()),
|
||||
timeout_ms: timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_temperature_c {
|
||||
for motor in motors {
|
||||
let temperature = motor.state().temperature_c;
|
||||
if temperature > limit {
|
||||
return Some(Abort::OverTemperature {
|
||||
temperature_c: temperature,
|
||||
limit_c: limit,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_rpm {
|
||||
let peak = motors
|
||||
.iter()
|
||||
.map(|motor| motor.state().rotor_speed_rpm.abs())
|
||||
.fold(0.0f32, f32::max);
|
||||
if peak > limit {
|
||||
match self.overspeed_since {
|
||||
None => self.overspeed_since = Some(now),
|
||||
Some(since)
|
||||
if now.saturating_duration_since(since)
|
||||
>= self.envelope.overspeed_grace =>
|
||||
{
|
||||
return Some(Abort::Overspeed {
|
||||
peak_rpm: peak,
|
||||
limit_rpm: limit,
|
||||
grace_ms: self.envelope.overspeed_grace.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Some(_) => {}
|
||||
}
|
||||
} else {
|
||||
self.overspeed_since = None;
|
||||
}
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_core::{MockMotor, MotorState};
|
||||
|
||||
fn motor_with(id: u8, rpm: f32, temperature_c: i8, age: Option<Duration>) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
let state = MotorState {
|
||||
rotor_speed_rpm: rpm,
|
||||
temperature_c,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
};
|
||||
match age {
|
||||
Some(age) => motor.inject_state_with_age(state, age),
|
||||
None => motor.inject_state(state),
|
||||
}
|
||||
motor
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_envelope_never_aborts() {
|
||||
let motors = vec![motor_with(1, 1.0e6, 120, Some(Duration::from_secs(5)))];
|
||||
let mut monitor = SafetyMonitor::new(SafetyEnvelope::new(), Instant::now());
|
||||
assert!(monitor.envelope().is_empty());
|
||||
assert_eq!(monitor.check(&motors, Instant::now()), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_needs_the_grace_period() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None, "宽限期内不得触发");
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(30)), None);
|
||||
let abort = monitor.check(&motors, now + grace);
|
||||
assert!(matches!(abort, Some(Abort::Overspeed { .. })), "{abort:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_timer_resets_when_back_under_limit() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None);
|
||||
let calm = vec![motor_with(1, 100.0, 30, None)];
|
||||
assert_eq!(monitor.check(&calm, now + Duration::from_millis(30)), None);
|
||||
// 重新超速：计时必须从头开始。
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(40)), None);
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(90)).is_none());
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(110)).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_and_feedback_loss_abort_immediately() {
|
||||
let envelope = SafetyEnvelope::new()
|
||||
.with_temperature_max(70)
|
||||
.with_feedback_timeout(Duration::from_millis(100));
|
||||
let hot = vec![motor_with(1, 0.0, 80, None)];
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), Instant::now());
|
||||
assert!(matches!(monitor.check(&hot, Instant::now()), Some(Abort::OverTemperature { .. })));
|
||||
|
||||
let stale = vec![motor_with(1, 0.0, 30, Some(Duration::from_millis(250)))];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&stale, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 1, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_without_any_feedback_is_lost() {
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
// 从未收到过反馈：age = None。
|
||||
let silent = vec![MockMotor::new(3)];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&silent, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 3, age_ms: None, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn trial_timeout_aborts() {
|
||||
let envelope = SafetyEnvelope::new().with_trial_timeout(Duration::from_millis(500));
|
||||
let motors = vec![motor_with(1, 0.0, 30, None)];
|
||||
let start = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope, start);
|
||||
assert_eq!(monitor.check(&motors, start + Duration::from_millis(100)), None);
|
||||
let abort = monitor.check(&motors, start + Duration::from_millis(600));
|
||||
assert!(matches!(abort, Some(Abort::Timeout { .. })), "{abort:?}");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,239 @@
|
||||
//! 真机试验：把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。
|
||||
//!
|
||||
//! 与 `dji-esc-sim` 的离线 harness 是**同一套场景词汇**：整定工具先用离线
|
||||
//! harness 搜索，再用本模块在台架上复核，两条路径的指标口径因此一致。
|
||||
//!
|
||||
//! # 安全契约
|
||||
//!
|
||||
//! - 每个采样拍都重发设定值——**命令是租约**，不重发会在 50ms 后失电；
|
||||
//! - 每个采样拍都过一遍 [`SafetyMonitor`]；
|
||||
//! - 无论正常结束、触发包络还是被取消，退出前一律 `disable()`（0 电流）。
|
||||
|
||||
use std::future::Future;
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use motor_core::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval};
|
||||
|
||||
use crate::safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
|
||||
/// 一次真机试验的结果。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TrialOutcome {
|
||||
/// 采集到的轨迹（触发中止时只含中止前的采样）。
|
||||
pub trace: Trace,
|
||||
/// 触发安全包络 / 被取消的原因；`None` 表示按场景跑完。
|
||||
pub abort: Option<Abort>,
|
||||
}
|
||||
|
||||
impl TrialOutcome {
|
||||
/// 是否完整跑完场景（没有被安全包络打断）。
|
||||
#[must_use]
|
||||
pub fn completed(&self) -> bool {
|
||||
self.abort.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 采样节拍下限（防止 `dt_s` 给出病态值把 CPU 打满）。
|
||||
const MIN_TICK: Duration = Duration::from_micros(50);
|
||||
|
||||
/// 在单台电机上重放一条场景。
|
||||
///
|
||||
/// - `effort_limit_raw`：本次试运行的 effort 限幅，仅用于标注 `saturated`
|
||||
/// （控制器真正的钳位在驱动层按 `EscConfig::current_limit` 完成）；
|
||||
/// - `lease_period`：命令租约的续期节拍（比 `scenario.dt_s` 更稀时以采样节拍为准）；
|
||||
/// - `cancel`：外部取消源（ctrl-c / 搜索预算耗尽）。
|
||||
pub async fn run_step_trial<M, F>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
cancel: F,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
F: Future<Output = ()>,
|
||||
{
|
||||
let dt = Duration::from_secs_f32(scenario.dt_s.max(1.0e-6)).max(MIN_TICK);
|
||||
let total = Duration::from_secs_f32(scenario.total_s().max(0.0));
|
||||
// 续租节拍不得稀于采样节拍，否则会短暂失电。
|
||||
let lease_period = lease_period.min(dt);
|
||||
|
||||
let started = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), started);
|
||||
let mut ticker = interval(dt);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut lease = interval(lease_period);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
let mut trace = Trace::with_capacity(scenario.samples());
|
||||
let mut setpoint = scenario.setpoint_at(0.0);
|
||||
let mut abort = None;
|
||||
tokio::pin!(cancel);
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
() = &mut cancel => {
|
||||
abort = Some(Abort::Cancelled);
|
||||
break;
|
||||
}
|
||||
_ = lease.tick() => {
|
||||
// 续租：命令是租约，不重发就失电。
|
||||
motor.set_velocity(setpoint);
|
||||
}
|
||||
_ = ticker.tick() => {
|
||||
let now = Instant::now();
|
||||
let elapsed = now.saturating_duration_since(started);
|
||||
if elapsed > total {
|
||||
break;
|
||||
}
|
||||
let t_s = elapsed.as_secs_f32();
|
||||
setpoint = scenario.setpoint_at(t_s);
|
||||
motor.set_velocity(setpoint);
|
||||
|
||||
let state = motor.state();
|
||||
let saturated =
|
||||
i32::from(state.effort_raw).abs() >= i32::from(effort_limit_raw);
|
||||
trace.push(t_s, setpoint, state.rotor_speed_rpm, state.effort_raw, saturated);
|
||||
|
||||
if let Some(reason) = monitor.check(std::slice::from_ref(motor), now) {
|
||||
abort = Some(reason);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 安全收尾：任何退出路径都先归零，绝不把非零给定留在总线上。
|
||||
motor.disable();
|
||||
TrialOutcome { trace, abort }
|
||||
}
|
||||
|
||||
/// 以 ctrl-c 作为取消源的 [`run_step_trial`] 包装。
|
||||
pub async fn run_step_trial_until_ctrl_c<M>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
{
|
||||
run_step_trial(motor, scenario, envelope, effort_limit_raw, lease_period, async {
|
||||
let _ = tokio::signal::ctrl_c().await;
|
||||
})
|
||||
.await
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_core::{Command, MockMotor, MotorState};
|
||||
|
||||
fn ready_motor(id: u8) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
motor.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
|
||||
motor
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn trial_follows_scenario_and_disables_at_the_end() {
|
||||
let motor = ready_motor(1);
|
||||
let scenario = Scenario::ramp_and_hold(1000.0, 0.1, 0.2);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(outcome.completed(), "{:?}", outcome.abort);
|
||||
assert!(outcome.trace.is_consistent());
|
||||
assert!(outcome.trace.len() > 250, "len = {}", outcome.trace.len());
|
||||
// 斜坡中点的设定值应约为一半。
|
||||
let mid = outcome.trace.window(0.049, 0.052).setpoint_rpm()[0];
|
||||
assert!((mid - 500.0).abs() < 30.0, "mid setpoint = {mid}");
|
||||
// 收尾必须归零。
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn overspeed_aborts_and_zeroes_output() {
|
||||
let motor = ready_motor(1);
|
||||
// 反馈恒为 5000 rpm，远超 3000 rpm 上限。
|
||||
motor.inject_state(MotorState {
|
||||
rotor_speed_rpm: 5000.0,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
});
|
||||
let envelope = SafetyEnvelope::overspeed_only(3000.0, Duration::from_millis(20));
|
||||
let scenario = Scenario::step(1000.0, 2.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(matches!(outcome.abort, Some(Abort::Overspeed { .. })), "{:?}", outcome.abort);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
// 中止得足够早，不该跑满场景。
|
||||
assert!(outcome.trace.len() < 200, "len = {}", outcome.trace.len());
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn stalled_feedback_aborts() {
|
||||
let motor = MockMotor::new(4);
|
||||
motor.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(500),
|
||||
);
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
let scenario = Scenario::step(500.0, 1.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(
|
||||
matches!(outcome.abort, Some(Abort::FeedbackLost { esc_id: 4, .. })),
|
||||
"{:?}",
|
||||
outcome.abort
|
||||
);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn cancellation_stops_and_zeroes_output() {
|
||||
let motor = ready_motor(2);
|
||||
let scenario = Scenario::step(800.0, 5.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
async { tokio::time::sleep(Duration::from_millis(50)).await },
|
||||
)
|
||||
.await;
|
||||
|
||||
assert_eq!(outcome.abort, Some(Abort::Cancelled));
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
assert!(!outcome.trace.is_empty());
|
||||
assert!(outcome.trace.len() < 200, "取消后不得继续采样");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
[package]
|
||||
name = "chassis-config"
|
||||
description = "chassis.toml schema: parsing, semantic validation, and mapping to driver configuration"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
dji-esc = { workspace = true }
|
||||
mecanum = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
@@ -1,10 +1,19 @@
|
||||
//! `chassis.toml` 配置模型与加载。
|
||||
//! `chassis.toml` 配置模型：解析、语义校验与到驱动配置的映射。
|
||||
//!
|
||||
//! 本 crate 是**配置 schema 的唯一来源**：`chassisd`（守护进程）与
|
||||
//! `chassis-tune`（自动整定）读同一份类型定义，因此整定工具写回的
|
||||
//! `[[motor]]` 片段必然能被守护进程读回，不需要两份 schema 手工同步。
|
||||
//!
|
||||
//! 全部类型都实现 `Serialize`，所以 [`Config::to_toml_string`] 可以把
|
||||
//! （可能已被整定工具改写过的）配置原样落盘。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{BusOptions, EscConfig, EscModel};
|
||||
use mecanum::Mecanum;
|
||||
use serde::Deserialize;
|
||||
use motor_core::VelocityGains;
|
||||
use serde::{Deserialize, Serialize};
|
||||
use thiserror::Error;
|
||||
|
||||
/// 配置错误。
|
||||
@@ -19,7 +28,7 @@ pub enum ConfigError {
|
||||
}
|
||||
|
||||
/// 顶层配置。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize)]
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct Config {
|
||||
/// CAN 配置。
|
||||
@@ -37,7 +46,7 @@ pub struct Config {
|
||||
/// CAN 接口配置（双 CAN 总线 seam）。
|
||||
///
|
||||
/// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize)]
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct CanConfig {
|
||||
/// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。
|
||||
@@ -51,7 +60,7 @@ pub struct CanConfig {
|
||||
}
|
||||
|
||||
/// 功能总线配置（双 CAN 之二）。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize)]
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct FunctionBusConfig {
|
||||
/// SocketCAN 接口名（如 `can1`）。
|
||||
@@ -82,7 +91,7 @@ impl ProtocolFamily {
|
||||
}
|
||||
|
||||
/// 底盘控制参数（全部可省略，采用默认值）。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize)]
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields, default)]
|
||||
pub struct ChassisConfig {
|
||||
/// 控制循环周期（毫秒）。
|
||||
@@ -91,6 +100,11 @@ pub struct ChassisConfig {
|
||||
pub command_timeout_ms: u64,
|
||||
/// 电机反馈离线阈值（毫秒）。
|
||||
pub feedback_timeout_ms: u64,
|
||||
/// 驱动层**命令租约**（毫秒）：控制循环超过该时长未刷新电机命令 →
|
||||
/// 驱动强制 0 电流（防控制任务死亡后旧指令持续输出）。
|
||||
///
|
||||
/// 必须 ≥ `control_period_ms`；默认 1kHz 下约 50 拍。
|
||||
pub motor_command_timeout_ms: u64,
|
||||
/// 最大前进 / 横移速度（m/s）。
|
||||
pub max_vx: f32,
|
||||
/// 最大横移速度（m/s）。
|
||||
@@ -105,6 +119,7 @@ impl Default for ChassisConfig {
|
||||
control_period_ms: 1,
|
||||
command_timeout_ms: 200,
|
||||
feedback_timeout_ms: 100,
|
||||
motor_command_timeout_ms: 50,
|
||||
max_vx: 3.0,
|
||||
max_vy: 3.0,
|
||||
max_omega: 6.0,
|
||||
@@ -113,7 +128,7 @@ impl Default for ChassisConfig {
|
||||
}
|
||||
|
||||
/// 麦轮几何（米）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Deserialize)]
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct MecanumConfig {
|
||||
/// 前后轮轴线到中心的纵向半距。
|
||||
@@ -125,7 +140,7 @@ pub struct MecanumConfig {
|
||||
}
|
||||
|
||||
/// 单电机配置。
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize)]
|
||||
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields)]
|
||||
pub struct MotorConfig {
|
||||
/// 便于日志辨认的名字（`front_left` 等）。
|
||||
@@ -170,7 +185,7 @@ fn default_current_limit() -> i16 {
|
||||
}
|
||||
|
||||
/// 配置文件中的电调型号。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize)]
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)]
|
||||
#[serde(deny_unknown_fields, rename_all = "UPPERCASE")]
|
||||
pub enum EscModelSerde {
|
||||
/// C620（GM3508）。
|
||||
@@ -188,6 +203,22 @@ impl From<EscModelSerde> for EscModel {
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorConfig {
|
||||
/// 速度环增益组（可整定参数）。
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self) -> VelocityGains {
|
||||
VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
|
||||
}
|
||||
|
||||
/// 覆写速度环增益组，其余字段（限流、方向、减速比）保持不变。
|
||||
pub fn set_velocity_gains(&mut self, gains: VelocityGains) {
|
||||
self.kp = gains.kp;
|
||||
self.ki = gains.ki;
|
||||
self.kd = gains.kd;
|
||||
self.feedforward = gains.feedforward;
|
||||
}
|
||||
}
|
||||
|
||||
impl Config {
|
||||
/// 从 TOML 文本解析并校验。
|
||||
///
|
||||
@@ -211,6 +242,69 @@ impl Config {
|
||||
Self::from_toml(&text)
|
||||
}
|
||||
|
||||
/// 序列化回 TOML 文本。
|
||||
///
|
||||
/// 与 [`Config::from_toml`] 往返等价（`from_toml(to_toml_string(c)) == c`），
|
||||
/// 因此整定工具改写增益后可以直接落盘成守护进程能读的配置。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 序列化失败（类型定义与 TOML 数据模型冲突时才会发生，属实现缺陷）。
|
||||
pub fn to_toml_string(&self) -> Result<String, ConfigError> {
|
||||
toml::to_string(self).map_err(|e| ConfigError::Parse(e.to_string()))
|
||||
}
|
||||
|
||||
/// 把配置写到文件。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 序列化失败或写入失败。
|
||||
pub fn save(&self, path: &std::path::Path) -> Result<(), ConfigError> {
|
||||
let text = self.to_toml_string()?;
|
||||
std::fs::write(path, text)
|
||||
.map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))
|
||||
}
|
||||
|
||||
/// 按名字查找电机下标。
|
||||
#[must_use]
|
||||
pub fn motor_index(&self, name: &str) -> Option<usize> {
|
||||
self.motors.iter().position(|m| m.name == name)
|
||||
}
|
||||
|
||||
/// 覆写第 `index` 台电机的速度环增益。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 下标越界时返回 [`ConfigError::Invalid`]。
|
||||
pub fn set_motor_gains(
|
||||
&mut self,
|
||||
index: usize,
|
||||
gains: VelocityGains,
|
||||
) -> Result<(), ConfigError> {
|
||||
let count = self.motors.len();
|
||||
let motor = self.motors.get_mut(index).ok_or_else(|| {
|
||||
ConfigError::Invalid(format!("motor index {index} out of 0..{count}"))
|
||||
})?;
|
||||
motor.set_velocity_gains(gains);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 按轮名覆写速度环增益（整定工具按名字写回时用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 名字不存在时返回 [`ConfigError::Invalid`]。
|
||||
pub fn set_motor_gains_by_name(
|
||||
&mut self,
|
||||
name: &str,
|
||||
gains: VelocityGains,
|
||||
) -> Result<(), ConfigError> {
|
||||
let index = self
|
||||
.motor_index(name)
|
||||
.ok_or_else(|| ConfigError::Invalid(format!("no motor named '{name}'")))?;
|
||||
self.set_motor_gains(index, gains)
|
||||
}
|
||||
|
||||
/// 语义校验。
|
||||
///
|
||||
/// # Errors
|
||||
@@ -228,11 +322,21 @@ impl Config {
|
||||
return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}")));
|
||||
}
|
||||
let c = &self.chassis;
|
||||
if c.control_period_ms == 0 || c.command_timeout_ms == 0 || c.feedback_timeout_ms == 0 {
|
||||
if c.control_period_ms == 0
|
||||
|| c.command_timeout_ms == 0
|
||||
|| c.feedback_timeout_ms == 0
|
||||
|| c.motor_command_timeout_ms == 0
|
||||
{
|
||||
return Err(ConfigError::Invalid(
|
||||
"control/command/feedback timeouts must be > 0 ms".into(),
|
||||
"control/command/feedback/motor-command timeouts must be > 0 ms".into(),
|
||||
));
|
||||
}
|
||||
if c.motor_command_timeout_ms < c.control_period_ms {
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"motor_command_timeout_ms ({}) must be >= control_period_ms ({})",
|
||||
c.motor_command_timeout_ms, c.control_period_ms
|
||||
)));
|
||||
}
|
||||
if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) {
|
||||
return Err(ConfigError::Invalid("speed limits must be > 0".into()));
|
||||
}
|
||||
@@ -254,13 +358,13 @@ impl Config {
|
||||
));
|
||||
}
|
||||
// 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。
|
||||
if let Some(function) = &self.can.function {
|
||||
if function.interface == self.can.interface {
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"function bus interface must differ from chassis interface (both '{}')",
|
||||
self.can.interface
|
||||
)));
|
||||
}
|
||||
if let Some(function) = &self.can.function
|
||||
&& function.interface == self.can.interface
|
||||
{
|
||||
return Err(ConfigError::Invalid(format!(
|
||||
"function bus interface must differ from chassis interface (both '{}')",
|
||||
self.can.interface
|
||||
)));
|
||||
}
|
||||
// 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。
|
||||
Ok(())
|
||||
@@ -321,6 +425,7 @@ impl Config {
|
||||
BusOptions {
|
||||
period: Duration::from_millis(self.chassis.control_period_ms),
|
||||
feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms),
|
||||
command_timeout: Duration::from_millis(self.chassis.motor_command_timeout_ms),
|
||||
}
|
||||
}
|
||||
|
||||
@@ -501,4 +606,78 @@ gear_ratio = 19.2
|
||||
let function = cfg.can.function.as_ref().unwrap();
|
||||
assert_eq!(function.interface, "can1");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn toml_round_trip_is_lossless() {
|
||||
let original = Config::from_toml(EXAMPLE).unwrap();
|
||||
let text = original.to_toml_string().unwrap();
|
||||
let reparsed = Config::from_toml(&text).unwrap();
|
||||
assert_eq!(reparsed, original);
|
||||
// 落盘文本必须仍然是合法配置（守护进程直接读它）。
|
||||
assert!(text.contains("[[motor]]"), "{text}");
|
||||
assert!(text.contains("interface = \"can0\""), "{text}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn round_trip_preserves_function_bus_and_timeouts() {
|
||||
let mut original = Config::mock_default();
|
||||
original.can.function = Some(FunctionBusConfig { interface: "can1".into() });
|
||||
original.chassis.motor_command_timeout_ms = 37;
|
||||
let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap();
|
||||
assert_eq!(reparsed, original);
|
||||
assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37);
|
||||
assert_eq!(reparsed.can.function.unwrap().interface, "can1");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_can_be_read_and_written_by_name() {
|
||||
let mut cfg = Config::from_toml(EXAMPLE).unwrap();
|
||||
assert_eq!(cfg.motor_index("rear_left"), Some(2));
|
||||
assert_eq!(cfg.motor_index("nope"), None);
|
||||
assert_eq!(cfg.motors[2].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));
|
||||
|
||||
let tuned = VelocityGains::new(2.0, 10.0, 0.1, 0.0);
|
||||
cfg.set_motor_gains_by_name("rear_left", tuned).unwrap();
|
||||
assert_eq!(cfg.motors[2].velocity_gains(), tuned);
|
||||
// 其余电机不受影响。
|
||||
assert_eq!(cfg.motors[0].velocity_gains(), VelocityGains::new(18.0, 0.6, 0.0, 9.0));
|
||||
// 机械 / 安全字段保持不变。
|
||||
assert_eq!(cfg.motors[2].current_limit, 12_000);
|
||||
assert!((cfg.motors[2].gear_ratio - 19.203).abs() < 1e-6);
|
||||
assert!(cfg.validate().is_ok());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_write_back_survives_round_trip() {
|
||||
let mut cfg = Config::mock_default();
|
||||
let tuned = VelocityGains::new(3.5, 22.0, 0.25, 0.75);
|
||||
cfg.set_motor_gains(0, tuned).unwrap();
|
||||
let reparsed = Config::from_toml(&cfg.to_toml_string().unwrap()).unwrap();
|
||||
assert_eq!(reparsed.motors[0].velocity_gains(), tuned);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_gains_out_of_range_is_rejected() {
|
||||
let mut cfg = Config::mock_default();
|
||||
assert!(matches!(
|
||||
cfg.set_motor_gains(9, VelocityGains::ZERO),
|
||||
Err(ConfigError::Invalid(_))
|
||||
));
|
||||
assert!(matches!(
|
||||
cfg.set_motor_gains_by_name("ghost", VelocityGains::ZERO),
|
||||
Err(ConfigError::Invalid(_))
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn save_and_load_file_round_trip() {
|
||||
let dir = std::env::temp_dir().join("chassis-config-roundtrip");
|
||||
std::fs::create_dir_all(&dir).unwrap();
|
||||
let path = dir.join("chassis.toml");
|
||||
let original = Config::mock_default();
|
||||
original.save(&path).unwrap();
|
||||
let loaded = Config::load(&path).unwrap();
|
||||
assert_eq!(loaded, original);
|
||||
std::fs::remove_file(&path).unwrap();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
[package]
|
||||
name = "chassis-tune"
|
||||
description = "Automated PID gain search for the chassis velocity loop: offline sweep/refine plus bench (hardware-in-the-loop) verification"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
default = []
|
||||
# 可选：用 plotters 输出 best.svg（默认关闭，避免 CI 依赖系统字体与更长编译）。
|
||||
plot = ["dep:plotters"]
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
chassis-bench = { workspace = true }
|
||||
chassis-config = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
argmin = { workspace = true }
|
||||
argmin-math = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
rayon = { workspace = true }
|
||||
indicatif = { workspace = true }
|
||||
csv = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
serde_json = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
plotters = { workspace = true, optional = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,173 @@
|
||||
//! 评估后端：把"一组增益有多好"这件事抽象出来。
|
||||
//!
|
||||
//! - [`SimEvaluator`]：离线 harness + 被控对象集成（域随机化），CPU 密集、可并行；
|
||||
//! - [`BenchEvaluator`](crate::bench::BenchEvaluator)：真机台架（`chassis-bench`），单轮、串行、有安全包络。
|
||||
//!
|
||||
//! 搜索器只认识 [`Evaluator`] trait，因此"先仿真粗调、再上台架复验"是同一套
|
||||
//! 代码换一个后端，而不是两份实现。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, PlantEnsemble, Scenario, simulate};
|
||||
use motor_core::VelocityGains;
|
||||
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
pub use crate::cost::Weights;
|
||||
|
||||
/// 一次评估的结果。
|
||||
pub type Evaluated = Score;
|
||||
|
||||
/// 被控对象评估后端。
|
||||
///
|
||||
/// 必须 `Send + Sync`：粗扫阶段由 `rayon` 并行调用。
|
||||
pub trait Evaluator: Send + Sync {
|
||||
/// 评估一组增益。
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated;
|
||||
/// 一行描述（写进报告，便于复现）。
|
||||
fn describe(&self) -> String;
|
||||
}
|
||||
|
||||
/// 离线评估：在一组被控对象参数上重放同一条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SimEvaluator {
|
||||
/// 被控对象集成（域随机化）。
|
||||
pub ensemble: PlantEnsemble,
|
||||
/// 评价场景。
|
||||
pub scenario: Scenario,
|
||||
/// 限幅。
|
||||
pub limits: LoopLimits,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 集成聚合权重：0 = 只看均值，1 = 只看最坏成员。
|
||||
pub robust_weight: f64,
|
||||
}
|
||||
|
||||
impl SimEvaluator {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
ensemble: PlantEnsemble,
|
||||
scenario: Scenario,
|
||||
limits: LoopLimits,
|
||||
objective: Objective,
|
||||
robust_weight: f64,
|
||||
) -> Self {
|
||||
Self { ensemble, scenario, limits, objective, robust_weight: robust_weight.clamp(0.0, 1.0) }
|
||||
}
|
||||
|
||||
/// 集成规模。
|
||||
#[must_use]
|
||||
pub fn ensemble_size(&self) -> usize {
|
||||
self.ensemble.len()
|
||||
}
|
||||
}
|
||||
|
||||
impl Evaluator for SimEvaluator {
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated {
|
||||
let mut costs = Vec::with_capacity(self.ensemble.len());
|
||||
let mut nominal: Option<Score> = 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::<f64>() / 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}");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,383 @@
|
||||
//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
|
||||
//!
|
||||
//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]，
|
||||
//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
|
||||
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
|
||||
//!
|
||||
//! # 安全设计
|
||||
//!
|
||||
//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
|
||||
//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
|
||||
//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
|
||||
//!
|
||||
//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
|
||||
//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。
|
||||
|
||||
use std::sync::Mutex;
|
||||
use std::time::Duration;
|
||||
|
||||
use anyhow::Context as _;
|
||||
use chassis_bench::{Abort, SafetyEnvelope, run_step_trial};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use motor_core::{Disable as _, MotorFeedback, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
/// 真机后端错误。
|
||||
#[derive(Debug, Error)]
|
||||
pub enum BenchError {
|
||||
/// 无法打开总线。
|
||||
#[error("cannot open CAN link: {0}")]
|
||||
Link(String),
|
||||
/// 电调总线启动失败。
|
||||
#[error("cannot start ESC bus: {0}")]
|
||||
Bus(String),
|
||||
/// 启动超时：目标电调没有反馈。
|
||||
#[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")]
|
||||
NoFeedback {
|
||||
/// 目标电调 ID。
|
||||
esc_id: u8,
|
||||
/// 等待时长（毫秒）。
|
||||
waited_ms: u64,
|
||||
},
|
||||
/// 搜索期间因安全事件中止且未要求继续。
|
||||
#[error("aborted by safety envelope: {reason}")]
|
||||
Aborted {
|
||||
/// 触发原因。
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
/// 真机评估配置。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BenchConfig {
|
||||
/// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
|
||||
pub esc: EscConfig,
|
||||
/// 总线运行参数。
|
||||
pub bus: BusOptions,
|
||||
/// 评估场景。
|
||||
pub scenario: Scenario,
|
||||
/// 安全包络。
|
||||
pub envelope: SafetyEnvelope,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 试验之间的静默期（等轮子停稳）。
|
||||
pub between_trials: Duration,
|
||||
/// 命令租约续期节拍。
|
||||
pub lease_period: Duration,
|
||||
/// 启动时等待反馈的超时。
|
||||
pub startup_timeout: Duration,
|
||||
/// 安全检查触发后是否继续搜索（默认 false = 立即停）。
|
||||
pub continue_on_abort: bool,
|
||||
}
|
||||
|
||||
impl BenchConfig {
|
||||
/// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
esc: EscConfig,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
objective: Objective,
|
||||
) -> Self {
|
||||
Self {
|
||||
esc,
|
||||
bus: BusOptions::default(),
|
||||
scenario,
|
||||
envelope,
|
||||
objective,
|
||||
between_trials: Duration::from_millis(500),
|
||||
lease_period: Duration::from_millis(5),
|
||||
startup_timeout: Duration::from_secs(5),
|
||||
continue_on_abort: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 真机评估器（持有总线与运行时）。
|
||||
pub struct BenchEvaluator {
|
||||
session: Mutex<Session>,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
objective: Objective,
|
||||
effort_limit_raw: i16,
|
||||
between_trials: Duration,
|
||||
lease_period: Duration,
|
||||
continue_on_abort: bool,
|
||||
/// 目前见过的最优轨迹（报告用）。
|
||||
best: Mutex<Option<(f64, Trace)>>,
|
||||
/// 中止记录（报告用）。
|
||||
aborts: Mutex<Vec<String>>,
|
||||
describe: String,
|
||||
}
|
||||
|
||||
struct Session {
|
||||
runtime: tokio::runtime::Runtime,
|
||||
bus: Option<EscBusHandle>,
|
||||
motor: EscMotor,
|
||||
}
|
||||
|
||||
impl BenchEvaluator {
|
||||
/// 打开链路、启动总线并等待目标电调反馈。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路 / 总线启动失败，或启动超时（电调无反馈）。
|
||||
pub fn open(link: can_link::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
|
||||
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<Trace> {
|
||||
self.best
|
||||
.lock()
|
||||
.unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
.as_ref()
|
||||
.map(|(_, trace)| trace.clone())
|
||||
}
|
||||
|
||||
/// 搜索期间的安全中止记录。
|
||||
#[must_use]
|
||||
pub fn aborts(&self) -> Vec<String> {
|
||||
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<can_link::CanLink, BenchError> {
|
||||
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 接口必须报错");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,302 @@
|
||||
//! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。
|
||||
//!
|
||||
//! 两个原则：
|
||||
//!
|
||||
//! 1. **硬约束不用权重表达**。过速 / 长时间饱和 / 不收敛不是"差一点"，
|
||||
//! 而是"不能用"——它们直接给罚值并记录判定，绝不允许被其他项的权重
|
||||
//! 抵消掉（否则搜索会主动挑一个会飞车的解）。
|
||||
//! 2. **各项归一化**。时间按轨迹时长归一、误差按 `|目标| × 时长` 归一、
|
||||
//! effort 按限幅归一，因此权重在不同场景 / 不同电机之间仍然可解释。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, SimError, Trace};
|
||||
|
||||
use crate::metrics::StepMetrics;
|
||||
|
||||
/// 罚值：远大于任何合法成本，但保持有限（避免 NaN 传染给优化器）。
|
||||
pub const PENALTY: f64 = 1.0e9;
|
||||
|
||||
/// 成本权重（各项已归一化到 O(1)）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Weights {
|
||||
/// 调节时间权重。
|
||||
pub settle: f64,
|
||||
/// 超调权重。
|
||||
pub overshoot: f64,
|
||||
/// 绝对误差积分（IAE）权重。
|
||||
pub iae: f64,
|
||||
/// effort（电流）权重。
|
||||
pub effort: f64,
|
||||
}
|
||||
|
||||
impl Default for Weights {
|
||||
fn default() -> Self {
|
||||
// 底盘速度环：跟得上、不过冲最重要；电流只作为次要惩罚
|
||||
// （省电流不是目的，避免烧电调才是）。
|
||||
Self { settle: 1.0, overshoot: 1.0, iae: 0.5, effort: 0.1 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Weights {
|
||||
/// 人类可读描述（日志 / 报告用）。
|
||||
#[must_use]
|
||||
pub fn describe(&self) -> String {
|
||||
format!(
|
||||
"cost={:.2}·settle+{:.2}·overshoot+{:.2}·iae+{:.2}·effort",
|
||||
self.settle, self.overshoot, self.iae, self.effort
|
||||
)
|
||||
}
|
||||
|
||||
/// 权重是否全部有限且非负。
|
||||
#[must_use]
|
||||
pub fn is_valid(&self) -> bool {
|
||||
[self.settle, self.overshoot, self.iae, self.effort]
|
||||
.iter()
|
||||
.all(|w| w.is_finite() && *w >= 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的判定。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Verdict {
|
||||
/// 达标（成本 = 加权指标）。
|
||||
Ok,
|
||||
/// 增益含 NaN / inf，或场景退化（被拒绝，不进控制器）。
|
||||
Rejected,
|
||||
/// 数值发散。
|
||||
Diverged,
|
||||
/// 过速。
|
||||
Overspeed,
|
||||
/// 饱和占比过高（积分绕死 / 限幅太紧）。
|
||||
Saturated,
|
||||
/// 在场景内没有收敛。
|
||||
NoSettle,
|
||||
}
|
||||
|
||||
impl Verdict {
|
||||
/// 是否达标。
|
||||
#[must_use]
|
||||
pub fn is_ok(self) -> bool {
|
||||
matches!(self, Self::Ok)
|
||||
}
|
||||
|
||||
/// 简短标签（报告 / CSV 用）。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Ok => "ok",
|
||||
Self::Rejected => "rejected",
|
||||
Self::Diverged => "diverged",
|
||||
Self::Overspeed => "overspeed",
|
||||
Self::Saturated => "saturated",
|
||||
Self::NoSettle => "no-settle",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的结果。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Score {
|
||||
/// 成本（越小越好）。
|
||||
pub cost: f64,
|
||||
/// 判定。
|
||||
pub verdict: Verdict,
|
||||
/// 指标（被拒绝时为 `None`）。
|
||||
pub metrics: Option<StepMetrics>,
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,572 @@
|
||||
//! 增益参数空间：轴定义、采样与**坐标空间**映射。
|
||||
//!
|
||||
//! # 为什么要有"坐标空间"
|
||||
//!
|
||||
//! 增益的合法取值跨越数量级（`ki` 可能在 0.1 与 100 之间），线性空间的
|
||||
//! 单纯形步长对数量级不敏感；同时 `kp/ki/kd` 必须为正。把每个轴映射到
|
||||
//! **坐标空间**（对数轴取 `ln`，线性轴恒等）后：
|
||||
//!
|
||||
//! - 无约束优化器不会走出正增益区域；
|
||||
//! - 步长在不同数量级上等价（±0.1 的坐标步长 = ×1.1 的比例步长）；
|
||||
//! - `LogOrZero` 轴在**粗扫**阶段可以精确取 0（`ff = 0` 是台架常态），
|
||||
//! 在**精修**阶段退化为对数轴（零是不可导的拐点，本地搜索收益为负）。
|
||||
|
||||
use motor_core::VelocityGains;
|
||||
use rand::Rng;
|
||||
use rand::seq::SliceRandom as _;
|
||||
use thiserror::Error;
|
||||
|
||||
/// 轴数量：`kp`、`ki`、`kd`、`feedforward`。
|
||||
pub const AXIS_COUNT: usize = 4;
|
||||
|
||||
/// 轴的取值范围映射方式。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Scale {
|
||||
/// 线性均匀。
|
||||
Linear,
|
||||
/// 对数均匀（要求 `min > 0`）。
|
||||
Log,
|
||||
/// 以概率 `p_zero` 取精确 0，否则对数均匀（用于 `ki` / `kd` / `ff`）。
|
||||
LogOrZero {
|
||||
/// 取 0 的概率。
|
||||
p_zero: f64,
|
||||
},
|
||||
}
|
||||
|
||||
impl Scale {
|
||||
/// 是否是对数族（`Log` / `LogOrZero`）。
|
||||
#[must_use]
|
||||
pub fn is_log(self) -> bool {
|
||||
matches!(self, Self::Log | Self::LogOrZero { .. })
|
||||
}
|
||||
|
||||
/// 从文本解析（CLI 用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回 [`GainsError::UnknownScale`]。
|
||||
pub fn parse(text: &str) -> Result<Self, GainsError> {
|
||||
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<VelocityGains> {
|
||||
let count = count.max(1);
|
||||
let mut columns: Vec<Vec<f64>> = 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<String> {
|
||||
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::<Vec<_>>().join("; ")
|
||||
}
|
||||
}
|
||||
|
||||
/// 解析 `MIN..MAX`（`..=` 视为等价）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 无法解析或区间非法时返回错误。
|
||||
pub fn parse_axis(name: &'static str, spec: &str, scale: Scale) -> Result<GainAxis, GainsError> {
|
||||
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<f64> = 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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
//! 底盘速度环 **PID 自动整定**：参数空间搜索 + 指标评估 + 报告。
|
||||
//!
|
||||
//! # 一条命令背后发生了什么
|
||||
//!
|
||||
//! 1. [`gains`] 定义搜索空间（四轴、对数刻度、可精确取 0 的前馈 / 微分）；
|
||||
//! 2. [`search`] 先做种子化分层粗扫（`rayon` 并行），再以 top-k 为起点做
|
||||
//! 多起点 `argmin` 局部精修；
|
||||
//! 3. 每次评估由 [`backend`] 执行：离线 harness 在**被控对象集成**上重放场景，
|
||||
//! 或（真机后端）在台架上跑一次受安全包络约束的试验；
|
||||
//! 4. [`metrics`] / [`cost`] 把轨迹折成加权成本，硬约束（过速 / 长时饱和 /
|
||||
//! 不收敛）直接给罚值，不参与权重折衷；
|
||||
//! 5. [`report`] 落盘 CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! # 超参数从哪来
|
||||
//!
|
||||
//! "搜什么、怎么打分、什么算达标"由 [`preset`] 定义，优先级为
|
||||
//! **内置默认 < 预设文件（`config/tune.toml`）< 显式命令行参数**。
|
||||
//! 一次评审过的预设可以长期复用，不再靠命令行回忆当时那组数。
|
||||
//!
|
||||
//! # 可复现
|
||||
//!
|
||||
//! 随机性只来自 `--seed`：同一 seed + 同一参数空间 + 同一后端 → 逐位一致的结果。
|
||||
//! 这是 CI 里做冒烟回归的前提。预设只是把参数空间 / 权重 / 场景固化下来，
|
||||
//! 因此"同一个预设 + 同一个 seed"在任何机器上都应当复现同一结果。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 真机后端（`--backend bench`）会**真的驱动电机**。安全由三层保证：
|
||||
//! 搜索前的强制确认标志、每次试验的 `chassis-bench` 安全包络、以及任何退出
|
||||
//! 路径都先归零电流。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod backend;
|
||||
pub mod bench;
|
||||
pub mod cost;
|
||||
pub mod gains;
|
||||
pub mod metrics;
|
||||
#[cfg(feature = "plot")]
|
||||
pub mod plot;
|
||||
pub mod preset;
|
||||
pub mod report;
|
||||
pub mod search;
|
||||
|
||||
pub use backend::{Evaluated, Evaluator, SimEvaluator};
|
||||
pub use bench::{BenchConfig, BenchError, BenchEvaluator};
|
||||
pub use cost::{Objective, PENALTY, Score, Verdict, Weights};
|
||||
pub use gains::{AXIS_COUNT, GainAxis, GainSpace, GainsError, Scale, parse_axis};
|
||||
pub use metrics::StepMetrics;
|
||||
pub use preset::{
|
||||
Acceptance, AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec,
|
||||
DEFAULT_PRESET_PATH, PRESET_VERSION, PlantSpec, Preset, PresetError, PresetFile, ProfileSpec,
|
||||
RangeSpec, ScaleSpec, ScenarioSpec, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning,
|
||||
};
|
||||
pub use report::{ReportContext, ReportError, write_reports};
|
||||
pub use search::{
|
||||
Phase, Ranked, SearchConfig, SearchError, SolverKind, Trial, evaluate_once, sweep,
|
||||
};
|
||||
|
||||
/// 被整定的物理量类型（来自 `motor-core`，与驱动层、台架共用同一个定义）。
|
||||
pub use motor_core::VelocityGains;
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,265 @@
|
||||
//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。
|
||||
//!
|
||||
//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能
|
||||
//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
|
||||
/// 稳态判据的相对带宽（±2% of target）。
|
||||
pub const SETTLING_BAND: f32 = 0.02;
|
||||
/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。
|
||||
///
|
||||
/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡，
|
||||
/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。
|
||||
pub const STEADY_FRACTION: f32 = 0.75;
|
||||
/// 指标最少需要的采样数（样本太少时一切统计都无意义）。
|
||||
pub const MIN_SAMPLES: usize = 8;
|
||||
|
||||
/// 一条步响应轨迹的指标集合。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct StepMetrics {
|
||||
/// 上升时间 10% → 90%（秒）；未到达时等于轨迹总时长。
|
||||
pub rise_time_s: f32,
|
||||
/// 超调量（%，相对目标；负值截为 0）。
|
||||
pub overshoot_pct: f32,
|
||||
/// 调节时间：最后一次离开 ±2% 带的时刻（秒）。
|
||||
pub settling_time_s: f32,
|
||||
/// 是否在稳态窗口内保持 ±2% 带（即真正收敛）。
|
||||
pub settled: bool,
|
||||
/// 稳态误差（%，相对目标）。
|
||||
pub ss_error_pct: f32,
|
||||
/// 绝对误差积分 ∫|e|dt（归一化前的原始值）。
|
||||
pub iae: f32,
|
||||
/// 误差平方积分 ∫e²dt。
|
||||
pub ise: f32,
|
||||
/// 时间加权绝对误差积分 ∫t|e|dt。
|
||||
pub itae: f32,
|
||||
/// 峰值 effort（raw，绝对值）。
|
||||
pub peak_effort_raw: f32,
|
||||
/// effort 均方根（raw）。
|
||||
pub rms_effort_raw: f32,
|
||||
/// effort 符号翻转次数（抖振）。
|
||||
pub effort_reversals: usize,
|
||||
/// 饱和采样占比（%）。
|
||||
pub saturated_pct: f32,
|
||||
/// 全过程最大转速（rpm，绝对值）。
|
||||
pub max_rpm: f32,
|
||||
/// 稳态窗口内的平均转速（rpm）。
|
||||
pub steady_rpm: f32,
|
||||
}
|
||||
|
||||
impl StepMetrics {
|
||||
/// 从轨迹计算指标；样本不足或目标转速为 0 时返回 `None`。
|
||||
#[must_use]
|
||||
pub fn from_trace(trace: &Trace, scenario: &Scenario) -> Option<Self> {
|
||||
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::<f32>()
|
||||
/ 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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,129 @@
|
||||
//! 可选（`--features plot`）：把最优步响应画成 SVG。
|
||||
//!
|
||||
//! 为什么是 SVG 而不是 PNG：矢量图在任何缩放级别都能看清起动段的细节，
|
||||
//! 且不需要位图编码器（依赖更小）。默认关闭该 feature——绘图不是整定的
|
||||
//! 必要条件，CSV 已经给了全部数据。
|
||||
|
||||
use std::path::Path;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use plotters::backend::SVGBackend;
|
||||
use plotters::chart::ChartBuilder;
|
||||
use plotters::drawing::IntoDrawingArea as _;
|
||||
use plotters::series::LineSeries;
|
||||
use plotters::style::{BLACK, BLUE, Color as _, RED, WHITE};
|
||||
|
||||
use crate::report::ReportError;
|
||||
|
||||
/// 写出步响应三联图：转速跟随、误差、电流。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 绘图或写文件失败时返回 [`ReportError::Plot`]。
|
||||
pub fn write_step_svg(path: &Path, trace: &Trace, scenario: &Scenario) -> Result<(), ReportError> {
|
||||
let root = SVGBackend::new(path, (1000, 900)).into_drawing_area();
|
||||
root.fill(&WHITE).map_err(plot_err)?;
|
||||
let (upper, lower) = root.split_vertically(600);
|
||||
let (middle, bottom) = lower.split_vertically(150);
|
||||
|
||||
let total = trace.t_s.last().copied().unwrap_or(1.0).max(1.0e-3);
|
||||
let peak = trace.rpm.iter().copied().fold(scenario.target_rpm, f32::max).max(1.0);
|
||||
let effort_peak =
|
||||
trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(1.0f32, f32::max);
|
||||
|
||||
// 转速跟随。
|
||||
let mut chart = ChartBuilder::on(&upper)
|
||||
.caption("velocity step response (rpm)", ("sans-serif", 24))
|
||||
.margin(12)
|
||||
.x_label_area_size(36)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, 0.0f32..peak * 1.1)
|
||||
.map_err(plot_err)?;
|
||||
chart.configure_mesh().x_desc("t (s)").y_desc("rpm").draw().map_err(plot_err)?;
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.setpoint_rpm.iter().copied()),
|
||||
&BLACK,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("setpoint")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], BLUE));
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.rpm.iter().copied()),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("measured")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], RED));
|
||||
chart.configure_series_labels().background_style(WHITE.mix(0.8)).draw().map_err(plot_err)?;
|
||||
|
||||
// 误差。
|
||||
let mut error_chart = ChartBuilder::on(&middle)
|
||||
.caption("error (rpm)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -peak..peak)
|
||||
.map_err(plot_err)?;
|
||||
error_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
error_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace
|
||||
.t_s
|
||||
.iter()
|
||||
.zip(trace.setpoint_rpm.iter().zip(trace.rpm.iter()))
|
||||
.map(|(&t, (&sp, &rpm))| (t, sp - rpm)),
|
||||
&RED,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
// 电流。
|
||||
let mut effort_chart = ChartBuilder::on(&bottom)
|
||||
.caption("effort (raw)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -effort_peak..effort_peak)
|
||||
.map_err(plot_err)?;
|
||||
effort_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
effort_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().zip(trace.effort_raw.iter()).map(|(&t, &raw)| (t, f32::from(raw))),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
root.present().map_err(plot_err)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn plot_err<E: std::fmt::Debug>(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("<?xml") || text.contains("<svg"), "not an svg");
|
||||
assert!(text.len() > 1000, "svg looks empty");
|
||||
let _ = std::fs::remove_file(&path);
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,713 @@
|
||||
//! 报告与产物写出：CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! 产物布局（`--out-dir`）：
|
||||
//!
|
||||
//! | 文件 | 内容 |
|
||||
//! | --- | --- |
|
||||
//! | `trials.csv` | 全部候选：增益 + 指标 + 成本 + 判定（可用任意表格软件复盘） |
|
||||
//! | `best.toml` | 可直接粘贴进 `chassis.toml` 的 `[[motor]]` 片段 |
|
||||
//! | `best-trace.csv` | 最优增益的完整步响应轨迹 |
|
||||
//! | `report.md` | 人类可读总结：设置、Top-N、结论与注意事项 |
|
||||
//! | `summary.json` | 机器可读结果（CI / 后续工具消费） |
|
||||
//! | `best.svg` | 步响应三联图（仅 `--features plot`） |
|
||||
|
||||
use std::fmt::Write as _;
|
||||
use std::path::{Path, PathBuf};
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use serde::Serialize;
|
||||
|
||||
use crate::gains::GainSpace;
|
||||
use crate::metrics::StepMetrics;
|
||||
use crate::search::{Ranked, SearchConfig, Trial, verdict_hint};
|
||||
|
||||
/// 报告写出错误。
|
||||
#[derive(Debug, thiserror::Error)]
|
||||
pub enum ReportError {
|
||||
/// 目录创建或文件写入失败。
|
||||
#[error("io error at {path}: {source}")]
|
||||
Io {
|
||||
/// 出错路径。
|
||||
path: PathBuf,
|
||||
/// 底层错误。
|
||||
source: std::io::Error,
|
||||
},
|
||||
/// CSV 写出失败。
|
||||
#[error("csv error: {0}")]
|
||||
Csv(#[from] csv::Error),
|
||||
/// JSON 写出失败。
|
||||
#[error("json error: {0}")]
|
||||
Json(#[from] serde_json::Error),
|
||||
/// TOML 写出失败。
|
||||
#[error("toml error: {0}")]
|
||||
Toml(#[from] toml::ser::Error),
|
||||
/// 没有可用的最优结果。
|
||||
#[error("no feasible trial to report")]
|
||||
NoBest,
|
||||
/// SVG 绘图失败（仅 `--features plot`）。
|
||||
#[error("plot error: {0}")]
|
||||
Plot(String),
|
||||
}
|
||||
|
||||
fn io(path: impl Into<PathBuf>, 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<Trace>,
|
||||
}
|
||||
|
||||
/// 写出全部产物，返回写出的文件列表。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 目录创建、文件写入或序列化失败时返回 [`ReportError`]。
|
||||
pub fn write_reports(ctx: &ReportContext, ranked: &Ranked) -> Result<Vec<PathBuf>, 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<BestSummary>,
|
||||
notes: Vec<String>,
|
||||
}
|
||||
|
||||
#[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<MetricsSummary>,
|
||||
}
|
||||
|
||||
#[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<String> = 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::<Vec<_>>()
|
||||
.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));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,628 @@
|
||||
//! 两阶段参数空间搜索：种子化分层粗扫 → `argmin` 多起点局部精修。
|
||||
//!
|
||||
//! # 为什么是两阶段
|
||||
//!
|
||||
//! 成本函数有一大片"罚值平原"（不收敛 / 饱和的候选成本都等于 `PENALTY`），
|
||||
//! 纯局部搜索会陷在平原上；而纯网格搜索在四个数量级跨度上需要天文数字的
|
||||
//! 评估次数。两阶段各取所长：
|
||||
//!
|
||||
//! 1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数把
|
||||
//! 空间铺满，拿到若干个互不相邻的可达解（top-k）；
|
||||
//! 2. **精修**：以每个可达解为起点做多起点 `argmin` 局部搜索（Nelder-Mead
|
||||
//! 或 PSO），在**坐标空间**（对数刻度）里收敛到局部最优。
|
||||
//!
|
||||
//! # 可复现性
|
||||
//!
|
||||
//! 每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，结果与线程
|
||||
//! 调度无关；粗扫结果按下标排序后再取 top-k，因此同 seed 两次运行逐位一致。
|
||||
|
||||
use std::sync::atomic::{AtomicU64, Ordering};
|
||||
|
||||
use argmin::core::{CostFunction, Error as ArgminError, Executor};
|
||||
use argmin::solver::neldermead::NelderMead;
|
||||
use argmin::solver::particleswarm::ParticleSwarm;
|
||||
use dji_esc_sim::Scenario;
|
||||
use indicatif::ProgressBar;
|
||||
use motor_core::VelocityGains;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
use rayon::prelude::*;
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Score, Verdict};
|
||||
use crate::gains::{AXIS_COUNT, GainSpace};
|
||||
|
||||
/// 搜索阶段的错误。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum SearchError {
|
||||
/// 参数空间非法。
|
||||
#[error("invalid gain space: {0}")]
|
||||
InvalidSpace(String),
|
||||
/// 粗扫样本数为 0。
|
||||
#[error("samples must be >= 1")]
|
||||
NoSamples,
|
||||
}
|
||||
|
||||
/// 候选来自哪个阶段。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Phase {
|
||||
/// 分层粗扫。
|
||||
Coarse,
|
||||
/// 局部精修。
|
||||
Refine,
|
||||
/// 用户指定 / 基线点。
|
||||
Seed,
|
||||
}
|
||||
|
||||
impl Phase {
|
||||
/// CSV / 报告标签。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Coarse => "coarse",
|
||||
Self::Refine => "refine",
|
||||
Self::Seed => "seed",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次被评估的候选。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Trial {
|
||||
/// 评估序号（按评估顺序递增，报告可复现）。
|
||||
pub index: u64,
|
||||
/// 所属阶段。
|
||||
pub phase: Phase,
|
||||
/// 增益。
|
||||
pub gains: VelocityGains,
|
||||
/// 评估结果。
|
||||
pub score: Score,
|
||||
}
|
||||
|
||||
impl Trial {
|
||||
/// 成本。
|
||||
#[must_use]
|
||||
pub fn cost(&self) -> f64 {
|
||||
self.score.cost
|
||||
}
|
||||
}
|
||||
|
||||
/// 局部精修求解器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub enum SolverKind {
|
||||
/// Nelder-Mead（无导数、对罚值平原鲁棒），默认。
|
||||
#[default]
|
||||
NelderMead,
|
||||
/// 粒子群（更全局，评估次数更高）。
|
||||
Pso,
|
||||
}
|
||||
|
||||
impl SolverKind {
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::NelderMead => "nelder-mead",
|
||||
Self::Pso => "pso",
|
||||
}
|
||||
}
|
||||
|
||||
/// 从文本解析。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回错误。
|
||||
pub fn parse(text: &str) -> Result<Self, SearchError> {
|
||||
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<Trial>,
|
||||
/// 最优候选。
|
||||
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<u64> {
|
||||
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<Option<(f64, [f64; AXIS_COUNT])>>,
|
||||
}
|
||||
|
||||
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<f64>;
|
||||
type Output = f64;
|
||||
|
||||
fn cost(&self, param: &Self::Param) -> Result<f64, ArgminError> {
|
||||
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<f64>> = 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<Vec<f64>, 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<Vec<f64>, 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<Ranked, SearchError> {
|
||||
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<Trial> = 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<Trial> = trials.iter().take(config.top_k.max(1)).copied().collect();
|
||||
|
||||
// ---------------- 阶段 2：多起点精修（并行） ----------------
|
||||
let coarse_count = trials.len() as u64;
|
||||
let refined: Vec<Trial> = 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}");
|
||||
}
|
||||
}
|
||||
@@ -10,16 +10,16 @@ description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
chassis-config = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
dji-esc-socketcan = { workspace = true }
|
||||
mecanum = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
|
||||
@@ -179,16 +179,10 @@ mod tests {
|
||||
let missing = MockMotor::new(2); // 无反馈
|
||||
let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
|
||||
let _ = mocks_ready;
|
||||
let wheels = wrap_wheels(
|
||||
[ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)],
|
||||
[GEAR; 4],
|
||||
);
|
||||
let ctrl = ChassisController::new(
|
||||
kinematics,
|
||||
wheels,
|
||||
(3.0, 3.0, 6.0),
|
||||
Duration::from_millis(200),
|
||||
);
|
||||
let wheels =
|
||||
wrap_wheels([ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [GEAR; 4]);
|
||||
let ctrl =
|
||||
ChassisController::new(kinematics, wheels, (3.0, 3.0, 6.0), Duration::from_millis(200));
|
||||
let (tx, rx) = watch::channel(Twist::ZERO);
|
||||
let handle = ctrl.spawn(rx, Duration::from_millis(1));
|
||||
|
||||
|
||||
@@ -1,13 +1,18 @@
|
||||
//! `chassisd` 库部分：配置模型、底盘控制循环与指令源抽象。
|
||||
//! `chassisd` 库部分：底盘控制循环与指令源抽象。
|
||||
//!
|
||||
//! 配置 schema 已独立为 [`chassis_config`]（守护进程与整定工具共用一份
|
||||
//! 定义），这里以 `config` 别名重新导出，保持 `chassisd::config::Config`
|
||||
//! 这一历史路径可用。
|
||||
//!
|
||||
//! 二进制入口见 `src/main.rs`。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod config;
|
||||
pub use chassis_config as config;
|
||||
|
||||
pub mod controller;
|
||||
pub mod source;
|
||||
|
||||
pub use config::{
|
||||
pub use chassis_config::{
|
||||
CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig,
|
||||
ProtocolFamily,
|
||||
};
|
||||
|
||||
@@ -87,7 +87,7 @@ async fn run(
|
||||
let link = if mock {
|
||||
let (bus_side, plant_side) = dji_esc::CanLink::pair(512);
|
||||
let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
|
||||
dji_esc::spawn_plant(plant_side, &esc_ids, bus_options.period);
|
||||
dji_esc_sim::spawn_plant(plant_side, &esc_ids, bus_options.period);
|
||||
info!(?esc_ids, "mock 模式：进程内电机仿真");
|
||||
bus_side
|
||||
} else {
|
||||
|
||||
@@ -7,7 +7,8 @@ use std::time::Duration;
|
||||
|
||||
use chassisd::config::Config;
|
||||
use chassisd::controller::{ChassisController, wrap_wheels};
|
||||
use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant};
|
||||
use dji_esc::{CanLink, EscBus, EscBusHandle};
|
||||
use dji_esc_sim::spawn_plant;
|
||||
use mecanum::Twist;
|
||||
use motor_core::MotorFeedback;
|
||||
use tokio::sync::watch;
|
||||
|
||||
@@ -0,0 +1,22 @@
|
||||
[package]
|
||||
name = "dji-esc-sim"
|
||||
description = "C620/C610 plant simulation: friction/delay model, scenario harness, and async plant rig"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,186 @@
|
||||
//! 被控对象集成：用同一组参数的**抖动样本**表达模型不确定性。
|
||||
//!
|
||||
//! 单点模型整定出来的增益往往对增益 / 时间常数 / 延迟 / 摩擦高度敏感
|
||||
//! （真车轮子之间、电量与温度变化都会漂移）。整定应针对一个**集成**做
|
||||
//! 鲁棒优化，而不是针对一条理想曲线做最优化。
|
||||
|
||||
use rand::Rng as _;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
|
||||
use crate::plant::PlantParams;
|
||||
|
||||
/// 集成相对基准参数的抖动幅度（比例，0 表示不抖动）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct PlantTolerances {
|
||||
/// 空载增益 ± 比例。
|
||||
pub gain_pct: f32,
|
||||
/// 时间常数 ± 比例。
|
||||
pub tau_pct: f32,
|
||||
/// 命令延迟在 `0..=delay_ticks_max` 拍内均匀取整。
|
||||
pub delay_ticks_max: u32,
|
||||
/// 摩擦三件套 ± 比例。
|
||||
pub friction_pct: f32,
|
||||
}
|
||||
|
||||
impl Default for PlantTolerances {
|
||||
fn default() -> Self {
|
||||
Self { gain_pct: 0.2, tau_pct: 0.3, delay_ticks_max: 3, friction_pct: 0.5 }
|
||||
}
|
||||
}
|
||||
|
||||
/// 一组被控对象参数（成员之间只有参数差异，模型结构相同）。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct PlantEnsemble {
|
||||
members: Vec<PlantParams>,
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,258 @@
|
||||
//! 离线场景评估：同步、无 I/O、无 async 的闭环重放。
|
||||
//!
|
||||
//! 本模块是自动整定的**求值内核**：给定一组速度环增益、一个被控对象与一条
|
||||
//! 场景，逐拍推进「控制器 → 限幅 → 被控对象」，返回完整 [`Trace`]。
|
||||
//!
|
||||
//! 与 `dji-esc` 的 `EscBus` 刻意保持**同一套物理帧语义**（见
|
||||
//! `tests/harness_parity.rs`）：
|
||||
//!
|
||||
//! ```text
|
||||
//! out = velocity_controller.update(setpoint, measured, dt) // 已按 ±current_limit 限幅
|
||||
//! raw = (out * direction) as i16 // 物理帧（direction = +1）
|
||||
//! cmd = clamp_current(model, raw.clamp(±current_limit)) // 电调满量程再夹一次
|
||||
//! ```
|
||||
//!
|
||||
//! 单次评估约 10^3~10^4 拍纯浮点运算，因此可以在 `rayon` 下并行搜索
|
||||
//! 上千组参数——这是"参数空间搜索"能落地的前提。
|
||||
|
||||
use dji_esc::{EscModel, clamp_current};
|
||||
use motor_core::{VelocityController, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::ensemble::PlantEnsemble;
|
||||
use crate::plant::{PlantMotor, PlantParams};
|
||||
use crate::scenario::Scenario;
|
||||
use crate::trace::Trace;
|
||||
|
||||
/// 闭环限幅与电调型号（决定 effort 的可达范围）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct LoopLimits {
|
||||
/// effort 限幅（raw）——同时是 PID 输出钳位点。
|
||||
pub current_limit_raw: i16,
|
||||
/// 电调型号（决定满量程）。
|
||||
pub model: EscModel,
|
||||
}
|
||||
|
||||
impl LoopLimits {
|
||||
/// `GM3508 + C620`、限流 12000 raw（≈14.6 A）——与 `EscConfig::gm3508` 一致。
|
||||
#[must_use]
|
||||
pub const fn gm3508() -> Self {
|
||||
Self { current_limit_raw: 12_000, model: EscModel::C620 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for LoopLimits {
|
||||
fn default() -> Self {
|
||||
Self::gm3508()
|
||||
}
|
||||
}
|
||||
|
||||
/// 离线评估失败原因。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum SimError {
|
||||
/// 增益含 NaN / inf（`Pid::new` 会 panic，必须在此拦下）。
|
||||
#[error("gains contain NaN or infinity")]
|
||||
NonFiniteGains,
|
||||
/// 场景或限幅退化（`dt <= 0`、总时长为 0、限流为 0 等）。
|
||||
#[error("degenerate scenario or limits")]
|
||||
DegenerateScenario,
|
||||
/// 数值发散（转速超出被控对象量程 100 倍）。
|
||||
#[error("simulation diverged at step {step}")]
|
||||
Diverged {
|
||||
/// 发散发生的采样序号。
|
||||
step: usize,
|
||||
},
|
||||
}
|
||||
|
||||
/// 在单台被控对象上重放一条场景。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 增益非有限、场景退化或数值发散时返回 [`SimError`]——调用方（整定器）
|
||||
/// 应把它折算成罚值，而不是把非法参数交给控制器。
|
||||
pub fn simulate(
|
||||
gains: VelocityGains,
|
||||
plant: &PlantParams,
|
||||
scenario: &Scenario,
|
||||
limits: LoopLimits,
|
||||
) -> Result<Trace, SimError> {
|
||||
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<Trace, SimError>)> {
|
||||
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(),
|
||||
"摩擦 + 延迟只会拖慢跟随"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
//! DJI 电调被控对象仿真：模型、场景与**离线评估 harness**。
|
||||
//!
|
||||
//! 本 crate 从 `dji-esc` 剥离而来，只负责"电调对面的那台电机"以及如何对它
|
||||
//! 做实验，不碰任何真实 I/O——总线接口是一对 `CanLink` 通道。
|
||||
//!
|
||||
//! # 模块职责
|
||||
//!
|
||||
//! - [`plant`]：被控对象模型（一阶转速 + 静/库仑/黏滞摩擦 + 命令延迟 + 负载）；
|
||||
//! - [`ensemble`]：参数集成（域随机化），表达模型不确定性；
|
||||
//! - [`scenario`]：设定值曲线与采样节拍（阶跃 / 斜坡 / 阶梯 / 升降）；
|
||||
//! - [`trace`]：试验轨迹（设定值、转速、effort、饱和标志）；
|
||||
//! - [`harness`]：**同步**闭环重放——自动整定的求值内核（纯 CPU、可并行）；
|
||||
//! - [`rig`]：**异步**假电调组，挂在 [`CanLink`] 上供 `EscBus` 联调。
|
||||
//!
|
||||
//! # 两条路径为什么都要有
|
||||
//!
|
||||
//! - `rig` 用于端到端联调（`chassisd --mock`、闭环集成测试），语义是
|
||||
//! "挂在总线上的假电调"；
|
||||
//! - `harness` 用于参数搜索，语义是"一条场景的数值积分"，单次评估只有
|
||||
//! 浮点运算、没有 tokio 任务与通道，因此可以在 `rayon` 下并行跑上千组
|
||||
//! 参数。两者的物理帧语义由 `tests/harness_parity.rs` 对齐。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod ensemble;
|
||||
pub mod harness;
|
||||
pub mod plant;
|
||||
pub mod rig;
|
||||
pub mod scenario;
|
||||
pub mod trace;
|
||||
|
||||
pub use ensemble::{PlantEnsemble, PlantTolerances};
|
||||
pub use harness::{LoopLimits, SimError, simulate, simulate_ensemble};
|
||||
pub use plant::{PlantMotor, PlantParams};
|
||||
pub use rig::{spawn_plant, spawn_plant_with};
|
||||
pub use scenario::{Scenario, SetpointProfile};
|
||||
pub use trace::{Trace, Window};
|
||||
|
||||
/// 复用的链路类型（`dji-esc` 的协议层与假电调通过它对接）。
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
@@ -0,0 +1,361 @@
|
||||
//! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。
|
||||
//!
|
||||
//! 电调坐标系（正电流 → 正转速），逐拍离散（`dt` 秒）：
|
||||
//!
|
||||
//! 1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才真正作用（模拟
|
||||
//! 链路 + 采样延迟；`0` 表示立即作用）。
|
||||
//! 2. **摩擦**：先由 `u` 与当前运动状态算出净转矩输入
|
||||
//! （静摩擦死区 → 库仑摩擦 → 黏滞阻尼，见 [`PlantMotor::step`]）。
|
||||
//! 3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
|
||||
//! 转速以时间常数 `tau` 一阶趋近（`alpha = 1 − exp(−dt/tau)`）。
|
||||
//! 4. **编码器积分与量化**：机械角按转速积分并对 8192 取模；反馈帧是
|
||||
//! `EscFeedback`，因此角度被截断为 `u16`、转速被截断为 `i16`——量化是
|
||||
//! 协议类型自带的，不需要额外开关。
|
||||
//!
|
||||
//! [`PlantParams::gm3508`] 复现最初的**无摩擦一阶模型**（`chassisd --mock`
|
||||
//! 与闭环测试依赖它，行为逐位不变）；[`PlantParams::gm3508_bench`] 是带摩擦
|
||||
//! 与延迟的**台架量级**预设，供自动整定使用。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use dji_esc::{ENCODER_TICKS, EscFeedback};
|
||||
|
||||
/// 转速绝对值低于该值（rpm）视为静止——静摩擦判别用。
|
||||
const STICK_RPM_EPS: f32 = 1.0;
|
||||
|
||||
/// 被控对象参数（电调坐标系）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct PlantParams {
|
||||
/// 空载增益（rpm / raw）：`GM3508` 实测约 0.565（≈ 9254 / 16384）。
|
||||
pub gain_rpm_per_raw: f32,
|
||||
/// 机电时间常数（秒）。
|
||||
pub tau_s: f32,
|
||||
/// 转速上限（rpm）。
|
||||
pub max_rpm: f32,
|
||||
/// 静摩擦阈值（raw）：静止且 |u| ≤ 该值时不产生任何运动（stick）。
|
||||
pub static_raw: f32,
|
||||
/// 库仑摩擦（raw）：与运动方向相反的恒定阻力。
|
||||
pub coulomb_raw: f32,
|
||||
/// 黏滞阻尼（raw / rpm）。
|
||||
pub viscous_raw_per_rpm: f32,
|
||||
/// 命令传输延迟（拍，`dt` 的整数倍）。
|
||||
pub delay_ticks: u32,
|
||||
/// 恒定外载（折算成 rpm，直接被目标转速减去）。
|
||||
pub load_rpm: f32,
|
||||
}
|
||||
|
||||
impl PlantParams {
|
||||
/// 无摩擦、无延迟的理想一阶模型（与历史 `PlantMotor::gm3508` 逐位一致）。
|
||||
#[must_use]
|
||||
pub const fn gm3508() -> Self {
|
||||
Self {
|
||||
gain_rpm_per_raw: 9254.0 / 16384.0,
|
||||
tau_s: 0.12,
|
||||
max_rpm: 9254.0,
|
||||
static_raw: 0.0,
|
||||
coulomb_raw: 0.0,
|
||||
viscous_raw_per_rpm: 0.0,
|
||||
delay_ticks: 0,
|
||||
load_rpm: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 台架量级预设：在 [`Self::gm3508`] 上叠加静摩擦 / 库仑摩擦 / 黏滞阻尼
|
||||
/// 与 2 拍命令延迟。
|
||||
///
|
||||
/// 摩擦量级取自 `docs/architecture.md` 与台架记录的空载实测（起动需
|
||||
/// 400~600 raw），**是量级估计而非辨识结果**；需要更贴合实车时可用
|
||||
/// [`Self::with_friction`] / [`Self::with_delay_ticks`] 覆盖。
|
||||
#[must_use]
|
||||
pub const fn gm3508_bench() -> Self {
|
||||
let mut params = Self::gm3508();
|
||||
params.static_raw = 450.0;
|
||||
params.coulomb_raw = 500.0;
|
||||
params.viscous_raw_per_rpm = 0.05;
|
||||
params.delay_ticks = 2;
|
||||
params
|
||||
}
|
||||
|
||||
/// 设置摩擦三件套（静摩擦阈值 / 库仑 / 黏滞）。
|
||||
#[must_use]
|
||||
pub const fn with_friction(
|
||||
mut self,
|
||||
static_raw: f32,
|
||||
coulomb_raw: f32,
|
||||
viscous_raw_per_rpm: f32,
|
||||
) -> Self {
|
||||
self.static_raw = static_raw;
|
||||
self.coulomb_raw = coulomb_raw;
|
||||
self.viscous_raw_per_rpm = viscous_raw_per_rpm;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置命令延迟（拍）。
|
||||
#[must_use]
|
||||
pub const fn with_delay_ticks(mut self, ticks: u32) -> Self {
|
||||
self.delay_ticks = ticks;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置恒定外载（rpm）。
|
||||
#[must_use]
|
||||
pub const fn with_load_rpm(mut self, rpm: f32) -> Self {
|
||||
self.load_rpm = rpm;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PlantMotor {
|
||||
params: PlantParams,
|
||||
speed_rpm: f32,
|
||||
angle_raw: f32,
|
||||
/// 命令延迟 FIFO（`delay_ticks == 0` 时恒为空）。
|
||||
delay_line: VecDeque<f32>,
|
||||
}
|
||||
|
||||
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, "复位后角度归零");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,147 @@
|
||||
//! 异步 plant rig：把被控对象挂到一对 [`CanLink`] 通道上。
|
||||
//!
|
||||
//! 与真实电调对调位置的"假电调"：消费命令帧（`0x200` / `0x1FF`），按
|
||||
//! `tick` 节拍产出反馈帧。`EscBus` 完全看不出区别，因此
|
||||
//! **协议 → 驱动 → 控制回路**的整条链路都能在无硬件环境下跑通
|
||||
//! （`chassisd --mock`、闭环集成测试）。
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{
|
||||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink, encode_feedback_frame,
|
||||
};
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
use tracing::trace;
|
||||
|
||||
use crate::plant::{PlantMotor, PlantParams};
|
||||
|
||||
/// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。
|
||||
///
|
||||
/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可（丢弃句柄 = 让任务独立运行）。
|
||||
pub fn spawn_plant(link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
|
||||
let specs = esc_ids.iter().map(|&id| (id, PlantParams::gm3508()));
|
||||
spawn_plant_with(link, specs, tick)
|
||||
}
|
||||
|
||||
/// 用逐电调的参数启动假电调组（异构轮子 / 域随机化场景）。
|
||||
pub fn spawn_plant_with(
|
||||
mut link: CanLink,
|
||||
specs: impl IntoIterator<Item = (u8, PlantParams)>,
|
||||
tick: Duration,
|
||||
) -> JoinHandle<()> {
|
||||
let specs: Vec<(u8, PlantParams)> = specs.into_iter().collect();
|
||||
let mut motors: HashMap<u8, PlantMotor> =
|
||||
specs.iter().map(|&(id, params)| (id, PlantMotor::new(params))).collect();
|
||||
let mut commands: HashMap<u8, i16> = 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<u8, i16>) {
|
||||
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<u8, i16> = (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();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,236 @@
|
||||
//! 场景定义：设定值曲线 + 采样节拍。
|
||||
//!
|
||||
//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、
|
||||
//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放，
|
||||
//! 因此整定结果可以跨后端比较。
|
||||
|
||||
/// 设定值曲线。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum SetpointProfile {
|
||||
/// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。
|
||||
Step,
|
||||
/// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。
|
||||
RampAndHold,
|
||||
/// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。
|
||||
Staircase {
|
||||
/// 阶梯数（≥ 1）。
|
||||
levels: u32,
|
||||
},
|
||||
/// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。
|
||||
UpDown,
|
||||
}
|
||||
|
||||
/// 一次试验的设定值与时间规格。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Scenario {
|
||||
/// 控制 / 采样节拍（秒）。
|
||||
pub dt_s: f32,
|
||||
/// 斜坡时间（秒）；`0` 表示瞬时。
|
||||
pub ramp_s: f32,
|
||||
/// 保持时间（秒）。
|
||||
pub hold_s: f32,
|
||||
/// 目标转子转速（rpm）。
|
||||
pub target_rpm: f32,
|
||||
/// 设定值曲线。
|
||||
pub profile: SetpointProfile,
|
||||
}
|
||||
|
||||
impl Scenario {
|
||||
/// 默认控制节拍（1 kHz，与 C620 命令周期一致）。
|
||||
pub const DEFAULT_DT_S: f32 = 0.001;
|
||||
|
||||
/// 阶跃 + 保持。
|
||||
#[must_use]
|
||||
pub const fn step(target_rpm: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s: 0.0,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::Step,
|
||||
}
|
||||
}
|
||||
|
||||
/// 斜坡 + 保持（台架默认整定场景）。
|
||||
#[must_use]
|
||||
pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::RampAndHold,
|
||||
}
|
||||
}
|
||||
|
||||
/// 换一个采样节拍。
|
||||
#[must_use]
|
||||
pub const fn with_dt_s(mut self, dt_s: f32) -> Self {
|
||||
self.dt_s = dt_s;
|
||||
self
|
||||
}
|
||||
|
||||
/// 换一条设定值曲线。
|
||||
#[must_use]
|
||||
pub const fn with_profile(mut self, profile: SetpointProfile) -> Self {
|
||||
self.profile = profile;
|
||||
self
|
||||
}
|
||||
|
||||
/// 场景总时长（秒）。
|
||||
#[must_use]
|
||||
pub fn total_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => self.hold_s,
|
||||
SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => {
|
||||
self.ramp_s + self.hold_s
|
||||
}
|
||||
SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s),
|
||||
}
|
||||
}
|
||||
|
||||
/// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。
|
||||
#[must_use]
|
||||
pub fn samples(&self) -> usize {
|
||||
let total = self.total_s();
|
||||
let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0;
|
||||
let total_ok = total.is_finite() && total > 0.0;
|
||||
if !dt_ok || !total_ok {
|
||||
return 1;
|
||||
}
|
||||
((total / self.dt_s).round() as usize).max(1)
|
||||
}
|
||||
|
||||
/// `t_s` 时刻的设定值（rpm）。
|
||||
#[must_use]
|
||||
pub fn setpoint_at(&self, t_s: f32) -> f32 {
|
||||
let target = self.target_rpm;
|
||||
let ramp = self.ramp_s;
|
||||
match self.profile {
|
||||
SetpointProfile::Step => target,
|
||||
SetpointProfile::RampAndHold => {
|
||||
if ramp > 0.0 && t_s < ramp {
|
||||
target * (t_s / ramp)
|
||||
} else {
|
||||
target
|
||||
}
|
||||
}
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let levels = levels.max(1);
|
||||
let total = ramp + self.hold_s;
|
||||
if total <= 0.0 {
|
||||
return target;
|
||||
}
|
||||
let rung_s = total / levels as f32;
|
||||
let index = (t_s / rung_s).floor().max(0.0) as u32;
|
||||
target * ((index.min(levels - 1) + 1) as f32 / levels as f32)
|
||||
}
|
||||
SetpointProfile::UpDown => {
|
||||
let up_end = ramp;
|
||||
let hold_end = up_end + self.hold_s;
|
||||
let down_end = hold_end + ramp;
|
||||
if t_s < up_end {
|
||||
if ramp > 0.0 { target * (t_s / ramp) } else { target }
|
||||
} else if t_s < hold_end {
|
||||
target
|
||||
} else if t_s < down_end {
|
||||
let progress = (t_s - hold_end) / ramp;
|
||||
target * (1.0 - progress)
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。
|
||||
#[must_use]
|
||||
pub fn steady_start_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => 0.0,
|
||||
SetpointProfile::RampAndHold => self.ramp_s,
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let level_s = self.level_s(levels);
|
||||
((levels.max(1) - 1) as f32) * level_s
|
||||
}
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的终点（秒）。
|
||||
#[must_use]
|
||||
pub fn steady_end_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
// 升降场景只评价上升段后的保持段。
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
_ => self.total_s(),
|
||||
}
|
||||
}
|
||||
|
||||
/// 阶梯的单级时长（秒）。
|
||||
fn level_s(&self, levels: u32) -> f32 {
|
||||
let levels = levels.max(1);
|
||||
let total = self.ramp_s + self.hold_s;
|
||||
if total > 0.0 { total / levels as f32 } else { 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
|
||||
#[track_caller]
|
||||
fn assert_close(actual: f32, expected: f32) {
|
||||
assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn step_is_immediate_and_lasts_hold() {
|
||||
let s = Scenario::step(1000.0, 0.5);
|
||||
assert!((s.total_s() - 0.5).abs() < 1e-6);
|
||||
assert_close(s.setpoint_at(0.0), 1000.0);
|
||||
assert_close(s.setpoint_at(0.4), 1000.0);
|
||||
assert_eq!(s.samples(), 500);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ramp_is_linear_then_holds() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0);
|
||||
assert_close(s.setpoint_at(0.0), 0.0);
|
||||
assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 1000.0);
|
||||
assert_close(s.setpoint_at(1.1), 1000.0);
|
||||
assert!((s.total_s() - 1.2).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn staircase_rises_in_equal_levels() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0)
|
||||
.with_profile(SetpointProfile::Staircase { levels: 4 });
|
||||
assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3);
|
||||
// 越界采样被夹在最后一级。
|
||||
assert_close(s.setpoint_at(5.0), 1000.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn up_down_returns_to_zero() {
|
||||
let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown);
|
||||
assert!((s.total_s() - 0.6).abs() < 1e-6);
|
||||
assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 600.0);
|
||||
assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.59), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_scenario_reports_one_sample() {
|
||||
let s = Scenario::step(1.0, 0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
let s = Scenario::step(1.0, 1.0).with_dt_s(0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,230 @@
|
||||
//! 试验轨迹：一次场景重放的全部采样。
|
||||
|
||||
/// 一条完整试验轨迹（各列等长，索引即采样序号）。
|
||||
#[derive(Debug, Clone, Default, PartialEq)]
|
||||
pub struct Trace {
|
||||
/// 采样时刻（秒）。
|
||||
pub t_s: Vec<f32>,
|
||||
/// 该时刻的设定值（rpm）。
|
||||
pub setpoint_rpm: Vec<f32>,
|
||||
/// 该时刻的实测转速（rpm，物理方向）。
|
||||
pub rpm: Vec<f32>,
|
||||
/// 该时刻下发的 effort（raw，已限幅）。
|
||||
pub effort_raw: Vec<i16>,
|
||||
/// 该时刻控制器输出是否贴着限幅（饱和）。
|
||||
pub saturated: Vec<bool>,
|
||||
}
|
||||
|
||||
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<usize>,
|
||||
}
|
||||
|
||||
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::<f32>() / 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);
|
||||
}
|
||||
}
|
||||
@@ -8,12 +8,15 @@ use std::time::Duration;
|
||||
|
||||
use dji_esc::{
|
||||
BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback,
|
||||
encode_feedback_frame, spawn_plant,
|
||||
encode_feedback_frame,
|
||||
};
|
||||
use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{sleep, timeout};
|
||||
use dji_esc_sim::spawn_plant;
|
||||
use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive, VelocityGains};
|
||||
use tokio::sync::mpsc;
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval, sleep, timeout};
|
||||
|
||||
/// 与 `PlantMotor::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
|
||||
/// 与 `PlantParams::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
|
||||
fn tuned(esc_id: u8, direction: f32) -> EscConfig {
|
||||
let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction);
|
||||
cfg.kp = 8.0;
|
||||
@@ -36,6 +39,35 @@ async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
|
||||
cond()
|
||||
}
|
||||
|
||||
/// 与 [`wait_until`] 相同，但每次轮询都重新下发 `target_rpm`。
|
||||
///
|
||||
/// **命令是租约**：驱动在 [`BusOptions::command_timeout`]（默认 50ms）内未收到
|
||||
/// 新命令就强制 0 输出。真实底盘循环每 1ms 重发一次，这里以 5ms 节拍模拟。
|
||||
async fn hold_until<M: VelocityDrive>(
|
||||
total_ms: u64,
|
||||
motor: &M,
|
||||
target_rpm: f32,
|
||||
mut cond: impl FnMut() -> bool,
|
||||
) -> bool {
|
||||
let mut waited = 0u64;
|
||||
loop {
|
||||
motor.set_velocity(target_rpm); // 续租
|
||||
if cond() {
|
||||
return true;
|
||||
}
|
||||
if waited >= total_ms {
|
||||
return false;
|
||||
}
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
waited += 5;
|
||||
}
|
||||
}
|
||||
|
||||
/// 保持 `target_rpm` 共 `ms` 毫秒（期间持续续租），仅推进仿真时间。
|
||||
async fn hold<M: VelocityDrive>(motor: &M, target_rpm: f32, ms: u64) {
|
||||
let _ = hold_until(ms, motor, target_rpm, || false).await;
|
||||
}
|
||||
|
||||
/// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。
|
||||
async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) {
|
||||
for &id in esc_ids {
|
||||
@@ -65,12 +97,12 @@ async fn velocity_loop_converges() {
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
motor.set_velocity(3000.0);
|
||||
let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 2950.0).await;
|
||||
let ok = hold_until(2000, &motor, 3000.0, || motor.state().rotor_speed_rpm >= 2950.0).await;
|
||||
assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
|
||||
|
||||
// 稳态保持（速度无漂移、反馈持续流入）。
|
||||
// 稳态保持（速度无漂移、反馈持续流入）；期间必须继续续租命令。
|
||||
let count_before = motor.state().feedback_count;
|
||||
sleep(Duration::from_millis(300)).await;
|
||||
hold(&motor, 3000.0, 300).await;
|
||||
let st = motor.state();
|
||||
assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}");
|
||||
assert!(
|
||||
@@ -93,7 +125,7 @@ async fn reversed_motor_converges_in_physical_frame() {
|
||||
let motor = bus.motor(2).unwrap().clone();
|
||||
|
||||
motor.set_velocity(2000.0);
|
||||
let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 1950.0).await;
|
||||
let ok = hold_until(2000, &motor, 2000.0, || motor.state().rotor_speed_rpm >= 1950.0).await;
|
||||
assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
|
||||
|
||||
let st = motor.state();
|
||||
@@ -135,6 +167,7 @@ async fn current_limit_clamps_saturation() {
|
||||
|
||||
let mut saw_saturated = false;
|
||||
for _ in 0..5 {
|
||||
bus.motor(1).unwrap().set_velocity(30000.0); // 续租，避免读到租约到期后的 0
|
||||
let frame = recv_command(&mut peer).await;
|
||||
let raw = slot_current(&frame, 0);
|
||||
assert!(raw <= 5000, "current {raw} exceeds limit 5000");
|
||||
@@ -144,6 +177,71 @@ async fn current_limit_clamps_saturation() {
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 命令租约
|
||||
|
||||
/// 反馈喂食器：每 5ms 注入一帧所有电机的反馈，让电机保持"在线"，
|
||||
/// 从而把「租约到期归零」与「离线保护归零」区分开。
|
||||
fn spawn_feedback(yield_tx: mpsc::Sender<CanFrame>, esc_ids: Vec<u8>) -> JoinHandle<()> {
|
||||
tokio::spawn(async move {
|
||||
let mut ticker = interval(Duration::from_millis(5));
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
|
||||
loop {
|
||||
ticker.tick().await;
|
||||
for &id in &esc_ids {
|
||||
if yield_tx.send(encode_feedback_frame(id, &fb)).await.is_err() {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
/// 一次性 `set_velocity` 在租约内出力，租约到期后必须自动归零；
|
||||
/// 重新下发又能恢复出力——这就是"命令是租约"契约的可执行规范。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn command_lease_expires_and_renews() {
|
||||
let (bus_link, mut peer) = CanLink::pair(256);
|
||||
let feeder = spawn_feedback(peer.outgoing_tx.clone(), vec![1]);
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。
|
||||
motor.set_velocity(3000.0);
|
||||
let mut live = false;
|
||||
for _ in 0..30 {
|
||||
if slot_current(&recv_command(&mut peer).await, 0) != 0 {
|
||||
live = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(live, "租约内应输出非零电流");
|
||||
|
||||
// 2) 沉默 200ms（> 50ms 租约）→ 必须强制归零。
|
||||
sleep(Duration::from_millis(200)).await;
|
||||
// 排空沉默期间积压的帧：其中前段仍带着租约到期前的旧命令。
|
||||
while peer.incoming_rx.try_recv().is_ok() {}
|
||||
sleep(Duration::from_millis(10)).await;
|
||||
for _ in 0..5 {
|
||||
let raw = slot_current(&recv_command(&mut peer).await, 0);
|
||||
assert_eq!(raw, 0, "租约到期后必须归零，实测 {raw}");
|
||||
}
|
||||
|
||||
// 3) 续租 → 恢复出力。
|
||||
motor.set_velocity(3000.0);
|
||||
let mut renewed = false;
|
||||
for _ in 0..30 {
|
||||
if slot_current(&recv_command(&mut peer).await, 0) != 0 {
|
||||
renewed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(renewed, "续租后应恢复非零电流");
|
||||
|
||||
feeder.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 组帧与路由
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
@@ -160,6 +258,9 @@ async fn current_commands_route_to_group_slots() {
|
||||
let mut saw_200 = false;
|
||||
let mut saw_1ff = false;
|
||||
for _ in 0..20 {
|
||||
// 续租两条直发命令，避免读到租约到期后的 0 槽。
|
||||
bus.motor(3).unwrap().set_effort_raw(1234);
|
||||
bus.motor(5).unwrap().set_effort_raw(-567);
|
||||
let frame = recv_command(&mut peer).await;
|
||||
match frame.id {
|
||||
0x200 if !saw_200 => {
|
||||
@@ -273,3 +374,46 @@ fn spawn_rejects_bad_configs() {
|
||||
"expected InvalidConfig, got {err:?}"
|
||||
);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 增益热更新
|
||||
|
||||
/// 自动整定的真机后端靠这条路径逐次试验：不重启总线，只换增益。
|
||||
/// 契约：① 读取即得到最新增益；② 换增益后控制器**状态复位**（无旧积分残留）；
|
||||
/// ③ 限幅等安全字段不受影响。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn velocity_gains_can_be_hot_swapped() {
|
||||
let (bus_link, plant_link) = CanLink::pair(256);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let cfg = tuned(1, 1.0);
|
||||
let limit = i32::from(cfg.current_limit);
|
||||
let bus = EscBus::spawn(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// ① 初始增益来自 EscConfig（含前馈）。
|
||||
assert_eq!(motor.velocity_gains(), cfg.velocity_gains());
|
||||
|
||||
// 让旧增益的积分先积累起来（大静差 + 深度饱和）。
|
||||
motor.set_velocity(9000.0);
|
||||
hold(&motor, 9000.0, 200).await;
|
||||
let stale = motor.velocity_gains();
|
||||
|
||||
// ② 热更新：控制器被重建。
|
||||
let swapped = VelocityGains::new(1.0, 0.0, 0.0, 0.0);
|
||||
motor.set_velocity_gains(swapped);
|
||||
assert_eq!(motor.velocity_gains(), swapped);
|
||||
assert_ne!(motor.velocity_gains(), stale);
|
||||
|
||||
// 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。
|
||||
motor.set_velocity(30_000.0);
|
||||
let saturated =
|
||||
hold_until(500, &motor, 30_000.0, || i32::from(motor.state().effort_raw) >= limit).await;
|
||||
assert!(saturated, "effort = {} 未达到限幅 {}", motor.state().effort_raw, limit);
|
||||
|
||||
// ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。
|
||||
let effort = i32::from(motor.state().effort_raw);
|
||||
assert_eq!(effort, limit, "饱和值必须正好是 current_limit");
|
||||
assert_eq!(motor.velocity_gains(), swapped, "热更新后增益保持不变");
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
@@ -0,0 +1,99 @@
|
||||
//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。
|
||||
//!
|
||||
//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是
|
||||
//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移，
|
||||
//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与
|
||||
//! 同一条场景下比较稳态与 effort。
|
||||
//!
|
||||
//! 允许的差异来自**离散化细节**（异步侧每拍实测 `dt`、反馈比命令晚一拍），
|
||||
//! 因此比较的是稳态量级而不是逐点相等。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{BusOptions, CanLink, EscBus, EscConfig};
|
||||
use dji_esc_sim::{LoopLimits, PlantParams, Scenario, simulate, spawn_plant};
|
||||
use motor_core::{MotorFeedback, VelocityDrive, VelocityGains};
|
||||
use tokio::time::sleep;
|
||||
|
||||
/// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。
|
||||
const GAINS: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0);
|
||||
const TARGET_RPM: f32 = 3000.0;
|
||||
const HOLD_MS: u64 = 2000;
|
||||
/// 稳态采样窗口（虚拟时间，毫秒）。
|
||||
const SAMPLE_FROM_MS: u64 = 1500;
|
||||
|
||||
/// 同步路径：最后 500 ms 的稳态统计。
|
||||
fn sync_steady_state() -> (f32, f32) {
|
||||
let scenario = Scenario::step(TARGET_RPM, HOLD_MS as f32 / 1000.0);
|
||||
let trace = simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508())
|
||||
.expect("simulation must succeed");
|
||||
let window = trace.window(SAMPLE_FROM_MS as f32 / 1000.0, HOLD_MS as f32 / 1000.0);
|
||||
let mean_effort =
|
||||
window.effort_raw().iter().map(|&raw| f32::from(raw)).sum::<f32>() / window.len() as f32;
|
||||
(window.mean_rpm(), mean_effort)
|
||||
}
|
||||
|
||||
/// 异步路径：`EscBus` + 假电调，逐拍续租直到 2 秒（虚拟时间）。
|
||||
async fn async_steady_state() -> (f32, f32) {
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let mut cfg = EscConfig::gm3508("front_left", 1, 1.0);
|
||||
cfg.kp = GAINS.kp;
|
||||
cfg.ki = GAINS.ki;
|
||||
cfg.kd = GAINS.kd;
|
||||
cfg.feedforward = GAINS.feedforward;
|
||||
cfg.current_limit = LoopLimits::gm3508().current_limit_raw;
|
||||
let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).expect("bus must start");
|
||||
let motor = bus.motor(1).expect("motor 1 exists").clone();
|
||||
|
||||
// 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。
|
||||
let mut waited = 0u64;
|
||||
let mut effort_sum = 0.0f32;
|
||||
let mut rpm_sum = 0.0f32;
|
||||
let mut samples = 0u32;
|
||||
while waited < HOLD_MS {
|
||||
motor.set_velocity(TARGET_RPM);
|
||||
if waited >= SAMPLE_FROM_MS {
|
||||
let state = motor.state();
|
||||
effort_sum += f32::from(state.effort_raw);
|
||||
rpm_sum += state.rotor_speed_rpm;
|
||||
samples += 1;
|
||||
}
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
waited += 5;
|
||||
}
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
(rpm_sum / samples as f32, effort_sum / samples as f32)
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn synchronous_harness_matches_async_closed_loop() {
|
||||
let (sync_rpm, sync_effort) = sync_steady_state();
|
||||
let (async_rpm, async_effort) = async_steady_state().await;
|
||||
|
||||
// 稳态转速：两条路径都应收敛到目标附近（理想模型 + 精确前馈 → 稳态无差）。
|
||||
let rpm_tolerance = 0.05 * TARGET_RPM;
|
||||
assert!(
|
||||
(sync_rpm - async_rpm).abs() < rpm_tolerance,
|
||||
"steady rpm mismatch: sync {sync_rpm}, async {async_rpm}"
|
||||
);
|
||||
assert!(
|
||||
(sync_rpm - TARGET_RPM).abs() < rpm_tolerance,
|
||||
"sync steady state off target: {sync_rpm}"
|
||||
);
|
||||
assert!(
|
||||
(async_rpm - TARGET_RPM).abs() < rpm_tolerance,
|
||||
"async steady state off target: {async_rpm}"
|
||||
);
|
||||
|
||||
// effort：前馈主导，两条路径量级必须接近（±20% 或 ±300 raw 取大者）。
|
||||
let effort_tolerance = (0.2 * sync_effort.abs()).max(300.0);
|
||||
assert!(
|
||||
(sync_effort - async_effort).abs() < effort_tolerance,
|
||||
"steady effort mismatch: sync {sync_effort}, async {async_effort}"
|
||||
);
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
[package]
|
||||
name = "dji-esc-slcan"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "SLCAN serial backend (e.g. /dev/ttyACM0) bridging to can-link channels"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
slcan = "0.1.3"
|
||||
serial = "0.4"
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
@@ -0,0 +1,348 @@
|
||||
//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can_link::CanLink`]。
|
||||
//!
|
||||
//! 基于 [`slcan`] crate（Lawicel ASCII 协议）：`S8` = 1 Mbps（C620 标准），`O` 打开通道。
|
||||
//! 阻塞串口 I/O 跑在专用线程上，与 SocketCAN 桥一样只暴露通道端点——对本 crate
|
||||
//! 的调用方而言，`dji-esc` 看到的是一个普通 [`CanLink`]，与传输介质无关。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
use std::io;
|
||||
use std::sync::Arc;
|
||||
use std::sync::atomic::{AtomicU64, Ordering};
|
||||
use std::thread;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use can_link::{CanFrame as LinkFrame, CanLink};
|
||||
use serial::prelude::*;
|
||||
use slcan::{BitRate, CanSocket};
|
||||
use tokio::sync::mpsc;
|
||||
use tracing::{debug, error, info, warn};
|
||||
|
||||
/// 链路通道缓冲（**故意很浅**）。
|
||||
///
|
||||
/// 两个方向的需求完全相反：
|
||||
///
|
||||
/// - **命令方向**（bus → 桥）：必须浅。驱动按 1kHz 生产命令，而 slcan 一次 write
|
||||
/// 在 CDC ACM 上要约 2ms，下行只能到几百帧/秒。若缓冲很深，队列会被"过时命令"
|
||||
/// 灌满，ESC 实际执行的是**一秒多以前**算出来的电流——速度环等于开了 1.4s 的
|
||||
/// 延迟，必然失控跑飞。取小值让 `send().await` 立刻回压，驱动自然降速到链路
|
||||
/// 能承受的节拍，每一帧都是**按最新反馈现算**的。
|
||||
/// - **反馈方向**（桥 → bus）：是采样流，来不及就丢（见 `pump` 的 `try_send`），
|
||||
/// 因此不需要深队列。
|
||||
const CHANNEL_BOUND: usize = 8;
|
||||
|
||||
/// 经典 CAN 标准帧 ID 上界（11 位）。超出者不是标准帧，一律丢弃：
|
||||
/// SLCAN 的 `t` 命令只承载 3 位十六进制 ID，截断后会把扩展帧冒充成标准帧。
|
||||
const MAX_STANDARD_ID: u32 = 0x7FF;
|
||||
|
||||
/// 反馈方向帧过滤：只接受「标准 ID + 8 字节」的数据帧。
|
||||
///
|
||||
/// C620 反馈固定 8 字节；DLC 不足的帧若放行，协议层会按越界读取拒绝，
|
||||
/// 但更早丢弃可以省掉一次无谓的解码。
|
||||
#[must_use]
|
||||
fn accepted_rx(frame: &slcan::CanFrame) -> Option<LinkFrame> {
|
||||
if frame.id > MAX_STANDARD_ID || frame.dlc != 8 {
|
||||
return None;
|
||||
}
|
||||
Some(LinkFrame::new(frame.id, frame.data))
|
||||
}
|
||||
|
||||
/// slcan CAN 波特率档位（对应协议 `Sn` 命令）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub enum CanSpeed {
|
||||
/// `S6`：500 kbps。
|
||||
K500,
|
||||
/// `S8`：1 Mbps（C620 标准）。
|
||||
#[default]
|
||||
M1,
|
||||
}
|
||||
|
||||
impl CanSpeed {
|
||||
fn to_slcan(self) -> BitRate {
|
||||
match self {
|
||||
Self::K500 => BitRate::Setup500Kbit,
|
||||
Self::M1 => BitRate::Setup1Mbit,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开参数。
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct SlcanOptions {
|
||||
/// 串口波特率（USB CDC ACM 常见 2_000_000；部分适配器 115_200）。
|
||||
pub serial_baud: u32,
|
||||
/// CAN 波特率档位。
|
||||
pub speed: CanSpeed,
|
||||
/// 串口超时。**读写共用**（见 `pump` 的说明）：`serial` crate 的
|
||||
/// `TTYPort::write` 也先按本超时 `poll(POLLOUT)`。
|
||||
///
|
||||
/// 太短会把正常的"这一拍写不出去"误判成写失败。总线繁忙时读几乎立即返回，
|
||||
/// 因此放大它只影响"总线完全静默"时的下行延迟，默认 50ms。
|
||||
pub read_timeout: Duration,
|
||||
}
|
||||
|
||||
impl Default for SlcanOptions {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
serial_baud: 2_000_000,
|
||||
speed: CanSpeed::M1,
|
||||
read_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开 slcan 设备并桥接为 [`CanLink`]（默认 1 Mbps / 2 Mbaud）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足、波特率不被支持等 `io::Error`。
|
||||
pub fn open_link(device: &str) -> io::Result<CanLink> {
|
||||
open_link_with(device, SlcanOptions::default())
|
||||
}
|
||||
|
||||
/// 按指定参数打开 slcan 设备。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 同 [`open_link`]。
|
||||
pub fn open_link_with(device: &str, options: SlcanOptions) -> io::Result<CanLink> {
|
||||
let mut port = serial::open(device)?;
|
||||
port.reconfigure(&|settings| {
|
||||
// `serial::BaudOther` 以 `usize` 计数；u32 → usize 在 32/64 位目标上均无损。
|
||||
settings.set_baud_rate(serial::BaudOther(options.serial_baud as usize))?;
|
||||
Ok(())
|
||||
})?;
|
||||
port.set_timeout(options.read_timeout)?;
|
||||
|
||||
let mut socket = CanSocket::new(port);
|
||||
socket.open(options.speed.to_slcan())?;
|
||||
info!(device, serial_baud = options.serial_baud, speed = ?options.speed, "slcan opened");
|
||||
|
||||
let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
|
||||
let CanLink { outgoing_tx, incoming_rx } = bridge_side;
|
||||
let stats = Arc::new(LinkStats::default());
|
||||
|
||||
thread::Builder::new()
|
||||
.name("slcan-bridge".into())
|
||||
.spawn(move || pump(socket, outgoing_tx, incoming_rx, stats))?;
|
||||
Ok(bus_side)
|
||||
}
|
||||
|
||||
#[derive(Default)]
|
||||
struct LinkStats {
|
||||
rx: AtomicU64,
|
||||
/// 因 bus 侧来不及消费而丢弃的反馈帧（反馈是采样流，丢弃是正常降级）。
|
||||
rx_dropped: AtomicU64,
|
||||
tx: AtomicU64,
|
||||
/// 写通道未就绪而推迟到下一轮重试的命令帧次数（帧被保留，不丢）。
|
||||
tx_retry: AtomicU64,
|
||||
quiet_bucket: AtomicU64,
|
||||
}
|
||||
|
||||
/// 每轮最多连续发送的命令帧数。
|
||||
///
|
||||
/// 电调总线按 1kHz 持续产出命令帧；若不加界，一次"排空"就可能永远排空——
|
||||
/// 尤其在某次写 poll 超时之后，新帧仍在流入，接收方向会被**永久饿死**。
|
||||
const TX_BURST: usize = 8;
|
||||
|
||||
/// 每轮最多读取的反馈帧数。
|
||||
///
|
||||
/// 一次 `write` 在 CDC ACM 上可能要 ~2ms，而发送上界是 8 帧，于是"每轮只读一帧"
|
||||
/// 会让接收被发送压到几十 Hz。这里每轮把已缓冲的反馈尽量读完（无数据即刻返回），
|
||||
/// 双向都不会饿死。
|
||||
const RX_BURST: usize = 64;
|
||||
|
||||
/// 瞬时（可恢复）I/O 错误：错过这一拍不代表链路坏了。
|
||||
///
|
||||
/// `serial` crate 的 `TTYPort` 把**同一个** timeout 同时用于读和写——
|
||||
/// `write()` 先做 `poll(POLLOUT, timeout)`，超时即返回 `TimedOut`。
|
||||
/// 这类事件在 USB CDC ACM 上很常见，绝不能当成致命错误。
|
||||
fn is_transient(e: &io::Error) -> bool {
|
||||
matches!(
|
||||
e.kind(),
|
||||
io::ErrorKind::TimedOut | io::ErrorKind::WouldBlock | io::ErrorKind::Interrupted
|
||||
)
|
||||
}
|
||||
|
||||
/// 写一帧，并区分**瞬时**与**致命**错误。
|
||||
///
|
||||
/// 返回 `Ok(true)` = 已写出；`Ok(false)` = 通道暂时写不动，调用方应保留该帧、
|
||||
/// **立刻让位给接收方向**，下一轮再试；`Err` = 真·致命（设备拔出、BrokenPipe 等）。
|
||||
///
|
||||
/// 关键：**写不动绝不能让接收方向一起停摆**。早期实现既无发送上界、又把
|
||||
/// `TimedOut` 当致命错误直接 return，结果第一拍命令没写出去就把整条链路带停了。
|
||||
fn write_frame(socket: &mut CanSocket<serial::SystemPort>, frame: &LinkFrame) -> io::Result<bool> {
|
||||
match socket.write(frame.id, &frame.data) {
|
||||
Ok(_) => Ok(true),
|
||||
Err(e) if is_transient(&e) => {
|
||||
debug!(error = %e, id = frame.id, "slcan write not ready, retry next round");
|
||||
Ok(false)
|
||||
}
|
||||
Err(e) => Err(e),
|
||||
}
|
||||
}
|
||||
|
||||
/// 单线程双向泵：优先排空待发命令，随后读一帧反馈。
|
||||
///
|
||||
/// `serial::SystemPort` 即 `serial::open` 返回的平台原生端口类型
|
||||
/// （Unix = `TTYPort`），因此本函数在 Linux 目标板与 macOS 开发机上同源。
|
||||
///
|
||||
/// 发送端与统计按值传入是有意的：它们随泵线程独占所有权，线程退出时
|
||||
/// `tx` 被 drop，总线侧据此观察到链路关闭。
|
||||
#[allow(clippy::needless_pass_by_value)]
|
||||
fn pump(
|
||||
mut socket: CanSocket<serial::SystemPort>,
|
||||
tx: mpsc::Sender<LinkFrame>,
|
||||
mut rx: mpsc::Receiver<LinkFrame>,
|
||||
stats: Arc<LinkStats>,
|
||||
) {
|
||||
let mut last_rx_ok = Instant::now();
|
||||
let mut last_stats = Instant::now();
|
||||
// 上一轮没能写出去的帧：保留而非丢弃，下一轮优先重试。
|
||||
let mut pending: Option<LinkFrame> = None;
|
||||
loop {
|
||||
// 写：**有界**排空待发命令；一旦通道写不动就交给接收方向。
|
||||
for _ in 0..TX_BURST {
|
||||
let frame = match pending.take() {
|
||||
Some(frame) => frame,
|
||||
None => match rx.try_recv() {
|
||||
Ok(frame) => frame,
|
||||
Err(mpsc::error::TryRecvError::Empty) => break,
|
||||
Err(mpsc::error::TryRecvError::Disconnected) => {
|
||||
debug!("bus side closed, slcan bridge exiting");
|
||||
let _ = socket.close();
|
||||
return;
|
||||
}
|
||||
},
|
||||
};
|
||||
// P-1：拒绝非标准 ID，避免 `as u16` 截断后冒充标准帧。
|
||||
if frame.id > MAX_STANDARD_ID {
|
||||
debug!(id = frame.id, "drop frame with non-standard CAN id");
|
||||
continue;
|
||||
}
|
||||
match write_frame(&mut socket, &frame) {
|
||||
Ok(true) => {
|
||||
stats.tx.fetch_add(1, Ordering::Relaxed);
|
||||
}
|
||||
Ok(false) => {
|
||||
stats.tx_retry.fetch_add(1, Ordering::Relaxed);
|
||||
pending = Some(frame);
|
||||
break;
|
||||
}
|
||||
Err(e) => {
|
||||
error!(error = %e, "slcan write failed, bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 周期性链路统计：现场判断"命令到底发出去没有"靠它。
|
||||
if last_stats.elapsed() >= Duration::from_secs(5) {
|
||||
last_stats = Instant::now();
|
||||
debug!(
|
||||
rx = stats.rx.load(Ordering::Relaxed),
|
||||
rx_dropped = stats.rx_dropped.load(Ordering::Relaxed),
|
||||
tx = stats.tx.load(Ordering::Relaxed),
|
||||
tx_retry = stats.tx_retry.load(Ordering::Relaxed),
|
||||
"slcan link stats"
|
||||
);
|
||||
}
|
||||
|
||||
// 读：**有界**批量读取已缓冲的反馈；无数据立即让位给发送方向。
|
||||
for _ in 0..RX_BURST {
|
||||
match socket.read() {
|
||||
Ok(frame) => {
|
||||
last_rx_ok = Instant::now();
|
||||
stats.rx.fetch_add(1, Ordering::Relaxed);
|
||||
let Some(frame) = accepted_rx(&frame) else {
|
||||
debug!(
|
||||
id = frame.id,
|
||||
dlc = frame.dlc,
|
||||
"drop non-standard or short slcan frame"
|
||||
);
|
||||
continue;
|
||||
};
|
||||
// 反馈是采样流：来不及就丢，绝不阻塞泵线程（否则又会饿死反方向）。
|
||||
match tx.try_send(frame) {
|
||||
Ok(()) => {}
|
||||
Err(mpsc::error::TrySendError::Full(_)) => {
|
||||
stats.rx_dropped.fetch_add(1, Ordering::Relaxed);
|
||||
}
|
||||
Err(mpsc::error::TrySendError::Closed(_)) => {
|
||||
debug!("bus side closed, slcan rx exiting");
|
||||
let _ = socket.close();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(e) if is_transient(&e) => {
|
||||
// S-5：长时间无反馈 → 怀疑接线/bus-off/电调未上电。
|
||||
let quiet = last_rx_ok.elapsed();
|
||||
if quiet > Duration::from_secs(1) {
|
||||
let bucket = quiet.as_secs();
|
||||
if stats.quiet_bucket.swap(bucket, Ordering::Relaxed) != bucket {
|
||||
warn!(
|
||||
quiet_ms = quiet.as_millis() as u64,
|
||||
rx = stats.rx.load(Ordering::Relaxed),
|
||||
tx = stats.tx.load(Ordering::Relaxed),
|
||||
tx_retry = stats.tx_retry.load(Ordering::Relaxed),
|
||||
"slcan: no feedback for >1s (wiring / bus-off / powered ESC?)"
|
||||
);
|
||||
}
|
||||
}
|
||||
break; // 缓冲已空，回到发送方向
|
||||
}
|
||||
Err(e) if e.kind() == io::ErrorKind::InvalidData => {
|
||||
// 单帧解析失败（噪声 / 半帧）不该杀掉整条链路。
|
||||
debug!(error = %e, "slcan: malformed frame dropped");
|
||||
}
|
||||
Err(e) => {
|
||||
error!(error = %e, "slcan read failed, bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn slcan_frame(id: u32, dlc: usize) -> slcan::CanFrame {
|
||||
slcan::CanFrame::new(id, dlc, &[0xAA; 8])
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepts_standard_8_byte_frames() {
|
||||
let frame = slcan::CanFrame::new(0x201, 8, &[1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
let link = accepted_rx(&frame).expect("0x201/8B must be accepted");
|
||||
assert_eq!(link.id, 0x201);
|
||||
assert_eq!(link.data, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_extended_id() {
|
||||
assert!(accepted_rx(&slcan_frame(0x800, 8)).is_none(), "11 位 ID 上界之外必须丢弃");
|
||||
assert!(accepted_rx(&slcan_frame(0x1FFF_FFFF, 8)).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_short_frames() {
|
||||
for dlc in 0..8 {
|
||||
assert!(accepted_rx(&slcan_frame(0x201, dlc)).is_none(), "dlc {dlc} 必须丢弃");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn max_standard_id_is_boundary() {
|
||||
assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID, 8)).is_some());
|
||||
assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID + 1, 8)).is_none());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn speed_maps_to_lawicel_setup_command() {
|
||||
assert_eq!(CanSpeed::M1.to_slcan() as u8, b'8');
|
||||
assert_eq!(CanSpeed::K500.to_slcan() as u8, b'6');
|
||||
assert_eq!(CanSpeed::default(), CanSpeed::M1);
|
||||
}
|
||||
}
|
||||
+115
-9
@@ -14,7 +14,7 @@ use std::time::Duration;
|
||||
|
||||
use motor_core::{
|
||||
Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController,
|
||||
PositionDrive, VelocityController, VelocityDrive,
|
||||
PositionDrive, VelocityController, VelocityDrive, VelocityGains,
|
||||
};
|
||||
use thiserror::Error;
|
||||
use tokio::sync::{mpsc, watch};
|
||||
@@ -44,6 +44,9 @@ pub struct EscConfig {
|
||||
/// 电调型号。
|
||||
pub model: EscModel,
|
||||
/// 减速比（电机转子 : 输出轴）。
|
||||
///
|
||||
/// 驱动层**不使用**此字段；由上层 [`motor_core::OutputShaft`] 应用。
|
||||
/// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。
|
||||
pub gear_ratio: f32,
|
||||
/// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
|
||||
/// 上层永远以"输出轴正转 = 前进方向"工作。
|
||||
@@ -99,6 +102,12 @@ impl EscConfig {
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self) -> VelocityGains {
|
||||
VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
|
||||
}
|
||||
|
||||
fn validate(&self) -> Result<(), String> {
|
||||
if command_frame_id(self.esc_id).is_none() {
|
||||
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
|
||||
@@ -130,11 +139,25 @@ pub struct BusOptions {
|
||||
pub period: Duration,
|
||||
/// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。
|
||||
pub feedback_timeout: Duration,
|
||||
/// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。
|
||||
///
|
||||
/// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` /
|
||||
/// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
|
||||
/// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可），
|
||||
/// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
|
||||
///
|
||||
/// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。
|
||||
pub command_timeout: Duration,
|
||||
}
|
||||
|
||||
impl Default for BusOptions {
|
||||
fn default() -> Self {
|
||||
Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) }
|
||||
Self {
|
||||
period: Duration::from_millis(1),
|
||||
feedback_timeout: Duration::from_millis(100),
|
||||
// 1kHz 下约 50 拍未刷新即失效。
|
||||
command_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -162,6 +185,8 @@ struct MotorShared {
|
||||
cfg: EscConfig,
|
||||
state: Mutex<MotorState>,
|
||||
cmd: Mutex<Command>,
|
||||
/// 命令写入时刻（看门狗用）。
|
||||
cmd_at: Mutex<Option<Instant>>,
|
||||
last_mode: Mutex<CommandMode>,
|
||||
vel_ctrl: Mutex<VelocityController>,
|
||||
pos_ctrl: Mutex<PositionController>,
|
||||
@@ -181,6 +206,7 @@ impl MotorShared {
|
||||
cfg,
|
||||
state: Mutex::new(MotorState::default()),
|
||||
cmd: Mutex::new(Command::Disabled),
|
||||
cmd_at: Mutex::new(None),
|
||||
last_mode: Mutex::new(CommandMode::Disabled),
|
||||
turn: Mutex::new(TurnTracker::new()),
|
||||
last_feedback: Mutex::new(None),
|
||||
@@ -188,6 +214,26 @@ impl MotorShared {
|
||||
}
|
||||
}
|
||||
|
||||
fn store_command(&self, cmd: Command) {
|
||||
*lock(&self.cmd) = cmd;
|
||||
*lock(&self.cmd_at) = Some(Instant::now());
|
||||
}
|
||||
|
||||
/// 当前速度环增益（来自控制器，而非启动时的 `cfg`）。
|
||||
fn velocity_gains(&self) -> VelocityGains {
|
||||
lock(&self.vel_ctrl).gains()
|
||||
}
|
||||
|
||||
/// 热更新速度环增益：重建控制器（等价于 reset，避免积分冲击）。
|
||||
///
|
||||
/// 只替换增益，不动限幅 / 型号 / 方向 —— 那些是安全与机械约束，
|
||||
/// 不属于可在线试错的整定量。
|
||||
fn set_velocity_gains(&self, gains: VelocityGains) {
|
||||
let limit = self.cfg.current_limit;
|
||||
let mut controller = lock(&self.vel_ctrl);
|
||||
*controller = VelocityController::from_gains(limit, gains);
|
||||
}
|
||||
|
||||
/// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
|
||||
fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
|
||||
let esc_rev = lock(&self.turn).update(fb.angle_raw);
|
||||
@@ -212,7 +258,13 @@ impl MotorShared {
|
||||
}
|
||||
|
||||
/// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。
|
||||
fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 {
|
||||
fn compute_effort(
|
||||
&self,
|
||||
dt: f32,
|
||||
feedback_timeout: Duration,
|
||||
command_timeout: Duration,
|
||||
now: Instant,
|
||||
) -> i16 {
|
||||
let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout);
|
||||
if !online {
|
||||
if !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
|
||||
@@ -223,6 +275,14 @@ impl MotorShared {
|
||||
*lock(&self.last_mode) = CommandMode::Disabled;
|
||||
return 0;
|
||||
}
|
||||
// 命令看门狗：上层任务死亡/卡死后旧指令不得持续输出。
|
||||
let cmd_fresh =
|
||||
matches!(*lock(&self.cmd_at), Some(t) if now.duration_since(t) <= command_timeout);
|
||||
if !cmd_fresh {
|
||||
self.reset_controllers();
|
||||
*lock(&self.last_mode) = CommandMode::Disabled;
|
||||
return 0;
|
||||
}
|
||||
let cmd = *lock(&self.cmd);
|
||||
// 模式切换复位：避免积分/微分记忆冲击。
|
||||
{
|
||||
@@ -257,6 +317,13 @@ impl MotorShared {
|
||||
}
|
||||
|
||||
/// 电机的公共句柄：可克隆，跨任务共享。
|
||||
///
|
||||
/// # 命令租约
|
||||
///
|
||||
/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
|
||||
/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。
|
||||
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出），
|
||||
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
|
||||
#[derive(Clone)]
|
||||
pub struct EscMotor {
|
||||
shared: Arc<MotorShared>,
|
||||
@@ -286,6 +353,25 @@ impl EscMotor {
|
||||
pub fn last_feedback(&self) -> Option<Instant> {
|
||||
*lock(&self.shared.last_feedback)
|
||||
}
|
||||
|
||||
/// 当前速度环增益。
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self) -> VelocityGains {
|
||||
self.shared.velocity_gains()
|
||||
}
|
||||
|
||||
/// 热更新速度环增益（**不需重启总线**，下一发送节拍生效）。
|
||||
///
|
||||
/// 用于自动整定：一次真机试验只改增益，不必反复开关串口 / CAN 接口。
|
||||
/// 控制器被重建，历史积分被丢弃——这正是一次新试验想要的起点。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// 增益含 NaN / inf 时 panic（同 [`Pid::new`]）；调用方应先检查
|
||||
/// [`VelocityGains::is_finite`]。
|
||||
pub fn set_velocity_gains(&self, gains: VelocityGains) {
|
||||
self.shared.set_velocity_gains(gains);
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for EscMotor {
|
||||
@@ -296,29 +382,38 @@ impl MotorFeedback for EscMotor {
|
||||
fn state(&self) -> MotorState {
|
||||
*lock(&self.shared.state)
|
||||
}
|
||||
|
||||
fn feedback_age(&self) -> Option<std::time::Duration> {
|
||||
let t = *lock(&self.shared.last_feedback);
|
||||
Some(Instant::now().saturating_duration_since(t?))
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for EscMotor {
|
||||
/// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
*lock(&self.shared.cmd) = Command::Velocity(rotor_rpm);
|
||||
self.shared.store_command(Command::Velocity(rotor_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl EffortDrive for EscMotor {
|
||||
/// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
*lock(&self.shared.cmd) = Command::EffortRaw(raw);
|
||||
self.shared.store_command(Command::EffortRaw(raw));
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionDrive for EscMotor {
|
||||
/// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
*lock(&self.shared.cmd) = Command::Position(rotor_rev);
|
||||
self.shared.store_command(Command::Position(rotor_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl Disable for EscMotor {
|
||||
/// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。
|
||||
fn disable(&self) {
|
||||
*lock(&self.shared.cmd) = Command::Disabled;
|
||||
self.shared.store_command(Command::Disabled);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -423,14 +518,20 @@ async fn tx_loop(
|
||||
options: BusOptions,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
let dt = options.period.as_secs_f32();
|
||||
let nominal_dt = options.period.as_secs_f32();
|
||||
let mut ticker = tick_every(options.period);
|
||||
// 实测 dt：丢拍/负载抖动时 PID 不得按名义 1ms 硬算（F-1）。
|
||||
let mut last_tick = Instant::now();
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = shutdown.changed() => break,
|
||||
_ = ticker.tick() => {}
|
||||
}
|
||||
let now = Instant::now();
|
||||
let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32();
|
||||
last_tick = now;
|
||||
// 夹在 [0.5, 4] × 名义周期，避免首拍/长卡死把积分或微分打飞。
|
||||
let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
|
||||
for (frame_id, group_start) in
|
||||
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
|
||||
{
|
||||
@@ -444,7 +545,12 @@ async fn tx_loop(
|
||||
let mut currents = [0i16; 4];
|
||||
for motor in &group {
|
||||
let slot = usize::from(motor.id() - group_start);
|
||||
currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now);
|
||||
currents[slot] = motor.shared.compute_effort(
|
||||
dt,
|
||||
options.feedback_timeout,
|
||||
options.command_timeout,
|
||||
now,
|
||||
);
|
||||
}
|
||||
if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
|
||||
info!("can link closed, tx loop exiting");
|
||||
|
||||
@@ -5,17 +5,18 @@
|
||||
//! - [`codec`]：纯函数帧编解码（C620 手册协议）；
|
||||
//! - [`turn`]：单圈编码器 → 连续多圈角；
|
||||
//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）；
|
||||
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全；
|
||||
//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。
|
||||
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全。
|
||||
//!
|
||||
//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction`
|
||||
//! 完成方向修正。
|
||||
//!
|
||||
//! 被控对象仿真（假电调）**不在本 crate**：见 `dji-esc-sim`。本 crate 只描述
|
||||
//! 真实电调在总线上的行为，不假设对面是谁。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod bus;
|
||||
pub mod codec;
|
||||
pub mod frame;
|
||||
pub mod sim;
|
||||
pub mod turn;
|
||||
|
||||
pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor};
|
||||
@@ -25,5 +26,4 @@ pub use codec::{
|
||||
decode_feedback, encode_current_frame, encode_feedback_frame,
|
||||
};
|
||||
pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE};
|
||||
pub use sim::{PlantMotor, spawn_plant};
|
||||
pub use turn::{ENCODER_TICKS, TurnTracker};
|
||||
|
||||
@@ -1,116 +0,0 @@
|
||||
//! 电机被控对象仿真：一阶转速模型 + 编码器积分。
|
||||
//!
|
||||
//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环
|
||||
//! （测试、`chassisd --mock` 均复用）。
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::time::Duration;
|
||||
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
use tracing::trace;
|
||||
|
||||
use crate::codec::{EscFeedback, encode_feedback_frame};
|
||||
use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
|
||||
|
||||
/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。
|
||||
///
|
||||
/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数
|
||||
/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PlantMotor {
|
||||
gain_rpm_per_raw: f32,
|
||||
tau: f32,
|
||||
max_rpm: f32,
|
||||
speed_rpm: f32,
|
||||
angle_raw: f32,
|
||||
}
|
||||
|
||||
impl PlantMotor {
|
||||
/// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。
|
||||
#[must_use]
|
||||
pub fn gm3508() -> Self {
|
||||
Self {
|
||||
gain_rpm_per_raw: 9254.0 / 16384.0,
|
||||
tau: 0.12,
|
||||
max_rpm: 9254.0,
|
||||
speed_rpm: 0.0,
|
||||
angle_raw: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 当前转子转速（rpm，电调侧）。
|
||||
#[must_use]
|
||||
pub fn speed_rpm(&self) -> f32 {
|
||||
self.speed_rpm
|
||||
}
|
||||
|
||||
/// 推进一个时间步，返回本步反馈帧内容。
|
||||
pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
|
||||
let target =
|
||||
(f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm);
|
||||
let alpha = 1.0 - (-dt / self.tau).exp();
|
||||
self.speed_rpm += (target - self.speed_rpm) * alpha;
|
||||
self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0);
|
||||
EscFeedback {
|
||||
angle_raw: self.angle_raw as u16,
|
||||
speed_rpm: self.speed_rpm as i16,
|
||||
current_raw,
|
||||
temperature_c: 30,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。
|
||||
///
|
||||
/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。
|
||||
pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
|
||||
let mut plants: HashMap<u8, PlantMotor> =
|
||||
esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect();
|
||||
let mut commands: HashMap<u8, i16> = esc_ids.iter().map(|&id| (id, 0)).collect();
|
||||
let dt = tick.as_secs_f32();
|
||||
let mut ticker = interval(tick);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
tokio::spawn(async move {
|
||||
loop {
|
||||
tokio::select! {
|
||||
frame = link.incoming_rx.recv() => match frame {
|
||||
Some(frame) => apply_command_frame(&frame, &mut commands),
|
||||
None => break,
|
||||
},
|
||||
_ = ticker.tick() => {
|
||||
for (&id, plant) in &mut plants {
|
||||
let cmd = commands.get(&id).copied().unwrap_or(0);
|
||||
let feedback = plant.step(cmd, dt);
|
||||
if link
|
||||
.outgoing_tx
|
||||
.send(encode_feedback_frame(id, &feedback))
|
||||
.await
|
||||
.is_err()
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
trace!("plant exiting");
|
||||
})
|
||||
}
|
||||
|
||||
fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap<u8, i16>) {
|
||||
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],
|
||||
]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3,6 +3,8 @@
|
||||
//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
|
||||
//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
|
||||
|
||||
/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
|
||||
@@ -66,6 +68,10 @@ impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
|
||||
// 只透传转子量；不伪造输出轴字段。
|
||||
self.inner.state()
|
||||
}
|
||||
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
self.inner.feedback_age()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
@@ -116,6 +122,9 @@ mod tests {
|
||||
fn state(&self) -> MotorState {
|
||||
MotorState::default()
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
Some(Duration::ZERO)
|
||||
}
|
||||
}
|
||||
impl VelocityDrive for Stub {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
|
||||
@@ -41,7 +41,7 @@ pub use position::{PositionController, PositionLoop};
|
||||
pub use state::MotorState;
|
||||
pub use testing::{MockHeading, MockMotor};
|
||||
pub use traits::{Disable, EffortDrive, Motor, MotorFeedback, PositionDrive, VelocityDrive};
|
||||
pub use velocity::VelocityController;
|
||||
pub use velocity::{VelocityController, VelocityGains};
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
use std::future::Future;
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use crate::{
|
||||
Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
|
||||
@@ -20,18 +21,30 @@ fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
pub struct MockMotor {
|
||||
id: u8,
|
||||
published: Arc<Mutex<(MotorState, Command)>>,
|
||||
last_feedback: Arc<Mutex<Option<Instant>>>,
|
||||
}
|
||||
|
||||
impl MockMotor {
|
||||
/// 创建指定 ID 的空电机。
|
||||
#[must_use]
|
||||
pub fn new(id: u8) -> Self {
|
||||
Self { id, published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))) }
|
||||
Self {
|
||||
id,
|
||||
published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))),
|
||||
last_feedback: Arc::new(Mutex::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// 注入反馈快照。
|
||||
/// 注入反馈快照（记为「刚刚收到」）。
|
||||
pub fn inject_state(&self, state: MotorState) {
|
||||
self.inject_state_with_age(state, Duration::ZERO);
|
||||
}
|
||||
|
||||
/// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。
|
||||
pub fn inject_state_with_age(&self, state: MotorState, age: Duration) {
|
||||
lock(&self.published).0 = state;
|
||||
let at = Instant::now().checked_sub(age);
|
||||
*lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now));
|
||||
}
|
||||
|
||||
/// 最近一次命令。
|
||||
@@ -48,6 +61,10 @@ impl MotorFeedback for MockMotor {
|
||||
fn state(&self) -> MotorState {
|
||||
lock(&self.published).0
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
let at = *lock(&self.last_feedback);
|
||||
Some(at?.elapsed())
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for MockMotor {
|
||||
@@ -110,7 +127,19 @@ mod tests {
|
||||
fn mock_motor_injects_state() {
|
||||
let m = MockMotor::new(1);
|
||||
assert!(!m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_none());
|
||||
m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
|
||||
assert!(m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_feedback_age_can_be_stale() {
|
||||
let m = MockMotor::new(1);
|
||||
m.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(250),
|
||||
);
|
||||
assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200)));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,6 +6,8 @@
|
||||
//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
|
||||
//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::MotorState;
|
||||
|
||||
/// 读反馈。所有可观察设备的最小面。
|
||||
@@ -15,6 +17,12 @@ pub trait MotorFeedback: Send + Sync {
|
||||
|
||||
/// 最新反馈快照。
|
||||
fn state(&self) -> MotorState;
|
||||
|
||||
/// 距最近一帧反馈的时长；`None` 表示从未收到。
|
||||
///
|
||||
/// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`]
|
||||
/// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。
|
||||
fn feedback_age(&self) -> Option<Duration>;
|
||||
}
|
||||
|
||||
/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。
|
||||
|
||||
@@ -2,6 +2,43 @@
|
||||
|
||||
use crate::pid::Pid;
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
///
|
||||
/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
|
||||
/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
|
||||
/// 传递、落盘与热更新，而无需重建整个速度环。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct VelocityGains {
|
||||
/// 比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 微分增益（raw / (rpm/s)）。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityGains {
|
||||
/// 全零增益。
|
||||
pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
|
||||
|
||||
/// 由四个分量构造。
|
||||
#[must_use]
|
||||
pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
|
||||
Self { kp, ki, kd, feedforward }
|
||||
}
|
||||
|
||||
/// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.kp.is_finite()
|
||||
&& self.ki.is_finite()
|
||||
&& self.kd.is_finite()
|
||||
&& self.feedforward.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
|
||||
///
|
||||
/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
|
||||
@@ -23,6 +60,29 @@ impl VelocityController {
|
||||
Self { pid, feedforward }
|
||||
}
|
||||
|
||||
/// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
|
||||
#[must_use]
|
||||
pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
|
||||
let limit = f32::from(output_limit_raw);
|
||||
let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
|
||||
Self { pid, feedforward: gains.feedforward }
|
||||
}
|
||||
|
||||
/// 当前增益组（含前馈），用于诊断与热更新前的读取。
|
||||
#[must_use]
|
||||
pub fn gains(&self) -> VelocityGains {
|
||||
VelocityGains {
|
||||
kp: self.pid.kp(),
|
||||
ki: self.pid.ki(),
|
||||
kd: self.pid.kd(),
|
||||
feedforward: self.feedforward,
|
||||
}
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
|
||||
pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
|
||||
let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
|
||||
@@ -84,4 +144,37 @@ mod tests {
|
||||
let out = vc.update(100.0, 0.0, 0.001);
|
||||
assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_round_trips_through_gains() {
|
||||
let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
|
||||
let mut from_data = VelocityController::from_gains(12_000, gains);
|
||||
let mut by_hand =
|
||||
VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
|
||||
|
||||
assert_eq!(from_data.gains(), gains);
|
||||
// 两条构造路径必须逐位一致。
|
||||
for step in 0..16 {
|
||||
let target = 1000.0 + step as f32;
|
||||
let measured = 900.0 + step as f32 * 3.0;
|
||||
assert_eq!(
|
||||
from_data.update(target, measured, 0.001).to_bits(),
|
||||
by_hand.update(target, measured, 0.001).to_bits()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_clamps_to_output_limit() {
|
||||
let mut vc = VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
|
||||
assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gains_finiteness_is_checkable() {
|
||||
assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
|
||||
assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
|
||||
}
|
||||
}
|
||||
|
||||
+109
-24
@@ -3,33 +3,102 @@
|
||||
## 分层
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────┐
|
||||
│ chassisd（守护进程） │
|
||||
│ 配置 / CLI / 1kHz 控制循环 / 安全逻辑 / 日志 │
|
||||
└──────┬───────────────────────────┬──────────┘
|
||||
│ mecanum（麦轮正逆解） │
|
||||
▼ ▼
|
||||
┌────────────┐ ┌──────────────────┐
|
||||
│ motor-core │◄─实现 trait──│ dji-esc │
|
||||
│ Motor trait│ │ C620/C610 协议 │
|
||||
│ MotorState │ │ EscBus 异步驱动 │
|
||||
│ Pid │ │ TurnTracker 多圈 │
|
||||
└────────────┘ └────────┬─────────┘
|
||||
│ CanLink（tokio mpsc 通道对）
|
||||
▼
|
||||
┌──────────────────────────┐
|
||||
│ dji-esc-socketcan(Linux) │
|
||||
│ socketcan ↔ CanLink 桥接 │
|
||||
└──────────────────────────┘
|
||||
┌────────────────┐ ┌──────────────────────┐ ┌─────────────────────────┐
|
||||
│ chassisd │ │ chassis-tune │ │ chassis-bench │
|
||||
│ 守护进程 │ │ 自动整定（搜索/报告） │ │ 台架（激励/包络/采样） │
|
||||
│ 1kHz 循环 / 安全│ │ sim ↓ bench ↓ │ │ chassis-spin 等 │
|
||||
└───┬────────┬───┘ └────┬──────────┬──────┘ └────┬───────────────────┘
|
||||
│ │ │ │ │
|
||||
│ │ │ └───────┬───────┘
|
||||
▼ ▼ ▼ ▼
|
||||
┌───────────┐ ┌──────────────────┐ ┌───────────────────────┐
|
||||
│ mecanum │ │ dji-esc-sim │ │ dji-esc-slcan │
|
||||
│ 麦轮运动学 │ │ 被控对象 / 场景 │ │ dji-esc-socketcan │
|
||||
└───────────┘ │ 离线 harness │ │ 真实总线 ↔ CanLink │
|
||||
│ 异步假电调 │ └──────────┬────────────┘
|
||||
└────────┬─────────┘ │
|
||||
│ CanLink（tokio mpsc 通道对）
|
||||
▼ ▼
|
||||
┌────────────────────┐ ┌──────────────────┐
|
||||
│ motor-core │◄─│ dji-esc │
|
||||
│ 能力 trait / Pid │ │ C620/C610 协议 │
|
||||
│ VelocityGains │ │ EscBus 异步驱动 │
|
||||
└────────────────────┘ └──────────────────┘
|
||||
|
||||
┌────────────────────┐
|
||||
│ chassis-config │ chassis.toml schema（chassisd + chassis-tune 共用）
|
||||
└────────────────────┘
|
||||
```
|
||||
|
||||
核心解耦点：`CanLink` 是一对 tokio 通道（发送 / 接收 `CanFrame`）。`dji-esc` 的协议状态机
|
||||
只面向通道工作，因此：
|
||||
|
||||
- 测试中用内存通道 + 一阶电机模型（`dji_esc::sim`）构成闭环仿真，全平台可跑；
|
||||
- `dji-esc-socketcan` 把真实 SocketCAN 接口桥接到通道，只在实车上出现；
|
||||
- 闭环仿真由 `dji-esc-sim` 提供两条路径：**异步假电调**（挂在同一对通道上，`chassisd --mock`
|
||||
与集成测试用）与**同步 harness**（纯浮点积分、无任务无通道，自动整定用，可并行上千次）；
|
||||
- `dji-esc-socketcan`（板载原生 SocketCAN）与 `dji-esc-slcan`（CANable 等 USB 适配器，
|
||||
串口 Lawicel 协议）只把真实总线桥接到通道，**不含任何应用逻辑**；应用住在 `chassis-bench`
|
||||
（台架）与 `chassis-tune`（整定）；
|
||||
- 未来更换总线介质（串口转 CAN、MCU 转发）只需新增一个桥接 crate。
|
||||
|
||||
## 被控对象模型（dji-esc-sim）
|
||||
|
||||
逐拍离散（`dt` 秒），电调坐标系（正电流 → 正转速）：
|
||||
|
||||
1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才作用（链路 + 采样延迟）；
|
||||
2. **摩擦**：运动时 `net = u − sign(rpm)·coulomb − viscous·rpm`；静止且 `|u| > static` 时
|
||||
`net = u − sign(u)·coulomb`；静止且 `|u| ≤ static` 时 `net = 0`（粘住）；
|
||||
3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
|
||||
以时间常数 `tau` 一阶趋近；
|
||||
4. **量化**：机械角按转速积分并对 8192 取模，反馈帧是 `EscFeedback`，
|
||||
因此角度被截断为 `u16`、转速为 `i16`——量化由协议类型自带。
|
||||
|
||||
`PlantParams::gm3508()` 是**无摩擦无延迟**的理想模型（`chassisd --mock` 与历史闭环测试依赖它，
|
||||
行为逐位不变）；`PlantParams::gm3508_bench()` 叠加静 / 库仑摩擦（400~600 raw 量级，
|
||||
取自台架实测）与 2 拍延迟，是整定的默认被控对象。
|
||||
|
||||
单点模型整定出来的增益往往对增益 / 时间常数 / 延迟高度敏感，因此 `chassis-tune` 默认在
|
||||
`PlantEnsemble`（域随机化：增益 ±20%、时间常数 ±30%、延迟 0~3 拍、摩擦 ±50%）上做鲁棒优化，
|
||||
而不是针对一条理想曲线做最优化。
|
||||
|
||||
## 自动整定（chassis-tune）
|
||||
|
||||
两阶段搜索：
|
||||
|
||||
1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数铺满四维空间；
|
||||
2. **精修**：以成本最低的 top-k 为起点，在**坐标空间**（对数刻度，增益恒正）做多起点
|
||||
`argmin` 局部搜索（Nelder-Mead 默认，可选 PSO）。
|
||||
|
||||
成本 = 归一化加权（调节时间 / 超调 / IAE / 电流）；**硬约束不进权重**：过速、饱和占比过高、
|
||||
不收敛直接给罚值并记录判定——否则搜索会主动挑一个会飞车的解。
|
||||
|
||||
可复现性：每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，粗扫结果排序后再取 top-k，
|
||||
因此同 seed 两次运行**逐位一致**（CI 的 `just tune-smoke` 依赖这一点）。
|
||||
|
||||
超参数（参数空间 / 场景 / 权重 / 验收阈值 / 搜索预算 / 被控对象 / 真机安全包络）不在命令行
|
||||
默认值里散着，而是版本化在 `config/tune.toml` 的具名预设中（见
|
||||
[`preset.rs`](../crates/chassis-tune/src/preset.rs)）。优先级为
|
||||
**内置默认 < 预设文件 < 显式 CLI**，逐字段覆盖；`current_limit` / `direction` / `gear_ratio`
|
||||
不是预设字段，写进预设文件会直接报错（安全参数不允许被预设绕过）。
|
||||
|
||||
真机后端（`--backend bench`）与仿真后端实现同一个 `Evaluator` trait，区别只是"评估一组增益"
|
||||
从浮点积分换成真机试验：打开链路 → 上电门控（反馈新鲜）→ 逐次热更新增益
|
||||
（`EscMotor::set_velocity_gains`，不重启总线）→ 逐拍重发设定值 + 安全包络 → 任何退出路径
|
||||
先 0 电流 → 试验间静默。搜索强制单线程，因为只有一条总线。
|
||||
|
||||
## 命令租约与链路深度（实车踩过的坑）
|
||||
|
||||
1. **命令是租约**：`EscBus` 在 `command_timeout`（默认 50ms）内没收到新命令就强制 0 输出
|
||||
（防上层任务死亡后旧指令持续输出）。所以 `set_velocity` 不是"设一次就完事"，上层必须
|
||||
逐拍重发；只在开头下发一次的台架脚本会在 50ms 后自动失电。
|
||||
2. **命令队列必须浅**：驱动按 1kHz 生产命令，而 slcan 一次 write 在 CDC ACM 上约 2ms，
|
||||
下行只有几百帧/秒。若 `CanLink` 命令方向缓冲很深（早期是 512），队列会被过时命令灌满，
|
||||
ESC 执行的是**一秒多以前**算出的电流——速度环等于开了 1.4s 延迟，轮子必然跑飞。
|
||||
现在 slcan 桥的 `CHANNEL_BOUND = 8`：`send().await` 立即回压，每一帧都按最新反馈现算。
|
||||
反馈方向是采样流，来不及就丢（`try_send`），绝不阻塞泵线程。
|
||||
3. **桥的两个方向不能互相饿死**：发送要有上界（`TX_BURST`）并在一帧写不动时立刻让位，
|
||||
接收要批量排空（`RX_BURST`）。`serial` crate 的 `TTYPort` 读写共用同一个超时，
|
||||
写超时属正常现象，不能当致命错误（否则一次写超时就会把接收方向一起带停）。
|
||||
|
||||
## DJI 电调 CAN 协议要点（C620 手册）
|
||||
|
||||
| 项目 | 值 |
|
||||
@@ -65,13 +134,29 @@ C620 内部**只闭电流环**：转速 / 位置环由主控侧 PID 实现（`mo
|
||||
|
||||
| 层级 | 内容 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试 | 全平台 |
|
||||
| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、守护进程全链路 | 全平台 |
|
||||
| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试、slcan 帧形状过滤 | 全平台 |
|
||||
| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、**命令租约到期与续租**、**增益热更新**、守护进程全链路 | 全平台 |
|
||||
| 模型一致性 | 同步 harness 与 `EscBus` 异步闭环在同一组增益下的稳态 / effort 对齐（防止"对着错模型调参"） | 全平台 |
|
||||
| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） |
|
||||
| 超参数预设 | schema 往返、逐字段覆盖与优先级、未知键 / 越界 / 缺失报错、`config/tune.toml` 全部预设可解算、命名与后端一致、安全参数不可注入 | 全平台 |
|
||||
| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） |
|
||||
| 实车台架 | `chassis-spin`（`chassis-bench`）四轮定速旋转：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 目标板 + slcan/SocketCAN（人工） |
|
||||
| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check | CI |
|
||||
|
||||
## 部署
|
||||
|
||||
`chassisd` 为单二进制守护进程：`--interface can0 --config /etc/chassisd/chassis.toml`。
|
||||
systemd 样例见 `deploy/chassisd.service`。板端接口就绪依赖：
|
||||
USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`。
|
||||
systemd 样例见 `deploy/chassisd.service`。板端 CAN 两条路径：
|
||||
|
||||
- **SocketCAN**：USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`；
|
||||
- **slcan**：USB 适配器（CANable2 等）在 CDC ACM 下枚举为 `/dev/ttyACM0`，由
|
||||
[`dji-esc-slcan`](../crates/dji-esc-slcan) 在用户态处理（不需 `ip link`，也绕开了
|
||||
gs_usb 驱动）。台架工具 `chassis-spin`（[`chassis-bench`](../crates/chassis-bench)）与
|
||||
自动整定的真机后端走这条路径。整定工具本身是宿主机 / 板端通用的纯 Rust 程序，
|
||||
`current_limit` 等安全参数由 CLI 显式给出，不参与搜索。
|
||||
|
||||
## 相关文档
|
||||
|
||||
- [底盘开发文档](chassis.md)——接线 / 标定 / 上车流程 / 自动整定与超参数预设 / 故障排查
|
||||
- [整体架构规划](../ARCHITECTURE.md)——全系统分层与阶段计划
|
||||
|
||||
|
||||
+748
@@ -0,0 +1,748 @@
|
||||
# 底盘开发文档
|
||||
|
||||
**适用对象**：哨兵机器人麦轮底盘（4 × GM3508 + C620 电调）
|
||||
**目标平台**：Allwinner T153（Cortex-A7，Linux，armv7）+ CANable2（slcan）或板载 SocketCAN
|
||||
**控制频率**：1 kHz（2ms 是旧方案；本项目实际为 1ms 节拍）
|
||||
**文档目的**：从零到上车的完整开发路径——接口约定、标定步骤、自动整定与超参数、安全与故障处理
|
||||
|
||||
---
|
||||
|
||||
## 目录
|
||||
|
||||
1. [范围与文档分工](#1-范围与文档分工)
|
||||
2. [快速上手](#2-快速上手)
|
||||
3. [硬件与接线](#3-硬件与接线)
|
||||
4. [坐标、单位与符号约定](#4-坐标单位与符号约定)
|
||||
5. [软件架构](#5-软件架构)
|
||||
6. [关键机制（实车踩过的坑）](#6-关键机制实车踩过的坑)
|
||||
7. [控制设计](#7-控制设计)
|
||||
8. [配置参考（chassis.toml）](#8-配置参考chassistoml)
|
||||
9. [标定与上车流程](#9-标定与上车流程)
|
||||
10. [自动整定与超参数预设](#10-自动整定与超参数预设)
|
||||
11. [台架工具](#11-台架工具)
|
||||
12. [安全设计](#12-安全设计)
|
||||
13. [测试与质量门禁](#13-测试与质量门禁)
|
||||
14. [部署](#14-部署)
|
||||
15. [故障排查](#15-故障排查)
|
||||
16. [术语表](#16-术语表)
|
||||
|
||||
---
|
||||
|
||||
## 1. 范围与文档分工
|
||||
|
||||
| 文档 | 回答的问题 | 读者 |
|
||||
| --- | --- | --- |
|
||||
| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 整套 RoboMaster CAN 控制系统的**全系统分层规划**（含云台、发射机构、delta10 IMU、T153 实时性、9 电机） | 架构决策者 |
|
||||
| [`docs/architecture.md`](architecture.md) | 本仓库**已实现**部分的解耦点：`CanLink` seam、被控对象模型、自动整定两阶段搜索、测试策略 | 改代码的人 |
|
||||
| **本文** | **底盘这一块**怎么开发、怎么标定、怎么整定、怎么上车、出事怎么办 | 底盘开发 / 调试 / 上车的人 |
|
||||
|
||||
`ARCHITECTURE.md` 里的 `rm-*` crate 划分是**规划**；本仓库实际落地为下面这套 crate 命名
|
||||
（功能对应关系见第 5 节）。本文只覆盖底盘切片，云台 / 发射 / IMU 不在范围内。
|
||||
|
||||
---
|
||||
|
||||
## 2. 快速上手
|
||||
|
||||
### 2.1 无硬件（macOS / Linux / CI 均可）
|
||||
|
||||
```sh
|
||||
cargo test --workspace # 单测 + 闭环仿真 + 整定，全部无需硬件
|
||||
cargo run -p chassisd -- --mock # 进程内电机模型的全链路演示
|
||||
cargo run -p chassis-tune -- presets # 看有哪些整定超参数预设
|
||||
just tune-smoke # 确定性整定冒烟（CI 同款）
|
||||
just lint # fmt + clippy（与 CI 同标准）
|
||||
```
|
||||
|
||||
### 2.2 上车（详细步骤见第 9 节）
|
||||
|
||||
```sh
|
||||
# 1) 只验链路与反馈，绝不发非零电流（接线排查第一步）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
|
||||
# 2) 四轮定性旋转（轮子必须架空或整车固定，急停可及）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --rpm 500
|
||||
|
||||
# 3) 先离线整定，再上台架复验（用版本化的超参数预设）
|
||||
cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name sim_bench_model
|
||||
cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name bench_noload \
|
||||
--backend bench --device /dev/ttyACM0 --i-know-the-wheel-spins
|
||||
|
||||
# 4) 把结果写回配置并跑守护进程
|
||||
cargo run --release -p chassis-tune -- sweep --config config/chassis.toml.example --motor front_left \
|
||||
--preset config/tune.toml --preset-name sim_bench_model --write-config /tmp/chassis.tuned.toml
|
||||
chassisd --config /tmp/chassis.tuned.toml --interface can0
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. 硬件与接线
|
||||
|
||||
### 3.1 拓扑
|
||||
|
||||
```
|
||||
T153 (Linux) Gm3508 × 4 + C620
|
||||
├── USB CDC /dev/ttyACM0 ──► CANable2 (slcan 固件) ──► CAN 总线 ──► 4 个电调
|
||||
└── 板载 CAN 控制器 can0 ──────────────────────────► （二选一）
|
||||
```
|
||||
|
||||
两条路径**互斥地二选一**，应用层无感知（都由 `CanLink` 抽象屏蔽）：
|
||||
|
||||
| 路径 | 打开方式 | 适用 |
|
||||
| --- | --- | --- |
|
||||
| **slcan**（`dji-esc-slcan`） | 用户态直接读写 `/dev/ttyACM0`（Lawicel ASCII 协议） | 免 `ip link`、绕开 `gs_usb` 驱动；台架与初始联调首选 |
|
||||
| **SocketCAN**（`dji-esc-socketcan`） | `ip link set can0 up type can bitrate 1000000 restart-ms 100` | 板载原生 CAN；生产部署 |
|
||||
|
||||
### 3.2 设备与协议常量（C620 手册）
|
||||
|
||||
| 项目 | 值 |
|
||||
| --- | --- |
|
||||
| 控制帧 ID | `0x200`（电调 1–4）、`0x1FF`（电调 5–8） |
|
||||
| 控制帧格式 | 8 字节 = 4 × **i16 大端**电流命令，按电调 ID 顺序 |
|
||||
| C620 电流标度 | ±16384 ↔ ±20 A |
|
||||
| C610 电流标度 | ±10000 ↔ ±10 A（M2006 组合；接硬件前需核对） |
|
||||
| 反馈帧 ID | `0x200 + 电调ID`，即 `0x201`–`0x208` |
|
||||
| 反馈帧格式 | u16 角度(0..8191) + i16 转速 rpm + i16 转矩电流 + i8 温度℃，均大端 |
|
||||
| 反馈频率 | 每电调 1 kHz |
|
||||
| 减速比 | GM3508 = 3591 / 187 ≈ **19.203**（转子 : 输出轴） |
|
||||
|
||||
C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-core::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。
|
||||
|
||||
### 3.3 电调 ID 与安装方向
|
||||
|
||||
默认约定（`config/chassis.toml.example`、`chassis-spin` 默认值一致）：
|
||||
|
||||
| 轮位 | 轮序下标 | 电调 ID | 安装方向 |
|
||||
| --- | --- | --- | --- |
|
||||
| 前左 FL | 0 | 1 | `+1` |
|
||||
| 前右 FR | 1 | 2 | `-1` |
|
||||
| 后左 RL | 2 | 3 | `+1` |
|
||||
| 后右 RR | 3 | 4 | `-1` |
|
||||
|
||||
左右镜像安装（`direction` 相反）使四轮在"底盘前进"时物理同向。`direction = -1` 的电机
|
||||
由**驱动层**在写命令与读反馈两个方向上统一翻转符号，上层永远看到"正转 = 前进方向"。
|
||||
|
||||
### 3.4 电流限幅
|
||||
|
||||
| 场景 | 建议限幅（raw） | 说明 |
|
||||
| --- | --- | --- |
|
||||
| 空载台架（`chassis-spin` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 |
|
||||
| 整车实车 | 6000 ~ 12000 | 视供电与机械而定；同时是 PID 抗饱和的钳位点 |
|
||||
|
||||
> **限流不是"越大越安全"**：它同时是 PID 输出的钳位点。设得远大于实际所需，
|
||||
> 积分会在堵转期间持续积累，一旦起动就冲过头。**必须贴近实际所需电流。**
|
||||
|
||||
### 3.5 接线前置条件（缺一不可）
|
||||
|
||||
- 电调供电与主控**共地**；CAN_H / CAN_L 不接反，两端 120Ω 终端电阻齐全。
|
||||
- 轮子**架空**或整车固定；急停开关在手可及处。
|
||||
- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`chassis-spin` 会打印每个电调的
|
||||
转速 / 温度 / 反馈年龄）。
|
||||
|
||||
---
|
||||
|
||||
## 4. 坐标、单位与符号约定
|
||||
|
||||
### 4.1 机器人坐标系（ISO）
|
||||
|
||||
- **x 前、y 左、ω 绕 z 逆时针为正**（`mecanum::Twist`）；
|
||||
- 轮序固定为 **前左 FL、前右 FR、后左 RL、后右 RR**（常量 `mecanum::FL/FR/RL/RR`，
|
||||
`chassis.toml` 的 `[[motor]]` 顺序、`--esc-ids` / `--directions` 的顺序都遵循它）；
|
||||
- 辊子 45°、俯视 X 型布置（RoboMaster 标准布局）。
|
||||
|
||||
### 4.2 运动学（`mecanum`）
|
||||
|
||||
设 `a = half_length + half_width`，四轮线速度：
|
||||
|
||||
```
|
||||
v_FL = vx - vy - a·ω
|
||||
v_FR = vx + vy + a·ω
|
||||
v_RL = vx + vy - a·ω
|
||||
v_RR = vx - vy + a·ω
|
||||
```
|
||||
|
||||
正解是逆解的左伪逆（JᵀJ 对角，可解析求平均）。实现见
|
||||
[`crates/mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` /
|
||||
`wheel_rpms` / `twist_from_wheel_rpms`。
|
||||
|
||||
### 4.3 单位与层级（最容易出错的地方）
|
||||
|
||||
| 量 | 所在层 | 单位 |
|
||||
| --- | --- | --- |
|
||||
| `Twist.vx/vy` | 应用 / 控制 | m/s |
|
||||
| `Twist.omega` | 应用 / 控制 | rad/s |
|
||||
| 轮端转速 | `OutputShaft` 呈现 | rpm（**输出轴**） |
|
||||
| 转子转速 | `EscBus` / `EscMotor` / `MotorState::rotor_speed_rpm` | rpm（**转子**） |
|
||||
| 控制量 effort | 驱动 / PID | 协议 **raw**（ESC = 电流，C620 ±16384） |
|
||||
| `kp/ki/kd/feedforward` | 速度环 | raw/rpm、raw/(rpm·s)、raw/(rpm/s)、raw/rpm |
|
||||
|
||||
- **减速比换算只发生在一个地方**：`motor-core::OutputShaft` 包装（`转子 = 输出轴 × gear_ratio`）。
|
||||
`EscBus` / `EscMotor` 只认转子，绝不碰减速比；`EscConfig::gear_ratio` 仅作配置元数据。
|
||||
- PID 的接线顺序：`ChassisController` 算出**轮端** rpm → `OutputShaft` 换算成转子 rpm →
|
||||
`EscMotor` 的速度环以转子 rpm 闭环。
|
||||
|
||||
---
|
||||
|
||||
## 5. 软件架构
|
||||
|
||||
### 5.1 Crate 分层与职责
|
||||
|
||||
**依赖只能向下**（上层可依赖下层，反之不行）：
|
||||
|
||||
```
|
||||
第 4 层 应用 / 工具 chassisd chassis-tune chassis-bench
|
||||
（含两个 bin）
|
||||
第 3 层 领域 chassis-config mecanum dji-esc-sim
|
||||
（schema / 运动学 / 被控对象 + 离线 harness）
|
||||
第 2 层 协议 / 设备 dji-esc（C620 编解码 + EscBus 驱动 + 命令租约）
|
||||
第 1 层 抽象 can-link（CanFrame + 通道对） motor-core（能力 trait + PID）
|
||||
第 0 层 传输 dji-esc-slcan（串口） / dji-esc-socketcan（内核 CAN）
|
||||
```
|
||||
|
||||
`dji-esc-slcan` / `dji-esc-socketcan` 只实现 `can-link` 的通道语义，**不含任何应用逻辑**；
|
||||
它们被第 4 层的工具与守护进程选用，而不是被协议层依赖。
|
||||
|
||||
| Crate | 职责 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| [`motor-core`](../crates/motor-core) | 能力 trait（`VelocityDrive` 等）、`MotorState`、`Pid`、`VelocityController`、`OutputShaft` | 纯逻辑，全平台 |
|
||||
| [`can-link`](../crates/can-link) | `CanFrame` + `CanLink` 通道对（**无协议知识**） | 纯逻辑，全平台 |
|
||||
| [`dji-esc`](../crates/dji-esc) | C620/C610 编解码、多圈累计、`EscBus` 异步驱动、命令租约 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-sim`](../crates/dji-esc-sim) | 被控对象模型、场景、**离线 harness**、异步假电调 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-slcan`](../crates/dji-esc-slcan) | slcan（Lawicel）串口 ↔ `CanLink` 桥接 | 全平台 |
|
||||
| [`dji-esc-socketcan`](../crates/dji-esc-socketcan) | SocketCAN ↔ `CanLink` 桥接 | Linux |
|
||||
| [`mecanum`](../crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 |
|
||||
| [`chassis-config`](../crates/chassis-config) | `chassis.toml` schema：解析 / 校验 / 写回 | 全平台 |
|
||||
| [`chassis-bench`](../crates/chassis-bench) | 台架：安全包络、采样、`chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） |
|
||||
| [`chassis-tune`](../crates/chassis-tune) | 速度环 PID 自动整定 + **超参数预设** | 全平台 |
|
||||
| [`chassisd`](../crates/chassisd) | 底盘守护进程：1 kHz 循环、安全逻辑、指令源 seam | 全平台（实车需 Linux） |
|
||||
|
||||
### 5.2 依赖方向（硬约束）
|
||||
|
||||
```
|
||||
chassisd ─┬─► chassis-config ─► motor-core, mecanum
|
||||
├─► dji-esc ──► can-link, motor-core
|
||||
├─► dji-esc-sim ─► dji-esc（协议类型）, can-link
|
||||
└─► dji-esc-socketcan ─► can-link
|
||||
|
||||
chassis-tune ─┬─► dji-esc-sim（离线 harness）
|
||||
├─► chassis-bench（安全包络 + 采样）
|
||||
├─► chassis-config（共用 schema）
|
||||
└─► dji-esc ─► can-link, motor-core
|
||||
|
||||
chassis-bench ─┬─► dji-esc ─► can-link, motor-core
|
||||
├─► dji-esc-sim（场景 / 轨迹类型）
|
||||
├─► dji-esc-slcan / dji-esc-socketcan
|
||||
└─► chassis-config
|
||||
```
|
||||
|
||||
**传输后端 crate 只做桥接，不含应用逻辑**：`chassis-spin` 曾住在 `dji-esc-slcan` 的
|
||||
example 里，现在统一收归 `chassis-bench`；整定逻辑住在 `chassis-tune`。要换总线介质
|
||||
（串口转 CAN、MCU 转发），只需**新增一个桥接 crate**，协议 / 控制 / 整定全部不动。
|
||||
|
||||
### 5.3 一个 1 ms 拍里发生了什么
|
||||
|
||||
```
|
||||
tick (1ms)
|
||||
├─ 读取 watch<Twist>（有新值则更新，记录时间戳）
|
||||
├─ 指令超时判定（默认 200ms）→ 超时则 Twist::ZERO
|
||||
├─ 幅值限幅 Twist::clamped(max_vx, max_vy, max_omega)（按比例收缩，保方向）
|
||||
├─ 上电门控：四轮是否都收到过反馈？否则全部 disable 并告警
|
||||
├─ 麦轮逆解 → 四轮轮端 rpm
|
||||
├─ OutputShaft 换算 → 转子 rpm
|
||||
└─ set_velocity（续租命令）→ EscBus 在自身 1 kHz 节拍上组帧、发送
|
||||
```
|
||||
|
||||
同时 `EscBus` 侧持续：接收反馈帧 → 分发到各 `EscMotor` → 更新 `MotorState`
|
||||
→ 检查反馈离线（默认 100 ms）→ 检查命令租约（默认 50 ms）。
|
||||
|
||||
---
|
||||
|
||||
## 6. 关键机制（实车踩过的坑）
|
||||
|
||||
### 6.1 命令是**租约**，不是设定值
|
||||
|
||||
`EscMotor::set_velocity` / `set_effort_raw` / `set_position` **每次调用即续租**。超过
|
||||
`BusOptions::command_timeout`（默认 **50 ms**）没有新命令，驱动强制 0 电流。
|
||||
|
||||
- 这是"上层任务死亡后旧指令继续输出"的防线；
|
||||
- 因此上层必须**逐拍重发**同一个设定值（`chassisd` 的 1 kHz 循环天然满足）；
|
||||
- 只在开头下发一次的台架脚本会在 50 ms 后自动失电——**这是预期行为，不是 bug**。
|
||||
|
||||
### 6.2 命令队列必须浅
|
||||
|
||||
驱动按 1 kHz 生产命令，而 slcan 一次 write 在 CDC ACM 上约 2 ms（下行只有几百帧/秒）。
|
||||
若命令方向缓冲很深，队列会被过时命令灌满，ESC 执行的是**一秒多以前**算出的电流——
|
||||
速度环等于开了 1.4 s 延迟，轮子必然跑飞。
|
||||
|
||||
因此 slcan 桥的 `CHANNEL_BOUND = 8`：`send().await` 立即回压，每一帧都按最新反馈现算；
|
||||
反馈方向是采样流，来不及就丢（`try_send`），**绝不阻塞泵线程**。
|
||||
|
||||
### 6.3 桥的两个方向不能互相饿死
|
||||
|
||||
- 发送有上界（`TX_BURST`），一帧写不动时立刻让位给接收；
|
||||
- 接收批量排空（`RX_BURST`）；
|
||||
- `serial` crate 的 `TTYPort` 读写**共用同一个超时**，写超时属正常现象，**不能当致命错误**
|
||||
（否则一次写超时就会把接收方向一起带停）。
|
||||
|
||||
### 6.4 反馈离线
|
||||
|
||||
任一电机反馈超时（`feedback_timeout`，默认 **100 ms**）→ 该电机命令强制 0 电流并告警。
|
||||
顶层另有**上电门控**：任一电机从未收到过反馈前，整组不下发速度命令。
|
||||
|
||||
---
|
||||
|
||||
## 7. 控制设计
|
||||
|
||||
### 7.1 PID（`motor-core::Pid`）
|
||||
|
||||
并联式 PID，显式时间步 `dt`（秒），特性：
|
||||
|
||||
1. **输出限幅** `[out_min, out_max]`（速度环里取 `±current_limit`）；
|
||||
2. **积分抗饱和**（clamping）：输出已饱和且误差继续同向时**暂停积分**；
|
||||
3. **微分作用于测量值**（不是误差），避免设定值阶跃的微分冲击；
|
||||
4. 可选**微分一阶低通**（`with_derivative_tau`），抑制编码器量化噪声；
|
||||
5. `dt <= 0` 或非有限输入 → 返回上次输出且不更新内部状态（防御跳帧 / 首帧）。
|
||||
|
||||
### 7.2 速度环（`motor-core::VelocityController`）
|
||||
|
||||
```
|
||||
effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rpm, ±limit )
|
||||
```
|
||||
|
||||
`feedforward` 单位 **raw / rpm**：稳态下所需控制量与转速近似成正比。
|
||||
|
||||
- **空载台架必须 ff = 0**：轮子架空时维持转速只需克服摩擦的极小电流，任何接近
|
||||
`1/K`（GM3508 空载 `K ≈ 0.57 rpm/raw` → `1/K ≈ 1.77`）的前馈都会把空载轮子直接推到最高转速。
|
||||
- **带载**才需要正前馈（抵消地面摩擦），典型 ≈ 1.77 raw/rpm，必须按实车整定。
|
||||
- 前馈可能把输出推出限幅，因此 `update` 在加完前馈后**再夹一次**。
|
||||
|
||||
### 7.3 增益的物理直觉（空载台架）
|
||||
|
||||
| 现象 | 原因 | 方向 |
|
||||
| --- | --- | --- |
|
||||
| 高 `kp`（8~18）震荡 | 被控对象近似积分器（≈12 rpm/s per raw）+ 反馈延迟 | 降 `kp`、提 `ki` |
|
||||
| `ki` 太小长期停在 300~400 rpm | 静摩擦需要积分缓慢"顶"到起动电流（stick-slip） | 提 `ki`（有 `current_limit` 钳位兜底） |
|
||||
| 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 |
|
||||
| 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 |
|
||||
|
||||
### 7.4 被控对象模型（`dji-esc-sim`）
|
||||
|
||||
逐拍离散（`dt` 秒，电调坐标系：正电流 → 正转速）：
|
||||
|
||||
1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才作用；
|
||||
2. **摩擦**：运动时 `net = u − sign(rpm)·coulomb − viscous·rpm`；静止且 `|u| > static`
|
||||
时 `net = u − sign(u)·coulomb`；静止且 `|u| ≤ static` 时 `net = 0`（粘住）；
|
||||
3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，以时间常数 `tau` 一阶趋近；
|
||||
4. **量化**：机械角按转速积分并对 8192 取模，角度截断为 `u16`、转速为 `i16`
|
||||
（量化由协议类型自带）。
|
||||
|
||||
`PlantParams::gm3508()` 是**无摩擦无延迟**的理想模型；`gm3508_bench()` 叠加实测量级的
|
||||
静 / 库仑摩擦（400~600 raw）与 2 拍延迟，是整定的默认被控对象。
|
||||
|
||||
---
|
||||
|
||||
## 8. 配置参考（chassis.toml）
|
||||
|
||||
样例：[`config/chassis.toml.example`](../config/chassis.toml.example)。
|
||||
schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程与整定工具共用同一份**——
|
||||
这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。
|
||||
|
||||
### 8.1 顶层
|
||||
|
||||
| 键 | 含义 |
|
||||
| --- | --- |
|
||||
| `[can] interface` | 底盘总线 SocketCAN 接口名（如 `can0`） |
|
||||
| `[can.function] interface` | 预留的功能总线（云台 / 拨弹），必须与底盘总线**不同名** |
|
||||
| `[chassis]` | 控制循环与安全超时、速度上限 |
|
||||
| `[mecanum]` | `half_length` / `half_width` / `wheel_radius`（米） |
|
||||
| `[[motor]]` × 4 | 顺序固定 FL / FR / RL / RR |
|
||||
|
||||
### 8.2 `[chassis]`（缺省值）
|
||||
|
||||
| 键 | 默认 | 含义 |
|
||||
| --- | --- | --- |
|
||||
| `control_period_ms` | 1 | 控制节拍（1 kHz） |
|
||||
| `command_timeout_ms` | 200 | 上层 twist 超时 → 目标速度归零 |
|
||||
| `feedback_timeout_ms` | 100 | 反馈离线 → 强制 0 电流 |
|
||||
| `motor_command_timeout_ms` | 50 | 驱动层命令租约（必须 ≥ `control_period_ms`） |
|
||||
| `max_vx` / `max_vy` / `max_omega` | 3.0 / 3.0 / 6.0 | 速度限幅（m/s、m/s、rad/s） |
|
||||
|
||||
### 8.3 `[[motor]]`
|
||||
|
||||
| 键 | 含义 |
|
||||
| --- | --- |
|
||||
| `name` | 轮名（`front_left` …），整定的 `--motor` 用它选目标 |
|
||||
| `esc_id` | 电调 ID（1–8，即 CAN 拨码） |
|
||||
| `model` | `C620` / `C610` |
|
||||
| `direction` | 安装方向 `+1` / `-1` |
|
||||
| `gear_ratio` | 减速比（转子 : 输出轴），GM3508 ≈ 19.203 |
|
||||
| `current_limit` | effort 限幅（raw），同时是 PID 钳位点 |
|
||||
| `kp` / `ki` / `kd` / `feedforward` | 速度环增益（整定的产物写回这里） |
|
||||
|
||||
### 8.4 校验规则（`Config::validate`）
|
||||
|
||||
- 电机数必须**恰好 4**；麦轮几何必须为正；四个超时必须 > 0；
|
||||
`motor_command_timeout_ms ≥ control_period_ms`；速度上限 > 0；`gear_ratio > 0`；
|
||||
- 底盘总线**只允许一种协议族**（四轮不能混用 C620 / C610）；
|
||||
- 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）；
|
||||
- `deny_unknown_fields`：**拼错的键直接报错**，不会被静默忽略。
|
||||
|
||||
---
|
||||
|
||||
## 9. 标定与上车流程
|
||||
|
||||
> 每一步都有明确的**通过判据**。不要跳步——跳过的步骤会在后面以"轮子跑飞"的形式还回来。
|
||||
|
||||
| # | 步骤 | 命令 | 通过判据 |
|
||||
| --- | --- | --- | --- |
|
||||
| 1 | **链路与反馈** | `chassis-spin --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID |
|
||||
| 2 | **电调 ID 对应** | 同 1，逐个拔插 | 拔掉哪个电调，哪个 ID 的反馈消失 → ID 映射正确 |
|
||||
| 3 | **方向** | `chassis-spin --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` |
|
||||
| 4 | **减速比 / 轮径** | 手转一圈 + 读反馈角度 | 反馈角度 × 减速比 ≈ 轮子转过角度；不符则改 `gear_ratio` |
|
||||
| 5 | **几何参数** | 卷尺量 `half_length` / `half_width` / `wheel_radius` | 填入 `chassis.toml`（影响运动学精度，不影响稳定性） |
|
||||
| 6 | **限流** | 从 1500 raw 起，逐步加到"能可靠起动且不冲" | 越小越好；实车再按需上调 |
|
||||
| 7 | **空载整定** | `chassis-tune sweep --preset-name bench_noload` | 出一组增益：超调 ≤ 25%、稳态误差 ≤ 8%、三次复验一致 |
|
||||
| 8 | **带载复验** | 整车落地，`--preset-name bench_loaded` | 超调 ≤ 15%、稳态误差 ≤ 5%；空载参数直接上车通常**不行** |
|
||||
| 9 | **双向验收** | `--preset-name field_updown` | 升速 / 降速 / 回零三个方向都达标，重复性 ≥ 4/5 |
|
||||
| 10 | **固化** | `--write-config` + 人工评审 | 增益写入 `chassis.toml`，限流 / 方向 / 减速比未被改动 |
|
||||
|
||||
**关于"仿真整定能不能直接上车"**：不能。仿真最优解只在被控对象模型内最优，模型里的
|
||||
摩擦 / 延迟是量级估计且不含测量噪声，因此可能偏激进。必须用同一场景在台架上复验
|
||||
（第 10.6 节的迁移证据）。
|
||||
|
||||
---
|
||||
|
||||
## 10. 自动整定与超参数预设
|
||||
|
||||
手动试凑 `kp/ki/kd/ff` 既慢又难复现。`chassis-tune` 把它变成**参数空间搜索 + 版本化超参数**。
|
||||
|
||||
### 10.1 搜索算法
|
||||
|
||||
两阶段：
|
||||
|
||||
1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数铺满四维空间；
|
||||
2. **精修**：以成本最低的 top-k 为起点，在**坐标空间**做多起点 `argmin` 局部搜索
|
||||
（Nelder-Mead 默认，可选 PSO）。
|
||||
|
||||
**坐标空间**是关键的数值技巧：`kp/ki/kd` 合法取值跨数量级且必须为正，每个轴映射到
|
||||
（对数轴取 `ln`，线性轴恒等）之后——
|
||||
|
||||
- 无约束优化器不会走出正增益区域；
|
||||
- 步长在不同数量级上等价（±0.1 坐标步长 = ×1.1 比例步长）；
|
||||
- `LogOrZero` 轴在**粗扫**阶段可精确取 0（`ff = 0` 是台架常态），在**精修**阶段退化为对数轴
|
||||
（零不可导，本地搜索无收益）。
|
||||
|
||||
### 10.2 成本函数与硬约束
|
||||
|
||||
成本 = 归一化加权（调节时间 / 超调 / IAE / 电流），各项已归一化到 O(1)，因此权重在不同
|
||||
场景 / 电机之间可解释。
|
||||
|
||||
**硬约束不进权重**：过速、饱和占比过高、不收敛 → 直接给罚值 `1e9` 并记录判定。
|
||||
否则搜索会主动挑一个"飞车但其它项很好看"的解。
|
||||
|
||||
| 判定 | 触发条件 |
|
||||
| --- | --- |
|
||||
| `rejected` | 增益含 NaN/inf，或场景退化 |
|
||||
| `diverged` | 数值发散 |
|
||||
| `overspeed` | 峰值转速 > 目标 × 3 + 500 |
|
||||
| `saturated` | 饱和占比 > 40% |
|
||||
| `no-settle` | 场景内未收敛 |
|
||||
|
||||
### 10.3 鲁棒优化（默认）
|
||||
|
||||
单点模型整定出的增益对增益 / 时间常数 / 延迟高度敏感，因此默认在 `PlantEnsemble`
|
||||
（域随机化：增益 ±20%、时间常数 ±30%、延迟 0~3 拍、摩擦 ±50%，8 个成员）上评估，
|
||||
成本按 `robust_weight` 在"均值"与"最坏成员"之间折衷（默认 0.5）。
|
||||
|
||||
### 10.4 超参数预设（`config/tune.toml`）
|
||||
|
||||
**这是"实验条件"的版本管理**：搜什么区间、用哪个场景、什么算好、什么算达标、搜多细，
|
||||
全部写在 [`config/tune.toml`](../config/tune.toml) 里，评审一次、长期复用、任何机器上可复现。
|
||||
|
||||
优先级（**逐字段**覆盖）：
|
||||
|
||||
```
|
||||
内置默认 < 预设文件里的预设 < 显式命令行参数
|
||||
```
|
||||
|
||||
因此"用预设跑一次 + 临时改一个权重"是安全的，不会把其它超参数打回硬编码值。
|
||||
|
||||
```sh
|
||||
chassis-tune presets # 列出全部预设（只读）
|
||||
chassis-tune sweep --preset config/tune.toml # 用 default_preset
|
||||
chassis-tune sweep --preset config/tune.toml --preset-name bench_loaded
|
||||
chassis-tune sweep --preset-name bench_loaded # 缺省路径即 config/tune.toml
|
||||
chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预设的单项
|
||||
```
|
||||
|
||||
**随仓库提供的预设**：
|
||||
|
||||
| 预设名 | 后端 | 场景 | 用途 |
|
||||
| --- | --- | --- | --- |
|
||||
| `sim_bench_model`（默认） | sim | ramp-and-hold 1500 rpm | 台架量级模型全量搜索：评审、CI、代码回归的基准 |
|
||||
| `sim_ideal` | sim | ramp-and-hold 1500 rpm | 理想模型秒级回归；**结果不可直接上车** |
|
||||
| `bench_noload` | bench | ramp-and-hold 800 rpm | 空载台架：小样本局部搜索，**前馈固定为 0** |
|
||||
| `bench_loaded` | bench | ramp-and-hold 1200 rpm | 带载（整车落地）：前馈区间上调至 0.5~2.5 |
|
||||
| `field_updown` | bench | **up-down** 1200 rpm | 实战验收：升 / 降 / 回零双向，阈值收紧到 12% / 3% |
|
||||
|
||||
**预设 schema**（`version = 1`，未知键报错，见
|
||||
[`crates/chassis-tune/src/preset.rs`](../crates/chassis-tune/src/preset.rs)）：
|
||||
|
||||
```toml
|
||||
version = 1
|
||||
default_preset = "sim_bench_model"
|
||||
|
||||
[preset.bench_noload]
|
||||
description = "空载台架：摩擦小、静差靠积分压制"
|
||||
backend = "bench"
|
||||
|
||||
[preset.bench_noload.space]
|
||||
kp = { min = 0.2, max = 12.0, scale = "log" } # scale: linear | log | log0
|
||||
feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜索时恒定
|
||||
|
||||
[preset.bench_noload.cost] # settle / overshoot / iae / effort
|
||||
[preset.bench_noload.acceptance] # max_overshoot_pct / max_ss_error_pct
|
||||
[preset.bench_noload.search] # seed / samples / top_k / solver / max_iters / particles / max_evals / threads
|
||||
[preset.bench_noload.sim] # plant / ensemble / robust_weight / gain_pct / tau_pct / delay_ticks_max / friction_pct
|
||||
[preset.bench_noload.bench] # max_rpm / max_temp_c / trial_timeout_ms / between_trials_ms /
|
||||
# startup_timeout_ms / continue_on_abort / validate_runs
|
||||
```
|
||||
|
||||
每个小节都可缺省，只写"与默认不同"的项；刻度（`scale`）与区间可以分别覆盖
|
||||
（`--kp-scale linear` 不改区间）。
|
||||
|
||||
> **安全边界**：`current_limit`、`direction`、`gear_ratio`、电调 ID、串口设备名
|
||||
> **不是**预设字段——写进预设文件会直接报错。它们只来自 `chassis.toml` 或显式 CLI，
|
||||
> 否则预设文件就成了绕过限流的口子。
|
||||
|
||||
**新增一个预设**：在 `config/tune.toml` 里加一节 → 跑
|
||||
`chassis-tune presets` 确认能解算 → 跑一次小样本
|
||||
`chassis-tune sweep --preset config/tune.toml --preset-name <新名> --samples 16` 确认可用。
|
||||
测试会强制：每个预设都有 `description`；`bench_*` / `field_*` 必须 `backend = "bench"`
|
||||
（防止"以为在台架验过、其实是离线跑的"）。
|
||||
|
||||
### 10.5 产物与判读（`--out-dir`）
|
||||
|
||||
| 文件 | 内容 |
|
||||
| --- | --- |
|
||||
| `trials.csv` | 全部候选 + 判定（可复现性 diff 的基准） |
|
||||
| `best.toml` | 可直接粘贴的增益片段 |
|
||||
| `best-trace.csv` | 最优解的轨迹 |
|
||||
| `report.md` | 人类可读报告：参数空间、权重、验收结论、**边界提示** |
|
||||
| `summary.json` | 机器可读摘要 |
|
||||
| `hil-validation.csv` | 仅真机后端：同一组增益的重复性复验 |
|
||||
|
||||
判读要点：
|
||||
|
||||
1. **最优解贴到参数空间边界**时报告会明确提示——那是"约束在决定结果"，应放宽
|
||||
`--kp` / `--ki` 再搜，而不是把边界值当最优解交付；
|
||||
2. **达标数 / 评估数**太低（如 9/198）说明大部分候选被硬约束拦下，通常意味着限流太小
|
||||
或参数空间太宽；
|
||||
3. 真机 `hil-validation.csv` 的**离散度**比均值更重要：三次结果差很多 = 参数贴着噪声，不该上车。
|
||||
|
||||
### 10.6 真机后端与迁移证据
|
||||
|
||||
`--backend bench` 与仿真实现同一个 `Evaluator` trait，区别只是"评估一组增益"从浮点积分
|
||||
换成真机试验：打开链路 → 上电门控 → 逐次**热更新**增益（不重启总线）→ 逐拍重发设定值 +
|
||||
安全包络 → 任何退出路径先 0 电流 → 试验间静默。搜索强制单线程（只有一条总线）。
|
||||
|
||||
```
|
||||
仿真整定 → 台架复验（同场景、同阈值）→ 重复性达标 → 写回 chassis.toml → 整车验收
|
||||
```
|
||||
|
||||
**可复现性**：同一 `seed` + 同一参数空间 + 同一后端 → **逐位一致**。CI 的
|
||||
`just tune-smoke` 依赖这一点（同一命令跑两次，`diff trials.csv` 必须为空）。
|
||||
|
||||
---
|
||||
|
||||
## 11. 台架工具
|
||||
|
||||
### 11.1 `chassis-spin`（`chassis-bench`，slcan，全平台）
|
||||
|
||||
四轮同时以固定**转子**转速旋转。关键参数：
|
||||
|
||||
| 参数 | 默认 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `--device` | `/dev/ttyACM0` | slcan 串口设备 |
|
||||
| `--serial-baud` | 2000000 | CDC ACM 一般忽略；CH340 类需与固件一致 |
|
||||
| `--serial-timeout-ms` | 50 | 读写共用 |
|
||||
| `--can-speed` | `1m` | `1m` / `500k` |
|
||||
| `--esc-ids` | `1,2,3,4` | 顺序 = FL、FR、RL、RR |
|
||||
| `--directions` | `1,-1,1,-1` | 各轮安装方向 |
|
||||
| `--rpm` | 500 | 目标**转子**转速（轮端 = 转子 ÷ 减速比） |
|
||||
| `--gear-ratio` | 19.203 | 减速比 |
|
||||
| `--current-limit` | 1500 | raw；同时是 PID 钳位点 |
|
||||
| `--ramp-ms` | 3000 | 缓升时间（让积分在起动前平滑建立，避免"先憋住、一动就飞"） |
|
||||
| `--hold-secs` | 10 | 目标转速保持时长 |
|
||||
| `--startup-timeout-ms` | 3000 | 等待全部电调反馈的超时 |
|
||||
| `--dry-run` | — | 只验证链路与反馈，**不发任何非零电流** |
|
||||
| `--allow-partial` | — | 允许缺失部分电调（默认要求四个全在线） |
|
||||
| `--kp/--ki/--kd/--feedforward` | 2 / 10 / 0 / 0 | 空载台架默认值（见 7.3） |
|
||||
| `--max-rpm` | 目标 × 2 + 200 | 过速保护上限 |
|
||||
|
||||
安全与使用要点：
|
||||
|
||||
- **轮子必须架空或整车固定**；ctrl-c 随时安全停机（先 0 电流再关总线）；
|
||||
- 过速保护：任一电调超过 `--max-rpm` 持续 60 ms 即立即停机；
|
||||
- 上电门控：四个电调都上报新鲜反馈前绝不下发速度；
|
||||
- 空载的合适整定与带载**完全不同**，空载**不要**加前馈。
|
||||
|
||||
### 11.2 `socketcan-spin`（`chassis-bench`，仅 Linux）
|
||||
|
||||
单电机链路验证，走 SocketCAN。用于板载原生 CAN 的接线排查：
|
||||
|
||||
```sh
|
||||
ip link set can0 up type can bitrate 1000000 restart-ms 100
|
||||
cargo run --release -p chassis-bench --bin socketcan-spin -- --interface can0 --dry-run
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 12. 安全设计
|
||||
|
||||
按"故障点 → 防线"排列，每一层都独立生效：
|
||||
|
||||
| # | 故障 | 防线 | 默认阈值 |
|
||||
| --- | --- | --- | --- |
|
||||
| 1 | 上层任务死亡，旧指令继续输出 | **命令租约**：超时未刷新 → 0 输出 | 50 ms |
|
||||
| 2 | 电机 / CAN 掉线 | **反馈离线**：超时 → 该电机 0 电流 + 告警 | 100 ms |
|
||||
| 3 | 上电瞬间反馈未到就发速度 | **上电门控**：全部见到反馈前全体 `disable` | — |
|
||||
| 4 | 上层不再发指令 | **指令超时**：twist 归零 | 200 ms |
|
||||
| 5 | 轮子飞车 | 台架 **过速保护**（超过上限持续 60 ms 停机） | 目标 × 2 + 200 rpm |
|
||||
| 6 | 堵转烧电机 | **限流**（同时是 PID 钳位点，抗饱和） | 台架 1500 / 实车 6000~12000 raw |
|
||||
| 7 | 电调过热 | 台架**温度保护** | 70 ℃（空载预设 60 ℃） |
|
||||
| 8 | 整定 / 台架脚本失控 | 试验**超时** + 立即 0 电流 + 试验间静默 | 6 s / 500 ms |
|
||||
| 9 | 误触真机模式 | `--i-know-the-wheel-spins` **强制显式确认**（预设也无法绕过） | — |
|
||||
| 10 | 命令队列积压导致延迟 | slcan 桥 `CHANNEL_BOUND = 8` + `send().await` 回压 | — |
|
||||
| 11 | 进程被杀 | systemd `Restart=always`（板端） | 1 s |
|
||||
|
||||
**出事时的正确操作顺序**：① 急停 / 断电 → ② ctrl-c 让进程走优雅停机（先 0 电流）→
|
||||
③ 记下 `--dry-run` 与 `hil-validation.csv` 的现象 → ④ 再分析。
|
||||
|
||||
---
|
||||
|
||||
## 13. 测试与质量门禁
|
||||
|
||||
### 13.1 分层测试
|
||||
|
||||
| 层级 | 内容 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| 单元 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试、slcan 帧形状过滤 | 全平台 |
|
||||
| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、命令租约到期与续租、增益热更新、守护进程全链路 | 全平台 |
|
||||
| 模型一致性 | 同步 harness 与 `EscBus` 异步闭环在同一组增益下的稳态 / effort 对齐（防止"对着错模型调参"） | 全平台 |
|
||||
| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） |
|
||||
| **预设** | schema 往返、逐字段覆盖、优先级（CLI > 预设 > 内置）、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 |
|
||||
| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） |
|
||||
| 实车台架 | `chassis-spin` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） |
|
||||
| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check；armv7 静态构建 `chassisd` | CI |
|
||||
|
||||
### 13.2 每个阶段完成后的门禁（`just lint` + 以下）
|
||||
|
||||
```sh
|
||||
cargo build --workspace # 无警告
|
||||
cargo test --workspace # 全绿
|
||||
cargo clippy --workspace --all-targets -- -D warnings
|
||||
cargo fmt --all -- --check
|
||||
RUSTDOCFLAGS="-D warnings" cargo doc --workspace --no-deps
|
||||
cargo check -p chassis-tune --features plot --all-targets # 可选 feature 防腐烂
|
||||
just tune-smoke # 同 seed 逐位一致 + 达标
|
||||
```
|
||||
|
||||
新增代码要求：有文档注释；关键路径有测试覆盖；**跨 crate 的常量（如限流、减速比）只有一个来源**。
|
||||
|
||||
---
|
||||
|
||||
## 14. 部署
|
||||
|
||||
### 14.1 交叉编译（musl 静态，无需交叉工具链）
|
||||
|
||||
```sh
|
||||
just build-armv7 # cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2）
|
||||
```
|
||||
|
||||
目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。二进制静态链接，不依赖板端 libc。
|
||||
|
||||
### 14.2 板端拉起 CAN
|
||||
|
||||
```sh
|
||||
# SocketCAN（板载原生控制器）
|
||||
ip link set can0 type can bitrate 1000000 restart-ms 100
|
||||
ip link set can0 up
|
||||
ip -d link show can0
|
||||
|
||||
# slcan（USB 适配器）：不需要 ip link，应用层直接打开串口
|
||||
chmod 666 /dev/ttyACM0
|
||||
```
|
||||
|
||||
### 14.3 systemd
|
||||
|
||||
[`deploy/chassisd.service`](../deploy/chassisd.service)：
|
||||
|
||||
```ini
|
||||
ExecStart=/usr/local/bin/chassisd --interface can0 --config /etc/chassisd/chassis.toml
|
||||
Restart=always
|
||||
RestartSec=1
|
||||
# 实时性（按需打开）：
|
||||
# CPUSchedulingPolicy=fifo
|
||||
# CPUSchedulingPriority=20
|
||||
# LimitRTPRIO=95
|
||||
```
|
||||
|
||||
> 电调未上电 / CAN 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off
|
||||
> （`ip -d link show can0` 可见），`chassisd` 侧表现为四电机反馈离线、强制零电流——
|
||||
> **这正是安全设计的预期行为**，不是故障。
|
||||
>
|
||||
> 调度策略：`ARCHITECTURE.md` 规划了 `SCHED_FIFO` + `mlockall`（对应 `rm-app::realtime`），
|
||||
> 本仓库目前**未实现**实时优先级提升，当前只依赖 1 kHz tokio 定时器 +
|
||||
> `MissedTickBehavior::Skip`。板端抖动实测与是否需要加固见第 15 节的抖动条目。
|
||||
|
||||
---
|
||||
|
||||
## 15. 故障排查
|
||||
|
||||
| 症状 | 可能原因 | 处理 |
|
||||
| --- | --- | --- |
|
||||
| `/dev/ttyACM0` 打不开 | 权限 / 设备名不符 / 固件非 slcan | `ls -l /dev/ttyACM*`；`chmod 666`；确认适配器是 Lawicel 固件（不是 gs_usb） |
|
||||
| 四个电调都无反馈 | CAN_H/L 接反、无终端电阻、电调未上电、波特率不符 | 万用表量差分电阻（约 60Ω）；`--dry-run` 复查；确认 `--can-speed 1m` |
|
||||
| 只有部分电调有反馈 | 电调 ID 冲突 / 拨码错 / 该段线断 | 逐个拔插定位（流程第 2 步） |
|
||||
| 上电后轮子纹丝不动 | 正常：上电门控未满足 | 等反馈同时打印"waiting for first feedback"；查反馈丢失原因 |
|
||||
| 台架脚本 50 ms 后失电 | 命令租约到期（只下发了一次） | 每拍重发命令；这不是 bug |
|
||||
| 轮子"先憋住、一动就飞" | 前馈过大 / 限流远大于实际所需 / 缓升太短 | ff → 0；限流贴近实际；`--ramp-ms` 加大 |
|
||||
| 速度环高频震荡 | `kp` 太大（近似积分器 + 延迟） | 降 `kp`、提 `ki`；参考第 7.3 节 |
|
||||
| 稳态长期差 100+ rpm | 静摩擦未被积分吃掉 | 提 `ki`；确认 `current_limit` 大于静摩擦（400~600 raw） |
|
||||
| 整定报告说"贴边界" | 约束在决定结果 | 放宽对应轴区间再搜，别交付边界值 |
|
||||
| 仿真整定的参数上车就飞 | 模型不含摩擦 / 噪声，仿真解偏激进 | 用 `bench_loaded` / `field_updown` 复验；以台架结果为准 |
|
||||
| `trials.csv` 两次不一致 | 改了参数空间 / 预设 / 后端 | 检查 `超参数来源` 打印行；同 seed 必须同空间同后端 |
|
||||
| 守护进程起来就退出 | 配置校验失败 / `--config` 缺失 | 看 stderr；`Config::validate` 的报错会指出具体键 |
|
||||
| 抖动大（>500 µs） | 标准 Linux 非硬实时 | 先降 `chassis` 控制频率或合并任务；再考虑调度优先级 / 异构核（`ARCHITECTURE.md` 决策点 D4） |
|
||||
|
||||
---
|
||||
|
||||
## 16. 术语表
|
||||
|
||||
| 术语 | 含义 |
|
||||
| --- | --- |
|
||||
| SLCAN | Serial CAN，通过串口传输 CAN 帧的 ASCII（Lawicel）协议 |
|
||||
| SocketCAN | Linux 内核 CAN 子系统 |
|
||||
| raw | 协议原始控制量单位（C620 电流 ±16384 ↔ ±20 A） |
|
||||
| 命令租约 | `set_*` 每次调用续租，超时未刷新即强制 0 输出 |
|
||||
| 上电门控 | 未见全部电机反馈前不下发速度命令 |
|
||||
| 转子 / 轮端 | 电机轴 / 减速器输出轴；换算只在 `OutputShaft` 发生 |
|
||||
| 域随机化 | 在一组扰动参数（增益 / 时间常数 / 延迟 / 摩擦）上做鲁棒优化 |
|
||||
| 代价空间 | 对数（或恒等）映射后的参数坐标，使正增益约束天然满足 |
|
||||
| 超参数预设 | 版本化的搜索条件：空间 / 场景 / 权重 / 阈值 / 预算 / 被控对象 / 安全包络 |
|
||||
| HIL | Hardware-in-the-loop，此处指 `--backend bench` 的真机逐次试验 |
|
||||
|
||||
---
|
||||
|
||||
## 相关文档
|
||||
|
||||
- [整体架构规划 `ARCHITECTURE.md`](../ARCHITECTURE.md)
|
||||
- [已实现架构与解耦点 `docs/architecture.md`](architecture.md)
|
||||
- [配置样例 `config/chassis.toml.example`](../config/chassis.toml.example)
|
||||
- [整定超参数预设 `config/tune.toml`](../config/tune.toml)
|
||||
- [项目 README](../README.md)
|
||||
|
||||
| 版本 | 日期 | 变更 |
|
||||
| --- | --- | --- |
|
||||
| 1.0 | 2026-09-23 | 首版：底盘接口约定、标定流程、自动整定与超参数预设、安全与故障处理 |
|
||||
@@ -37,12 +37,69 @@ build-armv7:
|
||||
cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
|
||||
# 推送到板子并试运行（默认 mock 模式）
|
||||
deploy board="root@169.254.63.4" mode="--mock":
|
||||
deploy board="root@192.168.2.2" mode="--mock":
|
||||
just build-armv7
|
||||
ssh {{board}} "mkdir -p /tmp/chassisd"
|
||||
scp -q target/armv7-unknown-linux-musleabihf/release/chassisd {{board}}:/tmp/chassisd/chassisd
|
||||
ssh {{board}} "/tmp/chassisd/chassisd {{mode}}"
|
||||
|
||||
# 板端：拉起 CAN 接口（1Mbps，bus-off 后 100ms 自动恢复）
|
||||
can-up board="root@169.254.63.4" iface="can0":
|
||||
# 板端：拉起 SocketCAN 接口（1Mbps，bus-off 后 100ms 自动恢复）
|
||||
# 注意：slcan 适配器（CANable 系列）走串口，不走 SocketCAN，台架用下面的 bench-* 配方。
|
||||
can-up board="root@192.168.2.2" iface="can0":
|
||||
ssh {{board}} "ip link set {{iface}} down; ip link set {{iface}} up type can bitrate 1000000 restart-ms 100 && ip -br link show {{iface}}"
|
||||
|
||||
# 交叉编译台架工具（armv7，musl 静态，无需交叉工具链）
|
||||
build-bench:
|
||||
rustup target add armv7-unknown-linux-musleabihf
|
||||
cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf
|
||||
|
||||
# 台架：四轮同时 500 rpm（转子）旋转 —— 编译 → 推送 → 板端运行
|
||||
# 轮子必须架空或整车固定；ctrl-c 安全停机。
|
||||
bench-spin board="root@192.168.2.2" device="/dev/ttyACM0" rpm="500" opts="":
|
||||
just build-bench
|
||||
scp -q target/armv7-unknown-linux-musleabihf/release/chassis-spin {{board}}:/tmp/chassis-spin
|
||||
ssh {{board}} "/tmp/chassis-spin --device {{device}} --rpm {{rpm}} {{opts}}"
|
||||
|
||||
# 台架：只验证 slcan 链路与四轮反馈（不发非零电流）
|
||||
bench-check board="root@192.168.2.2" device="/dev/ttyACM0":
|
||||
just build-bench
|
||||
scp -q target/armv7-unknown-linux-musleabihf/release/chassis-spin {{board}}:/tmp/chassis-spin
|
||||
ssh {{board}} "/tmp/chassis-spin --device {{device}} --dry-run"
|
||||
|
||||
# 自动整定：列出超参数预设（只读；预设文件 config/tune.toml）
|
||||
tune-presets preset="config/tune.toml":
|
||||
cargo run -p chassis-tune -- presets --preset {{preset}}
|
||||
|
||||
# 自动整定：用预设跑（超参数进版本管理；CLI 只覆盖本次要变的项）
|
||||
# 例：just tune-preset bench_loaded "--samples 16"
|
||||
tune-preset name="sim_bench_model" opts="" preset="config/tune.toml":
|
||||
cargo run --release -p chassis-tune -- sweep --preset {{preset}} --preset-name {{name}} {{opts}}
|
||||
|
||||
# 自动整定：仿真搜索（不碰硬件；同 seed 结果逐位一致）
|
||||
tune-sim samples="512" seed="20260101":
|
||||
cargo run --release -p chassis-tune -- sweep --samples {{samples}} --seed {{seed}} --out-dir target/tune
|
||||
|
||||
# 自动整定：冒烟（确定性、秒级；CI 与提交前自检用）
|
||||
tune-smoke samples="64" seed="7":
|
||||
cargo run --release -p chassis-tune -- sweep --samples {{samples}} --top-k 3 --max-iters 40 --ensemble 2 --seed {{seed}} --out-dir target/tune-smoke
|
||||
|
||||
# 自动整定：复算一组给定增益（退出码 0 通过 / 1 未通过 / 2 参数错）
|
||||
tune-validate gains="2,10,0,0":
|
||||
cargo run --release -p chassis-tune -- validate --gains {{gains}}
|
||||
|
||||
# 自动整定：带 SVG 图输出（需要 --features plot）
|
||||
tune-plot samples="128":
|
||||
cargo run --release -p chassis-tune --features plot -- sweep --samples {{samples}} --out-dir target/tune-plot
|
||||
|
||||
# 自动整定：把结果写回一份配置（需 --config 指向现有 chassis.toml）
|
||||
tune-write config="config/chassis.toml.example" motor="front_left" out="target/tune/chassis.tuned.toml":
|
||||
cargo run --release -p chassis-tune -- sweep --config {{config}} --motor {{motor}} --write-config {{out}} --samples 256 --out-dir target/tune
|
||||
|
||||
# 自动整定：**真机**逐次试验（轮子会转！轮子架空或整车固定，急停可及）
|
||||
# 例：just tune-bench board="root@192.168.2.2" device="/dev/ttyACM0" esc_id="1" samples="24"
|
||||
# 预设提供场景 / 参数空间 / 权重 / 阈值 / 安全包络；CLI 提供接线与样本数（CLI 优先）。
|
||||
tune-bench board="root@192.168.2.2" device="/dev/ttyACM0" esc_id="1" samples="24" preset="bench_noload" opts="":
|
||||
cargo build --release -p chassis-tune
|
||||
scp -q target/release/chassis-tune {{board}}:/tmp/chassis-tune
|
||||
scp -q config/tune.toml {{board}}:/tmp/tune.toml
|
||||
ssh {{board}} "/tmp/chassis-tune sweep --backend bench --preset /tmp/tune.toml --preset-name {{preset}} --device {{device}} --esc-id {{esc_id}} --samples {{samples}} --top-k 3 --i-know-the-wheel-spins {{opts}}"
|
||||
|
||||
Reference in New Issue
Block a user