refactor: 重构架构并整理仓库
This commit is contained in:
Horldsence
+20
-49
@@ -10,7 +10,6 @@ env:
|
||||
|
||||
jobs:
|
||||
lint-and-test:
|
||||
name: fmt / clippy / test / doc
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
@@ -18,58 +17,47 @@ jobs:
|
||||
with:
|
||||
components: rustfmt, clippy
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- name: rustfmt
|
||||
run: cargo fmt --all -- --check
|
||||
- name: clippy
|
||||
run: cargo clippy --workspace --all-targets -- -D warnings
|
||||
- name: test
|
||||
run: cargo test --workspace
|
||||
- name: doc
|
||||
env:
|
||||
RUSTDOCFLAGS: -D warnings
|
||||
run: cargo doc --workspace --no-deps
|
||||
# 整定工具的可选 feature 不在默认 clippy/test 里，单独编译一次防止腐烂。
|
||||
- name: chassis-tune plot feature
|
||||
- run: cargo fmt --all -- --check
|
||||
- run: cargo clippy --workspace --all-targets -- -D warnings
|
||||
- run: cargo test --workspace --all-targets
|
||||
- name: enforce flat crate source
|
||||
run: test -z "$(find crates/*/src -mindepth 2 -type d -print)"
|
||||
- name: forbid unsafe and no_std markers
|
||||
run: "! rg '\\bunsafe\\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml'"
|
||||
- name: optional plot feature
|
||||
run: cargo check -p chassis-tune --features plot --all-targets
|
||||
# 自动整定的确定性冒烟：同 seed 必须逐位一致，且最优解必须通过验收阈值。
|
||||
- name: tune smoke (deterministic, no hardware)
|
||||
- name: deterministic tuning smoke
|
||||
run: |
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \
|
||||
--out-dir target/tune-smoke
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 \
|
||||
--out-dir target/tune-smoke-again
|
||||
cargo run --release -p chassis-tune -- sweep --samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 --out-dir target/tune-smoke
|
||||
cargo run --release -p chassis-tune -- sweep --samples 64 --top-k 3 --max-iters 40 --ensemble 2 --seed 7 --out-dir target/tune-smoke-again
|
||||
diff target/tune-smoke/trials.csv target/tune-smoke-again/trials.csv
|
||||
|
||||
vcan-integration:
|
||||
name: vcan0 integration (real kernel CAN stack)
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
- uses: dtolnay/rust-toolchain@stable
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- name: set up vcan0
|
||||
run: |
|
||||
- run: |
|
||||
sudo modprobe vcan
|
||||
sudo ip link add dev vcan0 type vcan
|
||||
sudo ip link set up vcan0
|
||||
- name: run vcan tests
|
||||
run: cargo test -p dji-esc-socketcan -- --ignored --test-threads 1
|
||||
- run: cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1
|
||||
|
||||
cross-aarch64:
|
||||
name: cross check aarch64
|
||||
cross-check:
|
||||
runs-on: ubuntu-latest
|
||||
strategy:
|
||||
matrix:
|
||||
target: [aarch64-unknown-linux-gnu, armv7-unknown-linux-musleabihf]
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
- uses: dtolnay/rust-toolchain@stable
|
||||
with:
|
||||
targets: aarch64-unknown-linux-gnu
|
||||
targets: ${{ matrix.target }}
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- run: cargo check --workspace --target aarch64-unknown-linux-gnu
|
||||
- run: cargo check --workspace --target ${{ matrix.target }}
|
||||
|
||||
build-armv7:
|
||||
name: build armv7 static (board target)
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
@@ -77,21 +65,4 @@ jobs:
|
||||
with:
|
||||
targets: armv7-unknown-linux-musleabihf
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- run: cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
# 台架工具与整定工具同样要在板端跑（真机后端常直接跑在 T153 上）。
|
||||
- run: cargo build --release -p chassis-bench --bin chassis-spin --target armv7-unknown-linux-musleabihf
|
||||
- run: cargo build --release -p chassis-tune --target armv7-unknown-linux-musleabihf
|
||||
|
||||
coverage:
|
||||
name: coverage (llvm-cov)
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
- uses: dtolnay/rust-toolchain@stable
|
||||
- uses: Swatinem/rust-cache@v2
|
||||
- uses: taiki-e/install-action@cargo-llvm-cov
|
||||
- run: cargo llvm-cov --workspace --lcov --output-path lcov.info
|
||||
- uses: actions/upload-artifact@v4
|
||||
with:
|
||||
name: coverage-lcov
|
||||
path: lcov.info
|
||||
- run: cargo build --release -p chassisd -p spin -p chassis-tune --target armv7-unknown-linux-musleabihf
|
||||
|
||||
+115
-1130
File diff suppressed because it is too large
Load Diff
Generated
+215
-79
@@ -113,6 +113,23 @@ version = "1.5.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53"
|
||||
|
||||
[[package]]
|
||||
name = "bench"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"anyhow",
|
||||
"can",
|
||||
"esc-bus",
|
||||
"experiment",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"slcan 0.1.0",
|
||||
"socketcan 0.1.0",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "bit-set"
|
||||
version = "0.8.0"
|
||||
@@ -153,7 +170,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
|
||||
|
||||
[[package]]
|
||||
name = "can-link"
|
||||
name = "can"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"tokio",
|
||||
@@ -181,30 +198,15 @@ version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f079e83a288787bcd14a6aea84cee5c87a67c5a3e660c30f557a3d24761b3527"
|
||||
|
||||
[[package]]
|
||||
name = "chassis-bench"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can-link",
|
||||
"clap",
|
||||
"dji-esc",
|
||||
"dji-esc-sim",
|
||||
"dji-esc-slcan",
|
||||
"dji-esc-socketcan",
|
||||
"motor-core",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
"tracing-subscriber",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "chassis-config"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"dji-esc",
|
||||
"approx",
|
||||
"esc-bus",
|
||||
"mecanum",
|
||||
"motor-core",
|
||||
"motor-control",
|
||||
"motor-protocol",
|
||||
"serde",
|
||||
"thiserror",
|
||||
"toml",
|
||||
@@ -218,25 +220,32 @@ dependencies = [
|
||||
"approx",
|
||||
"argmin",
|
||||
"argmin-math",
|
||||
"can-link",
|
||||
"chassis-bench",
|
||||
"bench",
|
||||
"can",
|
||||
"chassis-config",
|
||||
"clap",
|
||||
"csv",
|
||||
"dji-esc",
|
||||
"dji-esc-sim",
|
||||
"esc-bus",
|
||||
"experiment",
|
||||
"indicatif",
|
||||
"motor-core",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"motor-model",
|
||||
"motor-protocol",
|
||||
"plotters",
|
||||
"rand",
|
||||
"rayon",
|
||||
"serde",
|
||||
"serde_json",
|
||||
"sim-harness",
|
||||
"slcan 0.1.0",
|
||||
"socketcan 0.1.0",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"toml",
|
||||
"tracing",
|
||||
"tracing-subscriber",
|
||||
"tuning",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@@ -244,15 +253,15 @@ name = "chassisd"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"chassis-config",
|
||||
"clap",
|
||||
"dji-esc",
|
||||
"dji-esc-sim",
|
||||
"dji-esc-socketcan",
|
||||
"esc-bus",
|
||||
"mecanum",
|
||||
"motor-core",
|
||||
"proptest",
|
||||
"thiserror",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"sim-harness",
|
||||
"socketcan 0.1.0",
|
||||
"tokio",
|
||||
"tracing",
|
||||
"tracing-subscriber",
|
||||
@@ -449,6 +458,27 @@ dependencies = [
|
||||
"syn 2.0.119",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "delta10"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can",
|
||||
"delta10-protocol",
|
||||
"imu",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "delta10-protocol"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"imu",
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "derive_builder"
|
||||
version = "0.20.2"
|
||||
@@ -501,53 +531,6 @@ dependencies = [
|
||||
"windows-sys",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dji-esc"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can-link",
|
||||
"motor-core",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dji-esc-sim"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can-link",
|
||||
"dji-esc",
|
||||
"motor-core",
|
||||
"rand",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dji-esc-slcan"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can-link",
|
||||
"serial",
|
||||
"slcan",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dji-esc-socketcan"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can-link",
|
||||
"dji-esc",
|
||||
"motor-core",
|
||||
"socketcan",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "dlib"
|
||||
version = "0.5.3"
|
||||
@@ -606,6 +589,30 @@ dependencies = [
|
||||
"windows-sys",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "esc-bus"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"experiment",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"motor-model",
|
||||
"motor-protocol",
|
||||
"sim-harness",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "experiment"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "fastrand"
|
||||
version = "2.5.0"
|
||||
@@ -839,6 +846,10 @@ version = "1.0.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39"
|
||||
|
||||
[[package]]
|
||||
name = "imu"
|
||||
version = "0.1.0"
|
||||
|
||||
[[package]]
|
||||
name = "indexmap"
|
||||
version = "2.14.2"
|
||||
@@ -990,8 +1001,41 @@ dependencies = [
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "motor-core"
|
||||
name = "motor"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"motor-protocol",
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "motor-control"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"motor",
|
||||
"pid",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "motor-model"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"motor-protocol",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "motor-protocol"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"proptest",
|
||||
"thiserror",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "nb"
|
||||
@@ -1143,6 +1187,13 @@ dependencies = [
|
||||
"rustc_version",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pid"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "pin-project-lite"
|
||||
version = "0.2.17"
|
||||
@@ -1546,12 +1597,41 @@ dependencies = [
|
||||
"libc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "sim-harness"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"can",
|
||||
"experiment",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"motor-model",
|
||||
"motor-protocol",
|
||||
"rand",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "slab"
|
||||
version = "0.4.12"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5"
|
||||
|
||||
[[package]]
|
||||
name = "slcan"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can",
|
||||
"serial",
|
||||
"slcan 0.1.3",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "slcan"
|
||||
version = "0.1.3"
|
||||
@@ -1577,6 +1657,21 @@ dependencies = [
|
||||
"windows-sys",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "socketcan"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"can",
|
||||
"esc-bus",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"motor-protocol",
|
||||
"socketcan 4.0.0",
|
||||
"thiserror",
|
||||
"tokio",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "socketcan"
|
||||
version = "4.0.0"
|
||||
@@ -1598,6 +1693,24 @@ dependencies = [
|
||||
"tokio",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "spin"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"bench",
|
||||
"clap",
|
||||
"esc-bus",
|
||||
"experiment",
|
||||
"mecanum",
|
||||
"motor",
|
||||
"motor-control",
|
||||
"slcan 0.1.0",
|
||||
"socketcan 0.1.0",
|
||||
"tokio",
|
||||
"tracing",
|
||||
"tracing-subscriber",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "strsim"
|
||||
version = "0.11.1"
|
||||
@@ -1809,6 +1922,29 @@ version = "0.20.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "17f77d76d837a7830fe1d4f12b7b4ba4192c1888001c7164257e4bc6d21d96b4"
|
||||
|
||||
[[package]]
|
||||
name = "tuning"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"approx",
|
||||
"argmin",
|
||||
"argmin-math",
|
||||
"experiment",
|
||||
"indicatif",
|
||||
"motor-control",
|
||||
"motor-model",
|
||||
"motor-protocol",
|
||||
"pid",
|
||||
"proptest",
|
||||
"rand",
|
||||
"rayon",
|
||||
"serde",
|
||||
"sim-harness",
|
||||
"thiserror",
|
||||
"toml",
|
||||
"tracing",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "unarray"
|
||||
version = "0.1.4"
|
||||
|
||||
+45
-20
@@ -1,17 +1,27 @@
|
||||
[workspace]
|
||||
resolver = "3"
|
||||
members = [
|
||||
"crates/motor-core",
|
||||
"crates/can-link",
|
||||
"crates/dji-esc",
|
||||
"crates/dji-esc-sim",
|
||||
"crates/dji-esc-socketcan",
|
||||
"crates/dji-esc-slcan",
|
||||
"crates/can",
|
||||
"crates/imu",
|
||||
"crates/motor-protocol",
|
||||
"crates/delta10-protocol",
|
||||
"crates/socketcan",
|
||||
"crates/slcan",
|
||||
"crates/delta10",
|
||||
"crates/motor",
|
||||
"crates/esc-bus",
|
||||
"crates/pid",
|
||||
"crates/motor-control",
|
||||
"crates/mecanum",
|
||||
"crates/motor-model",
|
||||
"crates/experiment",
|
||||
"crates/sim-harness",
|
||||
"crates/tuning",
|
||||
"crates/chassis-config",
|
||||
"crates/chassis-bench",
|
||||
"crates/chassis-tune",
|
||||
"crates/bench",
|
||||
"crates/chassisd",
|
||||
"crates/spin",
|
||||
"crates/chassis-tune",
|
||||
]
|
||||
|
||||
[workspace.package]
|
||||
@@ -27,20 +37,23 @@ strip = true
|
||||
[workspace.dependencies]
|
||||
# 异步运行时
|
||||
tokio = { version = "1", features = ["rt-multi-thread", "macros", "sync", "time", "signal"] }
|
||||
# CAN 总线（仅 Linux，由 dji-esc-socketcan 按 target 引入）
|
||||
socketcan = { version = "4", features = ["tokio"] }
|
||||
# CAN 总线（仅 Linux，由 socketcan 按 target 引入）
|
||||
socketcan-rs = { package = "socketcan", version = "4", features = ["tokio"] }
|
||||
# SLCAN 串口适配器（CANable 等，用户态 Lawicel 协议）
|
||||
slcan-proto = { package = "slcan", version = "0.1.3" }
|
||||
serial = "0.4"
|
||||
# 错误定义与结构化日志
|
||||
thiserror = "2"
|
||||
tracing = "0.1"
|
||||
tracing-subscriber = { version = "0.3", features = ["env-filter"] }
|
||||
# 配置与命令行（chassisd）
|
||||
# 配置与命令行
|
||||
serde = { version = "1", features = ["derive"] }
|
||||
toml = "0.9"
|
||||
clap = { version = "4", features = ["derive"] }
|
||||
# 测试
|
||||
proptest = "1"
|
||||
approx = "0.5"
|
||||
# 自动整定工具（chassis-tune）
|
||||
# 自动整定工具
|
||||
#
|
||||
# `rand` 对齐 argmin 依赖的 0.9，避免依赖树里出现两个 rand 大版本；
|
||||
# `argmin-math` 必须显式开启 `vec`（argmin 自身只启 `primitives`），
|
||||
@@ -56,16 +69,28 @@ anyhow = "1"
|
||||
# 仅在 chassis-tune 的 `plot` feature 下引入：只要 SVG 后端与折线系列，
|
||||
# 但文字标签需要 `ttf`（字体加载），因此不能用纯 svg_backend 最小集。
|
||||
plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] }
|
||||
# workspace 内部 crate
|
||||
motor-core = { path = "crates/motor-core" }
|
||||
can-link = { path = "crates/can-link" }
|
||||
dji-esc = { path = "crates/dji-esc" }
|
||||
dji-esc-sim = { path = "crates/dji-esc-sim" }
|
||||
dji-esc-socketcan = { path = "crates/dji-esc-socketcan" }
|
||||
dji-esc-slcan = { path = "crates/dji-esc-slcan" }
|
||||
|
||||
# ---- 职责型内部 crate ----
|
||||
can = { path = "crates/can" }
|
||||
imu = { path = "crates/imu" }
|
||||
motor-protocol = { path = "crates/motor-protocol" }
|
||||
delta10-protocol = { path = "crates/delta10-protocol" }
|
||||
socketcan = { path = "crates/socketcan" }
|
||||
slcan = { path = "crates/slcan" }
|
||||
delta10 = { path = "crates/delta10" }
|
||||
motor = { path = "crates/motor" }
|
||||
esc-bus = { path = "crates/esc-bus" }
|
||||
pid = { path = "crates/pid" }
|
||||
motor-control = { path = "crates/motor-control" }
|
||||
mecanum = { path = "crates/mecanum" }
|
||||
motor-model = { path = "crates/motor-model" }
|
||||
experiment = { path = "crates/experiment" }
|
||||
sim-harness = { path = "crates/sim-harness" }
|
||||
tuning = { path = "crates/tuning" }
|
||||
chassis-config = { path = "crates/chassis-config" }
|
||||
chassis-bench = { path = "crates/chassis-bench" }
|
||||
bench = { path = "crates/bench" }
|
||||
chassisd = { path = "crates/chassisd" }
|
||||
spin = { path = "crates/spin" }
|
||||
chassis-tune = { path = "crates/chassis-tune" }
|
||||
|
||||
[workspace.lints.rust]
|
||||
|
||||
@@ -1,171 +1,61 @@
|
||||
# sentry-chassis
|
||||
|
||||
机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），CAN 接入（SocketCAN 或串口 slcan），tokio 异步架构。
|
||||
RoboMaster 哨兵机器人麦轮底盘：Linux ARM 主控、4 × GM3508/C620、SocketCAN 或 SLCAN、tokio 异步运行时。
|
||||
|
||||
## 架构一览
|
||||
架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 21 个职责单一 crate 组成，不使用 `rm-` 前缀，crate 内源码保持扁平。
|
||||
|
||||
| crate | 职责 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| [`motor-core`](crates/motor-core) | 电机抽象：能力 trait、`MotorState`、PID / 速度环 / 位置环 | 纯逻辑，全平台 |
|
||||
| [`can-link`](crates/can-link) | CAN 传输抽象：`CanFrame` + `CanLink` 通道对（无协议知识） | 纯逻辑，全平台 |
|
||||
| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-sim`](crates/dji-esc-sim) | 被控对象仿真：摩擦 / 延迟 / 负载模型、场景与**离线评估 harness**、异步假电调 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-socketcan`](crates/dji-esc-socketcan) | SocketCAN 后端：真实 CAN 接口 ↔ 内存通道桥接 | Linux |
|
||||
| [`dji-esc-slcan`](crates/dji-esc-slcan) | slcan 串口后端：CANable 等 USB 适配器 ↔ 内存通道桥接 | 全平台 |
|
||||
| [`mecanum`](crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 |
|
||||
| [`chassis-config`](crates/chassis-config) | `chassis.toml` schema：解析、校验、写回（守护进程与整定工具共用） | 全平台 |
|
||||
| [`chassis-bench`](crates/chassis-bench) | 台架：激励场景、安全包络、真机试验采样 + `chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） |
|
||||
| [`chassis-tune`](crates/chassis-tune) | **速度环 PID 自动整定**：参数空间搜索 + 指标 / 成本 + 报告（仿真 / 真机）+ 版本化**超参数预设** | 全平台 |
|
||||
| [`chassisd`](crates/chassisd) | 底盘守护进程：配置、1kHz 控制循环、安全逻辑、`--mock` 模式 | 全平台（实车需 Linux） |
|
||||
## 结构
|
||||
|
||||
分层原则：`motor-core`、`dji-esc`、`dji-esc-sim` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道
|
||||
（`CanLink`），因此协议、运动学、控制与整定逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层
|
||||
（`dji-esc-socketcan` / `dji-esc-slcan`，以及组合它们的 `chassis-bench` / `chassisd`）。
|
||||
- `can` / `imu`：基础接口。
|
||||
- `motor-protocol` / `delta10-protocol`：纯协议编解码。
|
||||
- `socketcan` / `slcan` / `delta10`：硬件驱动。
|
||||
- `motor` / `esc-bus`：设备契约与安全总线运行时。
|
||||
- `pid` / `motor-control` / `mecanum`：纯控制算法。
|
||||
- `motor-model` / `experiment` / `sim-harness` / `tuning`：仿真与自动整定。
|
||||
- `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`：配置、台架和应用入口。
|
||||
|
||||
> 运输后端 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接，**不含应用**；
|
||||
> 台架程序住在 `chassis-bench`，自动整定住在 `chassis-tune`。
|
||||
|
||||
> **命令是租约**：`EscMotor::set_velocity` / `set_effort_raw` / `set_position` 每次调用即续租，
|
||||
> 超过 `BusOptions::command_timeout`（默认 50ms）未刷新，驱动强制 0 电流。
|
||||
> 控制循环必须**逐拍重发**同一个设定值（`chassisd` 的 1kHz 循环天然满足）。
|
||||
`esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为
|
||||
`motor::EffortPolicy` 传入总线，从而保持依赖无环。
|
||||
|
||||
## 开发
|
||||
|
||||
需要 Rust stable（仓库通过 `rust-toolchain.toml` 锁定）。
|
||||
|
||||
```sh
|
||||
cargo test --workspace # 单测 + 闭环仿真测试（无需硬件）
|
||||
cargo run -p chassisd -- --mock # 无硬件全链路演示
|
||||
just tune-smoke # 自动整定冒烟（确定性，无需硬件，CI 同款）
|
||||
just lint # fmt + clippy（与 CI 同标准）
|
||||
cargo test --workspace --all-targets
|
||||
cargo run -p chassisd -- --mock
|
||||
cargo run -p spin -- chassis --device /dev/ttyACM0 --dry-run
|
||||
cargo run -p chassis-tune -- presets --preset config/tune.toml
|
||||
just lint
|
||||
just tune-smoke
|
||||
```
|
||||
|
||||
### Linux 虚拟 CAN（vcan0）集成测试
|
||||
底盘控制频率为 1 kHz。命令是 50 ms 租约，上层必须逐拍重发目标值。
|
||||
|
||||
## 台架
|
||||
|
||||
```sh
|
||||
just vcan-test # 或见 justfile 内的 modprobe / ip link 命令
|
||||
# 四轮 500 rpm 转子，slcan
|
||||
cargo run --release -p spin -- chassis --device /dev/ttyACM0 --rpm 500
|
||||
|
||||
# 单电机 SocketCAN 验证
|
||||
cargo run --release -p spin -- motor can0 1 1000 10
|
||||
```
|
||||
|
||||
## 台架：四轮 500 rpm 旋转测试（slcan）
|
||||
轮子必须架空或整车固定，急停可及。工具默认限流、限时并支持 ctrl-c 安全停机。
|
||||
|
||||
实车台架验证工具 `chassis-spin`：四轮同时以固定**转子**转速旋转（默认 500 rpm，
|
||||
轮端 = 转子 ÷ 减速比 ≈ 26 rpm），带缓升、过速保护、上电门控与安全停机。
|
||||
## 自动整定
|
||||
|
||||
```sh
|
||||
# 本机直接跑（CANable2 在 /dev/ttyACM0）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0
|
||||
|
||||
# 只验证链路与反馈，不发非零电流（接线排查首选）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
|
||||
# 目标板（T153，armv7）：交叉编译 → 推送 → 运行
|
||||
just bench-spin # 默认 root@192.168.2.2 + /dev/ttyACM0 + 500 rpm
|
||||
just bench-check # 同上，但只做 dry-run
|
||||
```
|
||||
|
||||
安全与使用要点：
|
||||
|
||||
- **轮子必须架空或整车固定**；ctrl-c 随时安全停机（先 0 电流再关总线）。
|
||||
- 上电门控：四个电调都上报新鲜反馈前绝不下发速度；只接了部分电调时用 `--allow-partial` 降级。
|
||||
- 过速保护：任一电调超过 `--max-rpm`（默认 目标 × 2 + 200）持续 60ms 即立即停机。
|
||||
- 空载台架的合适整定与带载完全不同：默认 `kp=2 ki=10 ff=0 --current-limit 1500`，
|
||||
空载**不要**加前馈（会直接把轮子推到最高速度）。
|
||||
|
||||
## 自动整定（chassis-tune）
|
||||
|
||||
手动试凑 `kp/ki/kd/ff` 既慢又难复现。`chassis-tune` 把这件事变成**参数空间搜索**：
|
||||
种子化分层粗扫（`rayon` 并行）→ 以 top-k 为起点做多起点 `argmin` 局部精修（Nelder-Mead / PSO），
|
||||
每次评估都用同一套指标（上升时间、超调、调节时间、稳态误差、IAE、电流）和同一套硬约束
|
||||
（过速 / 长时间饱和 / 不收敛直接判罚，不参与权重折衷）。
|
||||
|
||||
```sh
|
||||
# 0) 看有哪些超参数预设（只读；预设文件见 config/tune.toml）
|
||||
cargo run -p chassis-tune -- presets
|
||||
|
||||
# 1) 仿真搜索（默认，不碰硬件；同 seed 结果逐位一致）
|
||||
cargo run --release -p chassis-tune -- sweep --samples 512 --out-dir target/tune
|
||||
|
||||
# 1b) 用预设跑：超参数（区间 / 场景 / 权重 / 阈值 / 预算 / 安全包络）进版本管理
|
||||
# 优先级：内置默认 < 预设文件 < 显式 CLI（逐字段覆盖）
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--preset config/tune.toml --preset-name bench_loaded
|
||||
|
||||
# 2) 只复算一组给定增益（CI / 回归用，按验收阈值给退出码：0 通过 / 1 未通过 / 2 参数错）
|
||||
cargo run -p chassis-tune -- validate --gains 2,10,0,0
|
||||
|
||||
# 3) 台架复验（**轮子会转**：必须架空或整车固定、急停可及）
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--preset config/tune.toml --preset-name bench_noload \
|
||||
--device /dev/ttyACM0 --esc-id 1 --i-know-the-wheel-spins
|
||||
--preset config/tune.toml --preset-name sim_bench_model \
|
||||
--samples 512 --out-dir target/tune
|
||||
```
|
||||
|
||||
随仓库提供的预设（`config/tune.toml`）：
|
||||
仿真搜索不接触硬件。真机后端会转动轮子，必须显式传入
|
||||
`--i-know-the-wheel-spins`。详细接线、标定和排障见
|
||||
[`docs/chassis.md`](docs/chassis.md)。
|
||||
|
||||
| 预设 | 后端 | 用途 |
|
||||
| --- | --- | --- |
|
||||
| `sim_bench_model`（默认） | 仿真 | 台架量级模型全量搜索：评审 / CI / 代码回归基准 |
|
||||
| `sim_ideal` | 仿真 | 理想模型秒级回归（结果**不可直接上车**） |
|
||||
| `bench_noload` | 真机 | 空载台架，小样本局部搜索，前馈固定 0 |
|
||||
| `bench_loaded` | 真机 | 带载（整车落地），前馈区间上调 |
|
||||
| `field_updown` | 真机 | 实战验收：升 / 降 / 回零双向，阈值收紧 |
|
||||
## 实时调度
|
||||
|
||||
预设**不能**携带 `current_limit` / `direction` / `gear_ratio`（写了直接报错）——
|
||||
安全与机械约束只来自 `chassis.toml` 或显式 CLI。
|
||||
|
||||
产物（`--out-dir`）：`trials.csv`（全部候选与判定）、`best.toml`（可粘贴的增益片段）、
|
||||
`best-trace.csv`、`report.md`、`summary.json`；台架模式另有 `hil-validation.csv`（同一组增益的重复性复验）。
|
||||
加 `--features plot` 还会输出 `best.svg` 三联图。
|
||||
|
||||
整定工具与守护进程**共用同一份配置 schema**（`chassis-config`），因此可以直接拿实车配置当输入：
|
||||
|
||||
```sh
|
||||
# 用 chassis.toml 里的限流 / 电调 ID / 方向 / 减速比（这些**不参与搜索**），
|
||||
# 打印与现有增益的成本对比，并把结果写回一份新配置
|
||||
cargo run --release -p chassis-tune -- sweep \
|
||||
--config config/chassis.toml.example --motor front_left \
|
||||
--write-config target/chassis.tuned.toml
|
||||
```
|
||||
|
||||
要点：
|
||||
|
||||
- **仿真整定只保证在被控对象模型内最优**。模型里的摩擦 / 延迟是量级估计，且不含测量噪声，
|
||||
因此仿真最优解可能偏激进；交付前必须用同一 `--scenario` 跑一次 `--backend bench` 复验。
|
||||
- 最优解贴到参数空间边界时报告会明确提示——那是"约束在决定结果"，应放宽 `--kp` / `--ki` 再搜。
|
||||
- 台架搜索串行执行（只有一条总线），每次试验后强制 0 电流并静默 `--between-trials-ms`；
|
||||
过速 / 过温 / 掉线 / 超时 / ctrl-c 任一命中即停机并收工。
|
||||
- `current_limit`、`direction`、`gear_ratio` 属于安全与机械约束，**不参与搜索**。
|
||||
- 换一组工况先加**预设**（`config/tune.toml`）而不是复制一长串命令行：
|
||||
`chassis-tune presets` 查看、`--preset-name` 选用、CLI 只覆盖本次要变的项。
|
||||
|
||||
## 部署到目标板（armv7，实测：创龙 T153 / Ubuntu 22.04）
|
||||
|
||||
二进制为 musl 静态链接，不依赖板端 libc，无需交叉工具链：
|
||||
|
||||
```sh
|
||||
just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2）
|
||||
# 或手动：
|
||||
cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf
|
||||
scp target/armv7-unknown-linux-musleabihf/release/chassisd root@192.168.2.2:/tmp/
|
||||
```
|
||||
|
||||
板端 CAN 有两条互斥路径：
|
||||
|
||||
- **SocketCAN**（板载原生 CAN 控制器，走 `can0`/`can1`）：
|
||||
`ip link set can0 up type can bitrate 1000000 restart-ms 100`，配 `--interface can0`。
|
||||
- **slcan**（USB 适配器，如 CANable2 在 CDC ACM 下枚举为 `/dev/ttyACM0`）：
|
||||
用 [`dji-esc-slcan`](crates/dji-esc-slcan)（`chassisd` / [`chassis-bench`](crates/chassis-bench) 均可），无需 `ip link`。
|
||||
|
||||
实车运行：`/tmp/chassisd --config /tmp/chassis.toml --interface can0`。板上运行样例见
|
||||
[`deploy/chassisd.service`](deploy/chassisd.service)，配置样例见
|
||||
[`config/chassis.toml.example`](config/chassis.toml.example)。
|
||||
|
||||
> 注意：电调未上电 / CAN 线未接时，SocketCAN 侧会因无 ACK 进入 bus-off（可用
|
||||
> `ip -d link show can0` 查看），chassisd 侧表现为四电机反馈离线、强制零电流——
|
||||
> 这正是安全设计的预期行为。
|
||||
|
||||
## 文档
|
||||
|
||||
- [**底盘开发文档**](docs/chassis.md)——硬件接线、坐标与单位约定、控制与安全设计、配置参考、
|
||||
**标定与上车流程**、自动整定与超参数预设、台架工具、故障排查（开发 / 调试 / 上车先看这份）
|
||||
- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、被控对象模型、参数空间搜索、测试策略
|
||||
- [整体架构规划](ARCHITECTURE.md)——全系统（含云台 / 发射 / IMU / 实时性）的 crate 划分与阶段计划
|
||||
仓库不使用 unsafe 或 libc FFI。生产调度策略由
|
||||
[`deploy/chassisd.service`](deploy/chassisd.service) 的 systemd 指令设置。
|
||||
std 程序不调用 `mlockall`。
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
[package]
|
||||
name = "bench"
|
||||
description = "Hardware bench links, safety envelope, and trial runner"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,947 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod link {
|
||||
//! 链路打开：把两种运输后端收敛成一个门面。
|
||||
//!
|
||||
//! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`CanLink`]"，
|
||||
//! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处，上层就不必
|
||||
//! 同时依赖两个运输 crate。
|
||||
|
||||
use std::io::Result;
|
||||
use std::time::Duration;
|
||||
|
||||
use can::CanLink;
|
||||
|
||||
/// slcan 波特率档位（对应协议 `Sn` 命令）。
|
||||
pub use slcan::CanSpeed;
|
||||
/// slcan 打开参数。
|
||||
pub use slcan::SlcanOptions;
|
||||
|
||||
/// 打开 slcan 串口设备（CANable 等），桥接为链路端点。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。
|
||||
pub fn open_slcan(device: &str, options: SlcanOptions) -> Result<CanLink> {
|
||||
slcan::open_link_with(device, options)
|
||||
}
|
||||
|
||||
/// 打开 SocketCAN 接口（Linux）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 未 up / 权限不足时的 `io::Error`；非 Linux 平台恒为错误。
|
||||
#[cfg(target_os = "linux")]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
socketcan::open_link(interface)
|
||||
}
|
||||
|
||||
/// 非 Linux 平台没有 SocketCAN。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 恒返回 `Unsupported`。
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn open_socketcan(interface: &str) -> Result<CanLink> {
|
||||
Err(std::io::Error::new(
|
||||
std::io::ErrorKind::Unsupported,
|
||||
format!("SocketCAN ({interface}) 仅支持 Linux"),
|
||||
))
|
||||
}
|
||||
|
||||
/// 默认 slcan 参数（1 Mbps、串口超时 50 ms）。
|
||||
#[must_use]
|
||||
pub fn default_slcan_options(serial_baud: u32, speed: CanSpeed) -> SlcanOptions {
|
||||
SlcanOptions { serial_baud, speed, read_timeout: Duration::from_millis(50) }
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn socketcan_is_unavailable_off_linux() {
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
{
|
||||
let err = open_socketcan("can0").expect_err("非 Linux 必须报错");
|
||||
assert_eq!(err.kind(), std::io::ErrorKind::Unsupported);
|
||||
}
|
||||
#[cfg(target_os = "linux")]
|
||||
{
|
||||
// Linux 上接口名不存在时也必须是 Err，而不是 panic。
|
||||
let _ = open_socketcan("definitely-not-a-can-interface");
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn slcan_failure_is_an_error_not_a_panic() {
|
||||
let result = open_slcan(
|
||||
"/definitely/not/a/device",
|
||||
default_slcan_options(2_000_000, CanSpeed::M1),
|
||||
);
|
||||
assert!(result.is_err());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_options_carry_the_requested_baud() {
|
||||
let options = default_slcan_options(115_200, CanSpeed::K500);
|
||||
assert_eq!(options.serial_baud, 115_200);
|
||||
assert_eq!(options.speed, CanSpeed::K500);
|
||||
assert_eq!(options.read_timeout, Duration::from_millis(50));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod safety {
|
||||
//! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。
|
||||
//!
|
||||
//! 台架工具（`chassis-spin`）与自动整定的真机后端（`chassis-tune --backend bench`）
|
||||
//! 共用同一套判定，避免安全逻辑在两个地方各写一遍、然后慢慢漂移。
|
||||
//!
|
||||
//! 所有阈值都是 `Option`：只开需要的那个。`chassis-spin` 只开过速保护
|
||||
//! （保持历史行为），自动整定则开启全部——因为搜索会主动试错，必须由
|
||||
//! 包络兜住"试错到失控"。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use motor::MotorFeedback;
|
||||
// 用 tokio 的时钟：`start_paused` 下虚拟时间可控，安全包络（宽限期 / 超时）
|
||||
// 才能被确定性地测试；生产构建里它就是 `std::time::Instant`。
|
||||
use thiserror::Error;
|
||||
use tokio::time::Instant;
|
||||
|
||||
/// 触发停机的原因。
|
||||
#[derive(Debug, Clone, PartialEq, Error)]
|
||||
pub enum Abort {
|
||||
/// 转速超过上限并持续超过宽限期。
|
||||
#[error("overspeed: {peak_rpm:.0} rpm > {limit_rpm:.0} rpm for over {grace_ms}ms")]
|
||||
Overspeed {
|
||||
/// 观测到的峰值转速（rpm）。
|
||||
peak_rpm: f32,
|
||||
/// 配置的上限（rpm）。
|
||||
limit_rpm: f32,
|
||||
/// 宽限期（毫秒）。
|
||||
grace_ms: u64,
|
||||
},
|
||||
/// 温度超过上限。
|
||||
#[error("overtemperature: {temperature_c}℃ > {limit_c}℃")]
|
||||
OverTemperature {
|
||||
/// 观测温度（℃）。
|
||||
temperature_c: i8,
|
||||
/// 上限（℃）。
|
||||
limit_c: i8,
|
||||
},
|
||||
/// 反馈超出新鲜度阈值（掉线）。
|
||||
#[error("esc {esc_id} feedback lost: age {age_ms:?}ms > {timeout_ms}ms")]
|
||||
FeedbackLost {
|
||||
/// 电调 ID。
|
||||
esc_id: u8,
|
||||
/// 实测反馈年龄（毫秒）。
|
||||
age_ms: Option<u128>,
|
||||
/// 阈值（毫秒）。
|
||||
timeout_ms: u64,
|
||||
},
|
||||
/// 整个试验超时。
|
||||
#[error("trial timeout after {elapsed_ms}ms")]
|
||||
Timeout {
|
||||
/// 已耗时（毫秒）。
|
||||
elapsed_ms: u128,
|
||||
},
|
||||
/// 上层主动取消（ctrl-c）。
|
||||
#[error("cancelled by operator")]
|
||||
Cancelled,
|
||||
}
|
||||
|
||||
/// 安全包络（阈值 + 宽限期）。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct SafetyEnvelope {
|
||||
/// 转速上限（rpm）。
|
||||
pub max_rpm: Option<f32>,
|
||||
/// 温度上限（℃）。
|
||||
pub max_temperature_c: Option<i8>,
|
||||
/// 反馈新鲜度阈值：超过它即视为掉线。
|
||||
pub feedback_timeout: Option<Duration>,
|
||||
/// 过速需要持续多久才判定为失控（抑制单帧噪声）。
|
||||
pub overspeed_grace: Duration,
|
||||
/// 单次试验的总时长上限。
|
||||
pub trial_timeout: Option<Duration>,
|
||||
}
|
||||
|
||||
impl Default for SafetyEnvelope {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
max_rpm: None,
|
||||
max_temperature_c: None,
|
||||
feedback_timeout: None,
|
||||
overspeed_grace: Duration::from_millis(60),
|
||||
trial_timeout: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl SafetyEnvelope {
|
||||
/// 空包络（不判定任何条件）。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 只开过速保护（`chassis-spin` 的历史行为）。
|
||||
#[must_use]
|
||||
pub fn overspeed_only(max_rpm: f32, grace: Duration) -> Self {
|
||||
Self { max_rpm: Some(max_rpm), overspeed_grace: grace, ..Self::default() }
|
||||
}
|
||||
|
||||
/// 开过速保护。
|
||||
#[must_use]
|
||||
pub fn with_overspeed(mut self, max_rpm: f32, grace: Duration) -> Self {
|
||||
self.max_rpm = Some(max_rpm);
|
||||
self.overspeed_grace = grace;
|
||||
self
|
||||
}
|
||||
|
||||
/// 开温度保护。
|
||||
#[must_use]
|
||||
pub fn with_temperature_max(mut self, max_temperature_c: i8) -> Self {
|
||||
self.max_temperature_c = Some(max_temperature_c);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开掉线保护。
|
||||
#[must_use]
|
||||
pub fn with_feedback_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.feedback_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 开试验总时长保护。
|
||||
#[must_use]
|
||||
pub fn with_trial_timeout(mut self, timeout: Duration) -> Self {
|
||||
self.trial_timeout = Some(timeout);
|
||||
self
|
||||
}
|
||||
|
||||
/// 是否需要任何判定（空包络时调用方可以跳过整段检查）。
|
||||
#[must_use]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.max_rpm.is_none()
|
||||
&& self.max_temperature_c.is_none()
|
||||
&& self.feedback_timeout.is_none()
|
||||
&& self.trial_timeout.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 带状态的安全监视器（过速宽限与总时长需要记忆）。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SafetyMonitor {
|
||||
envelope: SafetyEnvelope,
|
||||
started: Instant,
|
||||
overspeed_since: Option<Instant>,
|
||||
}
|
||||
|
||||
impl SafetyMonitor {
|
||||
/// 创建监视器并记录起点。
|
||||
#[must_use]
|
||||
pub fn new(envelope: SafetyEnvelope, now: Instant) -> Self {
|
||||
Self { envelope, started: now, overspeed_since: None }
|
||||
}
|
||||
|
||||
/// 当前包络。
|
||||
#[must_use]
|
||||
pub const fn envelope(&self) -> &SafetyEnvelope {
|
||||
&self.envelope
|
||||
}
|
||||
|
||||
/// 检查一组电机在当前时刻是否触碰包络。
|
||||
///
|
||||
/// 返回 `Some(Abort)` 即调用方**必须立刻停电流并结束本次试验**。
|
||||
/// 每次调用**不分配**（1 kHz 台架循环里每拍都要过一遍）。
|
||||
pub fn check<M: MotorFeedback>(&mut self, motors: &[M], now: Instant) -> Option<Abort> {
|
||||
if let Some(timeout) = self.envelope.trial_timeout {
|
||||
let elapsed = now.saturating_duration_since(self.started);
|
||||
if elapsed > timeout {
|
||||
return Some(Abort::Timeout { elapsed_ms: elapsed.as_millis() });
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(timeout) = self.envelope.feedback_timeout {
|
||||
for motor in motors {
|
||||
let age = motor.feedback_age();
|
||||
if age.is_none_or(|age| age > timeout) {
|
||||
return Some(Abort::FeedbackLost {
|
||||
esc_id: motor.id(),
|
||||
age_ms: age.map(|age| age.as_millis()),
|
||||
timeout_ms: timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_temperature_c {
|
||||
for motor in motors {
|
||||
let temperature = motor.state().temperature_c;
|
||||
if temperature > limit {
|
||||
return Some(Abort::OverTemperature {
|
||||
temperature_c: temperature,
|
||||
limit_c: limit,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if let Some(limit) = self.envelope.max_rpm {
|
||||
let peak = motors
|
||||
.iter()
|
||||
.map(|motor| motor.state().rotor_speed_rpm.abs())
|
||||
.fold(0.0f32, f32::max);
|
||||
if peak > limit {
|
||||
match self.overspeed_since {
|
||||
None => self.overspeed_since = Some(now),
|
||||
Some(since)
|
||||
if now.saturating_duration_since(since)
|
||||
>= self.envelope.overspeed_grace =>
|
||||
{
|
||||
return Some(Abort::Overspeed {
|
||||
peak_rpm: peak,
|
||||
limit_rpm: limit,
|
||||
grace_ms: self.envelope.overspeed_grace.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Some(_) => {}
|
||||
}
|
||||
} else {
|
||||
self.overspeed_since = None;
|
||||
}
|
||||
}
|
||||
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor::{MockMotor, MotorState};
|
||||
|
||||
fn motor_with(id: u8, rpm: f32, temperature_c: i8, age: Option<Duration>) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
let state = MotorState {
|
||||
rotor_speed_rpm: rpm,
|
||||
temperature_c,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
};
|
||||
match age {
|
||||
Some(age) => motor.inject_state_with_age(state, age),
|
||||
None => motor.inject_state(state),
|
||||
}
|
||||
motor
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn empty_envelope_never_aborts() {
|
||||
let motors = vec![motor_with(1, 1.0e6, 120, Some(Duration::from_secs(5)))];
|
||||
let mut monitor = SafetyMonitor::new(SafetyEnvelope::new(), Instant::now());
|
||||
assert!(monitor.envelope().is_empty());
|
||||
assert_eq!(monitor.check(&motors, Instant::now()), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_needs_the_grace_period() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None, "宽限期内不得触发");
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(30)), None);
|
||||
let abort = monitor.check(&motors, now + grace);
|
||||
assert!(matches!(abort, Some(Abort::Overspeed { .. })), "{abort:?}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn overspeed_timer_resets_when_back_under_limit() {
|
||||
let motors = vec![motor_with(1, 5000.0, 30, None)];
|
||||
let grace = Duration::from_millis(60);
|
||||
let mut monitor =
|
||||
SafetyMonitor::new(SafetyEnvelope::overspeed_only(3000.0, grace), Instant::now());
|
||||
let now = Instant::now();
|
||||
assert_eq!(monitor.check(&motors, now), None);
|
||||
let calm = vec![motor_with(1, 100.0, 30, None)];
|
||||
assert_eq!(monitor.check(&calm, now + Duration::from_millis(30)), None);
|
||||
// 重新超速：计时必须从头开始。
|
||||
assert_eq!(monitor.check(&motors, now + Duration::from_millis(40)), None);
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(90)).is_none());
|
||||
assert!(monitor.check(&motors, now + Duration::from_millis(110)).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn temperature_and_feedback_loss_abort_immediately() {
|
||||
let envelope = SafetyEnvelope::new()
|
||||
.with_temperature_max(70)
|
||||
.with_feedback_timeout(Duration::from_millis(100));
|
||||
let hot = vec![motor_with(1, 0.0, 80, None)];
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&hot, Instant::now()),
|
||||
Some(Abort::OverTemperature { .. })
|
||||
));
|
||||
|
||||
let stale = vec![motor_with(1, 0.0, 30, Some(Duration::from_millis(250)))];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&stale, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 1, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn motor_without_any_feedback_is_lost() {
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
// 从未收到过反馈：age = None。
|
||||
let silent = vec![MockMotor::new(3)];
|
||||
let mut monitor = SafetyMonitor::new(envelope, Instant::now());
|
||||
assert!(matches!(
|
||||
monitor.check(&silent, Instant::now()),
|
||||
Some(Abort::FeedbackLost { esc_id: 3, age_ms: None, .. })
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn trial_timeout_aborts() {
|
||||
let envelope = SafetyEnvelope::new().with_trial_timeout(Duration::from_millis(500));
|
||||
let motors = vec![motor_with(1, 0.0, 30, None)];
|
||||
let start = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope, start);
|
||||
assert_eq!(monitor.check(&motors, start + Duration::from_millis(100)), None);
|
||||
let abort = monitor.check(&motors, start + Duration::from_millis(600));
|
||||
assert!(matches!(abort, Some(Abort::Timeout { .. })), "{abort:?}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod sampler {
|
||||
//! 真机试验：把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。
|
||||
//!
|
||||
//! 与 `sim-harness` 的离线 harness 是**同一套场景词汇**：整定工具先用离线
|
||||
//! harness 搜索，再用本模块在台架上复核，两条路径的指标口径因此一致。
|
||||
//!
|
||||
//! # 安全契约
|
||||
//!
|
||||
//! - 每个采样拍都重发设定值——**命令是租约**，不重发会在 50ms 后失电；
|
||||
//! - 每个采样拍都过一遍 [`SafetyMonitor`]；
|
||||
//! - 无论正常结束、触发包络还是被取消，退出前一律 `disable()`（0 电流）。
|
||||
|
||||
use std::future::Future;
|
||||
use std::time::Duration;
|
||||
|
||||
use experiment::{Scenario, Trace};
|
||||
use motor::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval};
|
||||
|
||||
use crate::safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
|
||||
/// 一次真机试验的结果。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TrialOutcome {
|
||||
/// 采集到的轨迹（触发中止时只含中止前的采样）。
|
||||
pub trace: Trace,
|
||||
/// 触发安全包络 / 被取消的原因；`None` 表示按场景跑完。
|
||||
pub abort: Option<Abort>,
|
||||
}
|
||||
|
||||
impl TrialOutcome {
|
||||
/// 是否完整跑完场景（没有被安全包络打断）。
|
||||
#[must_use]
|
||||
pub fn completed(&self) -> bool {
|
||||
self.abort.is_none()
|
||||
}
|
||||
}
|
||||
|
||||
/// 采样节拍下限（防止 `dt_s` 给出病态值把 CPU 打满）。
|
||||
const MIN_TICK: Duration = Duration::from_micros(50);
|
||||
|
||||
/// 在单台电机上重放一条场景。
|
||||
///
|
||||
/// - `effort_limit_raw`：本次试运行的 effort 限幅，仅用于标注 `saturated`
|
||||
/// （控制器真正的钳位在驱动层按 `EscConfig::current_limit` 完成）；
|
||||
/// - `lease_period`：命令租约的续期节拍（比 `scenario.dt_s` 更稀时以采样节拍为准）；
|
||||
/// - `cancel`：外部取消源（ctrl-c / 搜索预算耗尽）。
|
||||
pub async fn run_step_trial<M, F>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
cancel: F,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
F: Future<Output = ()>,
|
||||
{
|
||||
let dt = Duration::from_secs_f32(scenario.dt_s.max(1.0e-6)).max(MIN_TICK);
|
||||
let total = Duration::from_secs_f32(scenario.total_s().max(0.0));
|
||||
// 续租节拍不得稀于采样节拍，否则会短暂失电。
|
||||
let lease_period = lease_period.min(dt);
|
||||
|
||||
let started = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope.clone(), started);
|
||||
let mut ticker = interval(dt);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut lease = interval(lease_period);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
let mut trace = Trace::with_capacity(scenario.samples());
|
||||
let mut setpoint = scenario.setpoint_at(0.0);
|
||||
let mut abort = None;
|
||||
tokio::pin!(cancel);
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
() = &mut cancel => {
|
||||
abort = Some(Abort::Cancelled);
|
||||
break;
|
||||
}
|
||||
_ = lease.tick() => {
|
||||
// 续租：命令是租约，不重发就失电。
|
||||
motor.set_velocity(setpoint);
|
||||
}
|
||||
_ = ticker.tick() => {
|
||||
let now = Instant::now();
|
||||
let elapsed = now.saturating_duration_since(started);
|
||||
if elapsed > total {
|
||||
break;
|
||||
}
|
||||
let t_s = elapsed.as_secs_f32();
|
||||
setpoint = scenario.setpoint_at(t_s);
|
||||
motor.set_velocity(setpoint);
|
||||
|
||||
let state = motor.state();
|
||||
let saturated =
|
||||
i32::from(state.effort_raw).abs() >= i32::from(effort_limit_raw);
|
||||
trace.push(t_s, setpoint, state.rotor_speed_rpm, state.effort_raw, saturated);
|
||||
|
||||
if let Some(reason) = monitor.check(std::slice::from_ref(motor), now) {
|
||||
abort = Some(reason);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 安全收尾：任何退出路径都先归零，绝不把非零给定留在总线上。
|
||||
motor.disable();
|
||||
TrialOutcome { trace, abort }
|
||||
}
|
||||
|
||||
/// 以 ctrl-c 作为取消源的 [`run_step_trial`] 包装。
|
||||
pub async fn run_step_trial_until_ctrl_c<M>(
|
||||
motor: &M,
|
||||
scenario: &Scenario,
|
||||
envelope: &SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
lease_period: Duration,
|
||||
) -> TrialOutcome
|
||||
where
|
||||
M: VelocityDrive + MotorFeedback + Disable,
|
||||
{
|
||||
run_step_trial(motor, scenario, envelope, effort_limit_raw, lease_period, async {
|
||||
let _ = tokio::signal::ctrl_c().await;
|
||||
})
|
||||
.await
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor::{Command, MockMotor, MotorState};
|
||||
|
||||
fn ready_motor(id: u8) -> MockMotor {
|
||||
let motor = MockMotor::new(id);
|
||||
motor.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
|
||||
motor
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn trial_follows_scenario_and_disables_at_the_end() {
|
||||
let motor = ready_motor(1);
|
||||
let scenario = Scenario::ramp_and_hold(1000.0, 0.1, 0.2);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(outcome.completed(), "{:?}", outcome.abort);
|
||||
assert!(outcome.trace.is_consistent());
|
||||
assert!(outcome.trace.len() > 250, "len = {}", outcome.trace.len());
|
||||
// 斜坡中点的设定值应约为一半。
|
||||
let mid = outcome.trace.window(0.049, 0.052).setpoint_rpm()[0];
|
||||
assert!((mid - 500.0).abs() < 30.0, "mid setpoint = {mid}");
|
||||
// 收尾必须归零。
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn overspeed_aborts_and_zeroes_output() {
|
||||
let motor = ready_motor(1);
|
||||
// 反馈恒为 5000 rpm，远超 3000 rpm 上限。
|
||||
motor.inject_state(MotorState {
|
||||
rotor_speed_rpm: 5000.0,
|
||||
feedback_count: 1,
|
||||
..MotorState::default()
|
||||
});
|
||||
let envelope = SafetyEnvelope::overspeed_only(3000.0, Duration::from_millis(20));
|
||||
let scenario = Scenario::step(1000.0, 2.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(matches!(outcome.abort, Some(Abort::Overspeed { .. })), "{:?}", outcome.abort);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
// 中止得足够早，不该跑满场景。
|
||||
assert!(outcome.trace.len() < 200, "len = {}", outcome.trace.len());
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn stalled_feedback_aborts() {
|
||||
let motor = MockMotor::new(4);
|
||||
motor.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(500),
|
||||
);
|
||||
let envelope = SafetyEnvelope::new().with_feedback_timeout(Duration::from_millis(100));
|
||||
let scenario = Scenario::step(500.0, 1.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
std::future::pending::<()>(),
|
||||
)
|
||||
.await;
|
||||
|
||||
assert!(
|
||||
matches!(outcome.abort, Some(Abort::FeedbackLost { esc_id: 4, .. })),
|
||||
"{:?}",
|
||||
outcome.abort
|
||||
);
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn cancellation_stops_and_zeroes_output() {
|
||||
let motor = ready_motor(2);
|
||||
let scenario = Scenario::step(800.0, 5.0);
|
||||
let outcome = run_step_trial(
|
||||
&motor,
|
||||
&scenario,
|
||||
&SafetyEnvelope::new(),
|
||||
12_000,
|
||||
Duration::from_millis(5),
|
||||
async { tokio::time::sleep(Duration::from_millis(50)).await },
|
||||
)
|
||||
.await;
|
||||
|
||||
assert_eq!(outcome.abort, Some(Abort::Cancelled));
|
||||
assert_eq!(motor.last_command(), Command::Disabled);
|
||||
assert!(!outcome.trace.is_empty());
|
||||
assert!(outcome.trace.len() < 200, "取消后不得继续采样");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod hil {
|
||||
//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
|
||||
//!
|
||||
//! 与 [`SimEvaluator`](crate::SimEvaluator) 实现同一个 [`Evaluator`]，
|
||||
//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
|
||||
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
|
||||
//!
|
||||
//! # 安全设计
|
||||
//!
|
||||
//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
|
||||
//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
|
||||
//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
|
||||
//!
|
||||
//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
|
||||
//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。
|
||||
|
||||
use std::sync::Mutex;
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{SafetyEnvelope, run_step_trial};
|
||||
use anyhow::Context as _;
|
||||
use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use experiment::Scenario;
|
||||
use motor::{Disable as _, MotorFeedback};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::sampler::TrialOutcome;
|
||||
|
||||
/// 真机后端错误。
|
||||
#[derive(Debug, Error)]
|
||||
pub enum BenchError {
|
||||
/// 无法打开总线。
|
||||
#[error("cannot open CAN link: {0}")]
|
||||
Link(String),
|
||||
/// 电调总线启动失败。
|
||||
#[error("cannot start ESC bus: {0}")]
|
||||
Bus(String),
|
||||
/// 启动超时：目标电调没有反馈。
|
||||
#[error(
|
||||
"esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)"
|
||||
)]
|
||||
NoFeedback {
|
||||
/// 目标电调 ID。
|
||||
esc_id: u8,
|
||||
/// 等待时长（毫秒）。
|
||||
waited_ms: u64,
|
||||
},
|
||||
/// 搜索期间因安全事件中止且未要求继续。
|
||||
#[error("aborted by safety envelope: {reason}")]
|
||||
Aborted {
|
||||
/// 触发原因。
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
/// 真机评估配置。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BenchConfig {
|
||||
/// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
|
||||
pub esc: EscConfig,
|
||||
/// 总线运行参数。
|
||||
pub bus: BusOptions,
|
||||
/// 评估场景。
|
||||
pub scenario: Scenario,
|
||||
/// 安全包络。
|
||||
pub envelope: SafetyEnvelope,
|
||||
/// 试验之间的静默期（等轮子停稳）。
|
||||
pub between_trials: Duration,
|
||||
/// 命令租约续期节拍。
|
||||
pub lease_period: Duration,
|
||||
/// 启动时等待反馈的超时。
|
||||
pub startup_timeout: Duration,
|
||||
}
|
||||
|
||||
impl BenchConfig {
|
||||
/// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
|
||||
#[must_use]
|
||||
pub fn new(esc: EscConfig, scenario: Scenario, envelope: SafetyEnvelope) -> Self {
|
||||
Self {
|
||||
esc,
|
||||
bus: BusOptions::default(),
|
||||
scenario,
|
||||
envelope,
|
||||
between_trials: Duration::from_millis(500),
|
||||
lease_period: Duration::from_millis(5),
|
||||
startup_timeout: Duration::from_secs(5),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 真机评估器（持有总线与运行时）。
|
||||
pub struct BenchEvaluator {
|
||||
session: Mutex<Session>,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
effort_limit_raw: i16,
|
||||
between_trials: Duration,
|
||||
lease_period: Duration,
|
||||
}
|
||||
|
||||
struct Session {
|
||||
runtime: tokio::runtime::Runtime,
|
||||
bus: Option<EscBusHandle<MotorPolicy>>,
|
||||
motor: EscMotor,
|
||||
}
|
||||
|
||||
impl BenchEvaluator {
|
||||
/// 打开链路、启动总线并等待目标电调反馈。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路 / 总线启动失败，或启动超时（电调无反馈）。
|
||||
pub fn open(link: can::CanLink, config: BenchConfig) -> Result<Self, BenchError> {
|
||||
let esc_id = config.esc.esc_id;
|
||||
let effort_limit_raw = config.esc.current_limit;
|
||||
let runtime = tokio::runtime::Builder::new_multi_thread()
|
||||
.worker_threads(2)
|
||||
.enable_all()
|
||||
.build()
|
||||
.map_err(|e| BenchError::Bus(e.to_string()))?;
|
||||
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(config.esc.esc_id, config.esc.current_limit, VelocityGains::ZERO);
|
||||
let bus = EscBus::spawn(link, vec![config.esc.clone()], config.bus.clone(), policy)
|
||||
.map_err(|e| BenchError::Bus(e.to_string()))?;
|
||||
let motor = bus
|
||||
.motor(esc_id)
|
||||
.ok_or_else(|| BenchError::Bus(format!("esc {esc_id} missing from the bus")))?
|
||||
.clone();
|
||||
|
||||
let startup_timeout = config.startup_timeout;
|
||||
let ready = runtime.block_on(async {
|
||||
let deadline = tokio::time::Instant::now() + startup_timeout;
|
||||
loop {
|
||||
if motor.feedback_age().is_some_and(|age| age <= Duration::from_millis(100)) {
|
||||
return true;
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
return false;
|
||||
}
|
||||
tokio::time::sleep(Duration::from_millis(20)).await;
|
||||
}
|
||||
});
|
||||
let evaluator = Self {
|
||||
session: Mutex::new(Session { runtime, bus: Some(bus), motor }),
|
||||
scenario: config.scenario,
|
||||
envelope: config.envelope,
|
||||
effort_limit_raw,
|
||||
between_trials: config.between_trials,
|
||||
lease_period: config.lease_period,
|
||||
};
|
||||
if !ready {
|
||||
// 启动失败也必须走完整停机路径：先 0 电流，再关总线。
|
||||
evaluator.shutdown();
|
||||
return Err(BenchError::NoFeedback {
|
||||
esc_id,
|
||||
waited_ms: startup_timeout.as_millis() as u64,
|
||||
});
|
||||
}
|
||||
Ok(evaluator)
|
||||
}
|
||||
|
||||
/// Scenario executed by each trial.
|
||||
#[must_use]
|
||||
pub fn scenario(&self) -> Scenario {
|
||||
self.scenario
|
||||
}
|
||||
|
||||
/// Stop outputs and close the bus.
|
||||
pub fn shutdown(&self) {
|
||||
let mut session =
|
||||
self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
let Some(bus) = session.bus.take() else {
|
||||
return;
|
||||
};
|
||||
for motor in bus.motors() {
|
||||
motor.disable();
|
||||
}
|
||||
session.runtime.block_on(async {
|
||||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||||
bus.shutdown().await;
|
||||
});
|
||||
}
|
||||
|
||||
/// Run one physical trial and return its raw trace and abort reason.
|
||||
pub fn run_trial(&self, gains: &VelocityGains) -> TrialOutcome {
|
||||
let mut session =
|
||||
self.session.lock().unwrap_or_else(std::sync::PoisonError::into_inner);
|
||||
let Session { runtime, motor, bus, .. } = &mut *session;
|
||||
let esc_id = motor.id();
|
||||
if let Some(bus) = bus.as_ref() {
|
||||
bus.with_policy(|policy| policy.set_velocity_gains(esc_id, *gains));
|
||||
}
|
||||
let scenario = self.scenario;
|
||||
let envelope = self.envelope.clone();
|
||||
let limit = self.effort_limit_raw;
|
||||
let lease = self.lease_period;
|
||||
let between = self.between_trials;
|
||||
|
||||
runtime.block_on(async {
|
||||
let trial = run_step_trial(
|
||||
motor,
|
||||
&scenario,
|
||||
&envelope,
|
||||
limit,
|
||||
lease,
|
||||
std::future::pending(),
|
||||
)
|
||||
.await;
|
||||
tokio::time::sleep(between).await;
|
||||
trial
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开真机链路：给了 `interface` 走 SocketCAN，否则走 slcan 串口。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备 / 接口打开失败（路径、权限、接口未 up、平台不支持）时返回错误。
|
||||
pub fn open_link(
|
||||
device: &str,
|
||||
serial_baud: u32,
|
||||
can_speed: crate::CanSpeed,
|
||||
interface: Option<&str>,
|
||||
) -> Result<can::CanLink, BenchError> {
|
||||
let result = match interface {
|
||||
Some(interface) => crate::open_socketcan(interface),
|
||||
None => crate::open_slcan(device, crate::default_slcan_options(serial_baud, can_speed)),
|
||||
};
|
||||
result
|
||||
.with_context(|| match interface {
|
||||
Some(interface) => {
|
||||
format!("打开 SocketCAN 接口 {interface} 失败（ip link / 权限？）")
|
||||
}
|
||||
None => format!("打开 slcan 设备 {device} 失败（路径 / 权限 / 占用？）"),
|
||||
})
|
||||
.map_err(|e| BenchError::Link(e.to_string()))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
fn config() -> BenchConfig {
|
||||
let mut esc = EscConfig::gm3508("bench", 1, 1.0);
|
||||
esc.current_limit = 1500;
|
||||
let scenario = Scenario::ramp_and_hold(500.0, 0.05, 0.15);
|
||||
let envelope = SafetyEnvelope::new()
|
||||
.with_overspeed(1200.0, Duration::from_millis(60))
|
||||
.with_temperature_max(70)
|
||||
.with_feedback_timeout(Duration::from_millis(200))
|
||||
.with_trial_timeout(Duration::from_secs(2));
|
||||
BenchConfig::new(esc, scenario, envelope)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn config_defaults_are_safe() {
|
||||
let config = config();
|
||||
assert_eq!(config.bus.period, Duration::from_millis(1));
|
||||
assert_eq!(config.bus.command_timeout, Duration::from_millis(50));
|
||||
assert!(config.between_trials >= Duration::from_millis(200));
|
||||
assert_eq!(config.lease_period, Duration::from_millis(5), "续租必须远密于 50ms 租约");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn link_open_failures_are_errors() {
|
||||
// 不存在的设备：必须返回 Err 而不是 panic（真机前最常见的失败模式）。
|
||||
let result =
|
||||
open_link("/definitely/not/a/device", 2_000_000, crate::CanSpeed::M1, None);
|
||||
assert!(result.is_err(), "不存在的串口设备必须报错");
|
||||
let missing_interface =
|
||||
open_link("ignored", 2_000_000, crate::CanSpeed::M1, Some("can-not-real"));
|
||||
assert!(missing_interface.is_err(), "不存在的 CAN 接口必须报错");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use hil::*;
|
||||
pub use link::*;
|
||||
pub use safety::*;
|
||||
pub use sampler::*;
|
||||
@@ -1,75 +0,0 @@
|
||||
//! CAN 传输抽象：帧与链路通道。
|
||||
//!
|
||||
//! 本 crate **不含任何协议知识**（无 DJI ID 常量）：只定义经典 CAN 数据帧
|
||||
//! 与一对 tokio 通道构成的链路端点。协议族（`dji-esc` / 未来 `dji-gm6020`）
|
||||
//! 与总线后端（SocketCAN 桥、仿真器）共享此类型，互不依赖。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
use tokio::sync::mpsc;
|
||||
|
||||
/// 经典 CAN 数据帧（数据长度固定 8 字节）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub struct CanFrame {
|
||||
/// 11 位标准 ID。
|
||||
pub id: u32,
|
||||
/// 数据域。
|
||||
pub data: [u8; 8],
|
||||
}
|
||||
|
||||
impl CanFrame {
|
||||
/// 构造一帧。
|
||||
#[must_use]
|
||||
pub const fn new(id: u32, data: [u8; 8]) -> Self {
|
||||
Self { id, data }
|
||||
}
|
||||
}
|
||||
|
||||
/// CAN 链路端点。
|
||||
///
|
||||
/// - `outgoing_tx`：本端发往总线的帧；
|
||||
/// - `incoming_rx`：本端从总线收到的帧。
|
||||
///
|
||||
/// 一条链路由两个端点构成（见 [`CanLink::pair`]）。持有本端、把对端交给
|
||||
/// 对侧（SocketCAN 桥、仿真器）即可通信。
|
||||
#[derive(Debug)]
|
||||
pub struct CanLink {
|
||||
/// 发往总线。
|
||||
pub outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
/// 来自总线。
|
||||
pub incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
}
|
||||
|
||||
impl CanLink {
|
||||
/// 由已有通道组装端点。
|
||||
#[must_use]
|
||||
pub const fn new(
|
||||
outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
) -> Self {
|
||||
Self { outgoing_tx, incoming_rx }
|
||||
}
|
||||
|
||||
/// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。
|
||||
///
|
||||
/// `bound` 为每方向缓冲帧数（建议 ≥ 64，1kHz 收发下留足余量）。
|
||||
#[must_use]
|
||||
pub fn pair(bound: usize) -> (Self, Self) {
|
||||
let (tx_a_to_b, rx_at_b) = mpsc::channel(bound);
|
||||
let (tx_b_to_a, rx_at_a) = mpsc::channel(bound);
|
||||
(Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn pair_loops_both_directions() {
|
||||
let (mut a, mut b) = CanLink::pair(8);
|
||||
a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200);
|
||||
assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
[package]
|
||||
name = "can"
|
||||
description = "CAN frame, bus interface, channel link, and test bus"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
mock = []
|
||||
|
||||
[dependencies]
|
||||
tokio = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,463 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod frame {
|
||||
//! CAN 抽象：经典数据帧与**总线 trait**。
|
||||
//!
|
||||
//! [`CanFrame`] 是全系统唯一的帧表示（协议层、驱动层、仿真层、设备层共享），
|
||||
//! 它**不含任何协议知识**（没有 DJI 帧 ID 常量）。字段与 ARCHITECTURE.md
|
||||
//! §5.1 一致：`id` / `data` / `dlc` / `is_extended`。
|
||||
//!
|
||||
//! [`CanBus`] 是 ARCHITECTURE.md §5.1 定义的硬件访问边界；`driver crates` 为
|
||||
//! 真实总线实现它，`can::mock` 为假电调实现它，
|
||||
//! [`CanLink`](crate::link::CanLink) 是它的通道式实现。
|
||||
|
||||
/// 经典 CAN 数据帧数据域长度（字节）。
|
||||
pub const CAN_DATA_LEN: usize = 8;
|
||||
|
||||
/// 经典 CAN 数据帧。
|
||||
///
|
||||
/// 当前所有设备（C620/C610、GM6020、DETA10）都用「11 位标准 ID + 8 字节」，
|
||||
/// 但字段保留完整形态：将来接扩展帧或短帧时不必改动所有构造点。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub struct CanFrame {
|
||||
/// 标准 11 位 / 扩展 29 位 ID。
|
||||
pub id: u32,
|
||||
/// 数据域（未使用的字节为 0）。
|
||||
pub data: [u8; CAN_DATA_LEN],
|
||||
/// 数据长度码（0..=8）。
|
||||
pub dlc: u8,
|
||||
/// 是否扩展帧（29 位 ID）。
|
||||
pub is_extended: bool,
|
||||
}
|
||||
|
||||
impl CanFrame {
|
||||
/// 构造一帧标准帧：8 字节数据、`dlc = 8`。
|
||||
///
|
||||
/// 这是仓库内**唯一**的常规构造方式；`dlc` / 扩展帧只在特殊场景用
|
||||
/// [`CanFrame::with_dlc`] / [`CanFrame::extended`] 调整。
|
||||
#[must_use]
|
||||
pub const fn new(id: u32, data: [u8; CAN_DATA_LEN]) -> Self {
|
||||
Self { id, data, dlc: CAN_DATA_LEN as u8, is_extended: false }
|
||||
}
|
||||
|
||||
/// 换一个数据长度码（链式）。
|
||||
#[must_use]
|
||||
pub const fn with_dlc(mut self, dlc: u8) -> Self {
|
||||
self.dlc = if dlc > CAN_DATA_LEN as u8 { CAN_DATA_LEN as u8 } else { dlc };
|
||||
self
|
||||
}
|
||||
|
||||
/// 标记为扩展帧（链式）。
|
||||
#[must_use]
|
||||
pub const fn extended(mut self) -> Self {
|
||||
self.is_extended = true;
|
||||
self
|
||||
}
|
||||
|
||||
/// 长度码是否覆盖完整数据域。
|
||||
#[must_use]
|
||||
pub const fn is_complete(&self) -> bool {
|
||||
self.dlc == CAN_DATA_LEN as u8
|
||||
}
|
||||
|
||||
/// ID 是否在 11 位标准帧范围内。
|
||||
#[must_use]
|
||||
pub const fn is_standard_id(&self) -> bool {
|
||||
self.id <= 0x7FF
|
||||
}
|
||||
}
|
||||
|
||||
/// CAN 总线访问错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum CanError {
|
||||
/// 链路已关闭（对端释放）。
|
||||
Closed,
|
||||
/// ID 与帧类型不匹配（如标准帧却给了 29 位 ID）。
|
||||
BadId(u32),
|
||||
/// 数据长度码非法（> 8）。
|
||||
BadDlc(u8),
|
||||
/// 底层收发失败（驱动自行补充上下文）。
|
||||
Io,
|
||||
}
|
||||
|
||||
impl core::fmt::Display for CanError {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
match self {
|
||||
Self::Closed => write!(f, "CAN 链路已关闭"),
|
||||
Self::BadId(id) => write!(f, "CAN ID {id:#x} 与帧类型不匹配"),
|
||||
Self::BadDlc(dlc) => write!(f, "CAN 数据长度码 {dlc} 非法（应 ≤ 8）"),
|
||||
Self::Io => write!(f, "CAN 收发失败"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for CanError {}
|
||||
|
||||
/// 硬件无关的 CAN 总线边界（ARCHITECTURE.md §5.1）。
|
||||
///
|
||||
/// 只描述"发一帧 / 收一帧 / 非阻塞收一帧"，**不规定**底层是 SocketCAN、
|
||||
/// 串口 slcan、内存通道还是假电调。`async fn in trait` 是本仓库对
|
||||
/// ARCHITECTURE.md §3.2 的直接采纳（Rust 1.75+ 稳定）。
|
||||
///
|
||||
/// # 关于非阻塞
|
||||
///
|
||||
/// 控制循环严格要求"读反馈不阻塞"（§6.3 第 2–5 步必须非阻塞）。
|
||||
/// 通道式实现天然支持 [`CanBus::try_recv`]；对不支持的系统调用，
|
||||
/// 默认实现返回 `Ok(None)` 而不是报错，让上层按"本轮没新帧"处理。
|
||||
pub trait CanBus {
|
||||
/// 底层错误类型。
|
||||
type Error: core::error::Error + Send + Sync + 'static;
|
||||
|
||||
/// 发送一帧。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭、帧类型非法或底层写失败时返回错误。
|
||||
fn send(
|
||||
&mut self,
|
||||
frame: &CanFrame,
|
||||
) -> impl core::future::Future<Output = Result<(), Self::Error>>;
|
||||
|
||||
/// 接收一帧（阻塞直到有帧或链路关闭）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭或底层读失败时返回错误。
|
||||
fn recv(&mut self) -> impl core::future::Future<Output = Result<CanFrame, Self::Error>>;
|
||||
|
||||
/// 非阻塞接收：本轮没有新帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 仅当链路**确定**已关闭 / 损坏时返回错误；"暂时没数据"不是错误。
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
|
||||
/// 帧是否符合"标准 ID + 完整 8 字节"的常规约定。
|
||||
#[must_use]
|
||||
pub const fn is_regular_standard_frame(frame: &CanFrame) -> bool {
|
||||
frame.is_standard_id() && !frame.is_extended && frame.is_complete()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn new_defaults_to_a_complete_standard_frame() {
|
||||
let frame = CanFrame::new(0x200, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(frame.dlc, 8);
|
||||
assert!(!frame.is_extended);
|
||||
assert!(frame.is_complete());
|
||||
assert!(is_regular_standard_frame(&frame));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn builders_are_chainable_and_clamp_dlc() {
|
||||
let short = CanFrame::new(0x201, [0; 8]).with_dlc(4);
|
||||
assert_eq!(short.dlc, 4);
|
||||
assert!(!short.is_complete());
|
||||
assert!(!is_regular_standard_frame(&short));
|
||||
|
||||
let over = CanFrame::new(0x201, [0; 8]).with_dlc(9);
|
||||
assert_eq!(over.dlc, 8, "dlc 必须被夹到数据域长度");
|
||||
|
||||
let extended = CanFrame::new(0x1_2345, [0; 8]).extended();
|
||||
assert!(extended.is_extended);
|
||||
assert!(!is_regular_standard_frame(&extended));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn standard_id_boundary() {
|
||||
assert!(CanFrame::new(0x7FF, [0; 8]).is_standard_id());
|
||||
assert!(!CanFrame::new(0x800, [0; 8]).is_standard_id());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn errors_render_in_chinese() {
|
||||
let text = CanError::BadId(0x1234).to_string();
|
||||
assert!(text.contains("0x1234"), "{text}");
|
||||
assert!(CanError::BadDlc(9).to_string().contains('9'));
|
||||
assert!(!CanError::Closed.to_string().is_empty());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use frame::*;
|
||||
|
||||
pub mod link {
|
||||
//! 通道式总线端点：[`CanLink`] 与它对 [`CanBus`] 的实现。
|
||||
//!
|
||||
//! 一对 tokio 通道构成一条链路：本端发出的帧由对端收到，反之亦然。
|
||||
//! 这是本仓库"**帧传输的唯一接缝**"——协议层与设备层只面向通道工作，
|
||||
//! 因此同一套协议 / 控制 / 整定代码可以对接真实总线（`driver crates` 的
|
||||
//! SocketCAN / slcan 桥）或纯内存假电调（`can::mock`），
|
||||
//! 无需任何条件编译。
|
||||
|
||||
use tokio::sync::mpsc;
|
||||
|
||||
use crate::frame::{CanBus, CanError, CanFrame};
|
||||
|
||||
/// CAN 链路的**一端**。
|
||||
///
|
||||
/// - `outgoing_tx`：本端发往总线的帧；
|
||||
/// - `incoming_rx`：本端从总线收到的帧。
|
||||
#[derive(Debug)]
|
||||
pub struct CanLink {
|
||||
/// 发往总线。
|
||||
pub outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
/// 来自总线。
|
||||
pub incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
}
|
||||
|
||||
impl CanLink {
|
||||
/// 由已有通道组装端点。
|
||||
#[must_use]
|
||||
pub const fn new(
|
||||
outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
) -> Self {
|
||||
Self { outgoing_tx, incoming_rx }
|
||||
}
|
||||
|
||||
/// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。
|
||||
///
|
||||
/// `bound` 为每方向缓冲帧数。命令方向必须**浅**（1kHz 生产、总线消费
|
||||
/// 不过来时靠 `send().await` 回压，见 `slcan` 的说明）；
|
||||
/// 反馈方向是采样流，可以深一些。
|
||||
#[must_use]
|
||||
pub fn pair(bound: usize) -> (Self, Self) {
|
||||
let (tx_a_to_b, rx_at_b) = mpsc::channel(bound);
|
||||
let (tx_b_to_a, rx_at_a) = mpsc::channel(bound);
|
||||
(Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b))
|
||||
}
|
||||
}
|
||||
|
||||
impl CanBus for CanLink {
|
||||
type Error = CanError;
|
||||
|
||||
async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> {
|
||||
if frame.dlc > 8 {
|
||||
return Err(CanError::BadDlc(frame.dlc));
|
||||
}
|
||||
self.outgoing_tx.send(*frame).await.map_err(|_| CanError::Closed)
|
||||
}
|
||||
|
||||
async fn recv(&mut self) -> Result<CanFrame, Self::Error> {
|
||||
self.incoming_rx.recv().await.ok_or(CanError::Closed)
|
||||
}
|
||||
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
match self.incoming_rx.try_recv() {
|
||||
Ok(frame) => Ok(Some(frame)),
|
||||
Err(mpsc::error::TryRecvError::Empty) => Ok(None),
|
||||
Err(mpsc::error::TryRecvError::Disconnected) => Err(CanError::Closed),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn pair_loops_both_directions() {
|
||||
let (mut a, mut b) = CanLink::pair(8);
|
||||
a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap();
|
||||
assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200);
|
||||
assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn can_bus_impl_sends_receives_and_polls() {
|
||||
let (mut a, mut b) = CanLink::pair(8);
|
||||
assert_eq!(a.try_recv().unwrap(), None, "空通道不是错误");
|
||||
a.send(&CanFrame::new(0x1FF, [7; 8])).await.unwrap();
|
||||
assert_eq!(b.try_recv().unwrap().map(|f| f.id), Some(0x1FF));
|
||||
assert_eq!(b.try_recv().unwrap(), None, "取走之后就没有了");
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn recv_reports_closed_link() {
|
||||
let (mut a, b) = CanLink::pair(8);
|
||||
drop(b);
|
||||
assert_eq!(a.recv().await.unwrap_err(), CanError::Closed);
|
||||
assert_eq!(a.try_recv().unwrap_err(), CanError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn send_rejects_illegal_dlc() {
|
||||
let (mut a, _b) = CanLink::pair(8);
|
||||
let bad = CanFrame::new(0x200, [0; 8]).with_dlc(8).with_dlc(8);
|
||||
assert!(a.send(&bad).await.is_ok());
|
||||
// `with_dlc` 本身会夹住越界值，因此非法 dlc 只能由手工构造的帧产生。
|
||||
let mut manual = CanFrame::new(0x200, [0; 8]);
|
||||
manual.dlc = 9;
|
||||
assert_eq!(a.send(&manual).await.unwrap_err(), CanError::BadDlc(9));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use link::*;
|
||||
|
||||
pub mod mock {
|
||||
//! 驱动级测试替身：[`MockBus`]（`mock` feature）。
|
||||
//!
|
||||
//! **这里没有电机模型**——那是 `sim-harness` 的事（`sim-harness::rig` 的异步假电调带
|
||||
//! 摩擦 / 延迟物理模型）。本模块只做驱动层该做的事：
|
||||
//!
|
||||
//! - 记录所有**发出去**的帧（断言"上层到底写了什么"）；
|
||||
//! - 按需**注入**要收到的帧（模拟总线上的反馈）；
|
||||
//! - 可以设置成"链路已断开"，用于测试离线 / 安全路径。
|
||||
//!
|
||||
//! 有了它，协议层与设备层的测试不必启动 tokio 任务，也不必构造真实总线。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use crate::frame::{CanBus, CanError, CanFrame};
|
||||
|
||||
/// 记录发送、可注入接收的假总线。
|
||||
#[derive(Debug, Default)]
|
||||
pub struct MockBus {
|
||||
/// 上层通过 [`CanBus::send`] 发出的帧（按时间顺序）。
|
||||
sent: Vec<CanFrame>,
|
||||
/// 等待被上层收到的帧。
|
||||
pending: VecDeque<CanFrame>,
|
||||
/// 置位后所有操作都返回 [`CanError::Closed`]。
|
||||
closed: bool,
|
||||
}
|
||||
|
||||
impl MockBus {
|
||||
/// 空总线。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 注入一帧"来自总线"的数据。
|
||||
pub fn inject(&mut self, frame: CanFrame) {
|
||||
self.pending.push_back(frame);
|
||||
}
|
||||
|
||||
/// 模拟链路断开。
|
||||
pub fn close(&mut self) {
|
||||
self.closed = true;
|
||||
}
|
||||
|
||||
/// 已发出的帧。
|
||||
#[must_use]
|
||||
pub fn sent(&self) -> &[CanFrame] {
|
||||
&self.sent
|
||||
}
|
||||
|
||||
/// 最近发出的一帧。
|
||||
#[must_use]
|
||||
pub fn last_sent(&self) -> Option<&CanFrame> {
|
||||
self.sent.last()
|
||||
}
|
||||
|
||||
/// 清空发送记录。
|
||||
pub fn clear_sent(&mut self) {
|
||||
self.sent.clear();
|
||||
}
|
||||
|
||||
/// 还剩多少帧没被取走。
|
||||
#[must_use]
|
||||
pub fn pending_len(&self) -> usize {
|
||||
self.pending.len()
|
||||
}
|
||||
}
|
||||
|
||||
impl CanBus for MockBus {
|
||||
type Error = CanError;
|
||||
|
||||
// 这里用 `std::future::ready` 而不是 `async fn`：内存总线没有真正的等待，
|
||||
// 返回一个已完成的 future 更诚实（也避免"async 函数里没有 await"的告警）。
|
||||
fn send(
|
||||
&mut self,
|
||||
frame: &CanFrame,
|
||||
) -> impl std::future::Future<Output = Result<(), Self::Error>> {
|
||||
let result = if self.closed {
|
||||
Err(CanError::Closed)
|
||||
} else if frame.dlc > 8 {
|
||||
Err(CanError::BadDlc(frame.dlc))
|
||||
} else if !frame.is_standard_id() && !frame.is_extended {
|
||||
Err(CanError::BadId(frame.id))
|
||||
} else {
|
||||
self.sent.push(*frame);
|
||||
Ok(())
|
||||
};
|
||||
std::future::ready(result)
|
||||
}
|
||||
|
||||
fn recv(&mut self) -> impl std::future::Future<Output = Result<CanFrame, Self::Error>> {
|
||||
let result = if self.closed {
|
||||
Err(CanError::Closed)
|
||||
} else {
|
||||
self.pending.pop_front().ok_or(CanError::Closed)
|
||||
};
|
||||
std::future::ready(result)
|
||||
}
|
||||
|
||||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error> {
|
||||
if self.closed {
|
||||
return Err(CanError::Closed);
|
||||
}
|
||||
Ok(self.pending.pop_front())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test]
|
||||
async fn records_sent_frames_in_order() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.send(&CanFrame::new(0x200, [1; 8])).await.unwrap();
|
||||
bus.send(&CanFrame::new(0x200, [2; 8])).await.unwrap();
|
||||
assert_eq!(bus.sent().len(), 2);
|
||||
assert_eq!(bus.last_sent().unwrap().data[0], 2);
|
||||
bus.clear_sent();
|
||||
assert!(bus.sent().is_empty());
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn injected_frames_come_back_once() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(CanFrame::new(0x201, [9; 8]));
|
||||
assert_eq!(bus.pending_len(), 1);
|
||||
let frame = bus.recv().await.unwrap();
|
||||
assert_eq!(frame.id, 0x201);
|
||||
assert_eq!(bus.try_recv().unwrap(), None, "取走之后就没有了");
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn closed_bus_reports_errors_on_every_path() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.close();
|
||||
assert_eq!(
|
||||
bus.send(&CanFrame::new(0x200, [0; 8])).await.unwrap_err(),
|
||||
CanError::Closed
|
||||
);
|
||||
assert_eq!(bus.recv().await.unwrap_err(), CanError::Closed);
|
||||
assert_eq!(bus.try_recv().unwrap_err(), CanError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn illegal_dlc_is_rejected_before_recording() {
|
||||
let mut bus = MockBus::new();
|
||||
let mut bad = CanFrame::new(0x200, [0; 8]);
|
||||
bad.dlc = 9;
|
||||
assert_eq!(bus.send(&bad).await.unwrap_err(), CanError::BadDlc(9));
|
||||
assert!(bus.sent().is_empty(), "非法帧不得进入记录");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use mock::*;
|
||||
@@ -1,32 +0,0 @@
|
||||
[package]
|
||||
name = "chassis-bench"
|
||||
description = "Bench harness: stimulus, safety envelope, trial sampling, and the bench binaries"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
# 二进制从 `src/bin/` 自动发现：
|
||||
# - `chassis-spin`：四轮定速旋转（slcan，全平台）
|
||||
# - `socketcan-spin`：单电机验证（SocketCAN，仅 Linux）
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
dji-esc-slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
dji-esc-socketcan = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -1,554 +0,0 @@
|
||||
//! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 500 rpm）。
|
||||
//!
|
||||
//! ```sh
|
||||
//! # T153 板载 CANable2（slcan 固件）→ /dev/ttyACM0，四轮 500 rpm 转子，缓升 1s 保持 10s
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- \
|
||||
//! --device /dev/ttyACM0 --rpm 500
|
||||
//!
|
||||
//! # 只验证链路与反馈，不发任何非零电流（接线排查用）
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
//! ```
|
||||
//!
|
||||
//! # 抽象边界
|
||||
//!
|
||||
//! 本工具是**组合根**：`dji-esc-slcan`（链路桥接）→ `dji-esc`（C620 协议 +
|
||||
//! 速度环）→ `motor-core`（`VelocityDrive` / `MotorFeedback` / `Disable` 能力
|
||||
//! trait）。激励场景与安全包络来自同 crate 的 `chassis-bench` 库，与
|
||||
//! `chassis-tune` 的真机后端**共用同一套实现**。
|
||||
//!
|
||||
//! # 命令租约（重要）
|
||||
//!
|
||||
//! `EscBus` 的命令是**租约**：`command_timeout`（默认 50ms）内没有重新下发命令，
|
||||
//! 驱动会强制 0 电流。所以控制循环必须**逐拍重发**同一个设定值——本工具在 1kHz
|
||||
//! 节拍里重发（与 `chassisd` 的控制循环一致）。只在开头下发一次是**不行的**。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 轮子必须架空或整车固定，急停可及。默认低限流、有限时长、ctrl-c 任意时刻
|
||||
//! 安全停机（先 0 电流再关总线）；未收到全部反馈前绝不下发速度。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::process::ExitCode;
|
||||
use std::time::Duration;
|
||||
|
||||
use chassis_bench::{SafetyEnvelope, SafetyMonitor, Scenario};
|
||||
use clap::{Parser, ValueEnum};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_slcan::{CanSpeed, SlcanOptions, open_link_with};
|
||||
use motor_core::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval, sleep};
|
||||
|
||||
/// 稳态判据：平均转子转速与目标的相对误差上限。
|
||||
const TOLERANCE: f32 = 0.05;
|
||||
/// 状态打印周期。
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(200);
|
||||
/// 反馈新鲜度阈值：`feedback_age` 超过它即视为该电调掉线。
|
||||
const FRESH: Duration = Duration::from_millis(100);
|
||||
/// 过速需要持续这么久才判定为失控并停机（避免单帧噪声误触发）。
|
||||
const OVERSPEED_GRACE: Duration = Duration::from_millis(60);
|
||||
/// 控制节拍（C620 期望 1kHz 命令）。
|
||||
const CONTROL_PERIOD: Duration = Duration::from_millis(1);
|
||||
/// 四轮默认名（顺序固定 FL、FR、RL、RR）。
|
||||
const WHEEL_NAMES: [&str; 4] = ["front_left", "front_right", "rear_left", "rear_right"];
|
||||
|
||||
#[derive(Debug, Parser)]
|
||||
#[command(name = "chassis-spin", version, about = "台架：四轮同时旋转（默认 500 rpm 转子）")]
|
||||
struct Args {
|
||||
/// slcan 串口设备（T153 板载 CANable2 为 /dev/ttyACM0）。
|
||||
#[arg(long, default_value = "/dev/ttyACM0")]
|
||||
device: String,
|
||||
|
||||
/// 串口波特率（CDC ACM 一般忽略；CH340 类适配器需与固件一致）。
|
||||
#[arg(long, default_value_t = 2_000_000)]
|
||||
serial_baud: u32,
|
||||
|
||||
/// 串口超时（毫秒）。读写共用：`serial` 的 `TTYPort::write` 也按它 poll(POLLOUT)。
|
||||
#[arg(long, default_value_t = 50)]
|
||||
serial_timeout_ms: u64,
|
||||
|
||||
/// CAN 波特率档位（C620 标准 = 1Mbps）。
|
||||
#[arg(long, value_enum, default_value_t = CanSpeedArg::M1)]
|
||||
can_speed: CanSpeedArg,
|
||||
|
||||
/// 电调 ID 列表，顺序 = FL、FR、RL、RR。
|
||||
#[arg(long, value_delimiter = ',', default_value = "1,2,3,4")]
|
||||
esc_ids: Vec<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
|
||||
}
|
||||
@@ -1,121 +0,0 @@
|
||||
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||||
//!
|
||||
//! ```sh
|
||||
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||||
//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10
|
||||
//! ```
|
||||
//!
|
||||
//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。
|
||||
//!
|
||||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use std::time::Duration;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc::{BusOptions, EscBus, EscConfig};
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc_socketcan::open_link;
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor_core::Motor;
|
||||
#[cfg(target_os = "linux")]
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
#[tokio::main]
|
||||
async fn main() {
|
||||
let mut args = std::env::args().skip(1);
|
||||
let interface = args.next().unwrap_or_else(|| "can0".into());
|
||||
let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
|
||||
let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
|
||||
let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
|
||||
|
||||
println!("=== 单电机抽象验证 ===");
|
||||
println!(
|
||||
"接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s"
|
||||
);
|
||||
|
||||
let link = open_link(&interface).unwrap_or_else(|e| {
|
||||
eprintln!(
|
||||
"打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）"
|
||||
);
|
||||
std::process::exit(1);
|
||||
});
|
||||
let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
|
||||
println!(
|
||||
"电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}",
|
||||
config.model, config.gear_ratio, config.current_limit
|
||||
);
|
||||
|
||||
let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法");
|
||||
let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
|
||||
|
||||
spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
|
||||
|
||||
bus.shutdown().await;
|
||||
println!("已退出");
|
||||
}
|
||||
|
||||
/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
|
||||
/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
|
||||
///
|
||||
/// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发
|
||||
/// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值
|
||||
/// （真实底盘的控制循环每 1ms 重发一次）。
|
||||
#[cfg(target_os = "linux")]
|
||||
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||||
const LEASE_PERIOD: Duration = Duration::from_millis(10);
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(100);
|
||||
|
||||
motor.set_velocity(target_rpm);
|
||||
println!("已下发目标转速，开始观测（100ms 周期）……\n");
|
||||
|
||||
let mut lease = interval(LEASE_PERIOD);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut report = interval(REPORT_PERIOD);
|
||||
report.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = tokio::time::Instant::now() + duration;
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = tokio::signal::ctrl_c() => {
|
||||
println!("\n收到 ctrl-c");
|
||||
break;
|
||||
}
|
||||
_ = lease.tick() => motor.set_velocity(target_rpm), // 续租
|
||||
_ = report.tick() => {
|
||||
let st = motor.state();
|
||||
if st.has_feedback() {
|
||||
println!(
|
||||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||||
st.rotor_speed_rpm,
|
||||
st.effort_raw,
|
||||
st.temperature_c,
|
||||
st.rotor_revolutions,
|
||||
st.feedback_count
|
||||
);
|
||||
} else {
|
||||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||||
}
|
||||
}
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
motor.disable();
|
||||
// 让 0 电流帧至少发出一拍再关总线。
|
||||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||||
println!("已停机（0 电流）");
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
fn main() {
|
||||
eprintln!("spin 仅支持 Linux（需要 SocketCAN）");
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
//! 台架工具库：激励场景、安全包络与真机试验采样。
|
||||
//!
|
||||
//! 本 crate 是**台架相关一切的唯一归属**：以前 `chassis-spin` 是
|
||||
//! `dji-esc-slcan` 的一个 example、`spin` 是 `dji-esc-socketcan` 的一个
|
||||
//! example——运输后端 crate 却装着应用级工具。现在：
|
||||
//!
|
||||
//! - 运输 crate（`dji-esc-slcan` / `dji-esc-socketcan`）只做桥接；
|
||||
//! - 台架程序与可复用的安全 / 采样逻辑住在这里；
|
||||
//! - `chassis-tune` 的真机后端复用 [`sampler`] 与 [`safety`]，不重复实现。
|
||||
//!
|
||||
//! 二进制：
|
||||
//!
|
||||
//! - `chassis-spin`：四轮定速旋转（slcan，全平台）；
|
||||
//! - `socketcan-spin`：单电机链路验证（SocketCAN，仅 Linux）；
|
||||
//! - [`link`]：两种运输后端的统一门面（台架与真机整定共用）。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod link;
|
||||
pub mod safety;
|
||||
pub mod sampler;
|
||||
|
||||
pub use link::{CanSpeed, SlcanOptions, default_slcan_options, open_slcan, open_socketcan};
|
||||
pub use safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
pub use sampler::{TrialOutcome, run_step_trial, run_step_trial_until_ctrl_c};
|
||||
|
||||
/// 与离线 harness 共用的场景 / 轨迹类型（同一套实验词汇）。
|
||||
pub use dji_esc_sim::{Scenario, SetpointProfile, Trace};
|
||||
@@ -1,87 +0,0 @@
|
||||
//! 链路打开：把两种运输后端收敛成一个门面。
|
||||
//!
|
||||
//! 台架工具与自动整定的真机后端都需要"给个设备名，拿回一条 [`CanLink`]"，
|
||||
//! 而不关心底层是 slcan 串口还是 SocketCAN。把选择留在这一处，上层就不必
|
||||
//! 同时依赖两个运输 crate。
|
||||
|
||||
use std::io::Result;
|
||||
use std::time::Duration;
|
||||
|
||||
use can_link::CanLink;
|
||||
|
||||
/// slcan 波特率档位（对应协议 `Sn` 命令）。
|
||||
pub use dji_esc_slcan::CanSpeed;
|
||||
/// slcan 打开参数。
|
||||
pub use dji_esc_slcan::SlcanOptions;
|
||||
|
||||
/// 打开 slcan 串口设备（CANable 等），桥接为链路端点。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 设备不存在、权限不足或波特率不被支持时的 `io::Error`。
|
||||
pub fn open_slcan(device: &str, options: SlcanOptions) -> Result<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));
|
||||
}
|
||||
}
|
||||
@@ -1,322 +0,0 @@
|
||||
//! 台架安全包络：把"什么时候必须立刻停机"变成可复用的判定。
|
||||
//!
|
||||
//! 台架工具（`chassis-spin`）与自动整定的真机后端（`chassis-tune --backend bench`）
|
||||
//! 共用同一套判定，避免安全逻辑在两个地方各写一遍、然后慢慢漂移。
|
||||
//!
|
||||
//! 所有阈值都是 `Option`：只开需要的那个。`chassis-spin` 只开过速保护
|
||||
//! （保持历史行为），自动整定则开启全部——因为搜索会主动试错，必须由
|
||||
//! 包络兜住"试错到失控"。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use motor_core::MotorFeedback;
|
||||
// 用 tokio 的时钟：`start_paused` 下虚拟时间可控，安全包络（宽限期 / 超时）
|
||||
// 才能被确定性地测试；生产构建里它就是 `std::time::Instant`。
|
||||
use thiserror::Error;
|
||||
use tokio::time::Instant;
|
||||
|
||||
/// 触发停机的原因。
|
||||
#[derive(Debug, Clone, PartialEq, Error)]
|
||||
pub enum Abort {
|
||||
/// 转速超过上限并持续超过宽限期。
|
||||
#[error("overspeed: {peak_rpm:.0} rpm > {limit_rpm:.0} rpm for over {grace_ms}ms")]
|
||||
Overspeed {
|
||||
/// 观测到的峰值转速（rpm）。
|
||||
peak_rpm: f32,
|
||||
/// 配置的上限（rpm）。
|
||||
limit_rpm: f32,
|
||||
/// 宽限期（毫秒）。
|
||||
grace_ms: u64,
|
||||
},
|
||||
/// 温度超过上限。
|
||||
#[error("overtemperature: {temperature_c}℃ > {limit_c}℃")]
|
||||
OverTemperature {
|
||||
/// 观测温度（℃）。
|
||||
temperature_c: i8,
|
||||
/// 上限（℃）。
|
||||
limit_c: i8,
|
||||
},
|
||||
/// 反馈超出新鲜度阈值（掉线）。
|
||||
#[error("esc {esc_id} feedback lost: age {age_ms:?}ms > {timeout_ms}ms")]
|
||||
FeedbackLost {
|
||||
/// 电调 ID。
|
||||
esc_id: u8,
|
||||
/// 实测反馈年龄（毫秒）。
|
||||
age_ms: Option<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:?}");
|
||||
}
|
||||
}
|
||||
@@ -1,239 +0,0 @@
|
||||
//! 真机试验：把一条 [`Scenario`] 在真实电调上重放并采集成 [`Trace`]。
|
||||
//!
|
||||
//! 与 `dji-esc-sim` 的离线 harness 是**同一套场景词汇**：整定工具先用离线
|
||||
//! harness 搜索，再用本模块在台架上复核，两条路径的指标口径因此一致。
|
||||
//!
|
||||
//! # 安全契约
|
||||
//!
|
||||
//! - 每个采样拍都重发设定值——**命令是租约**，不重发会在 50ms 后失电；
|
||||
//! - 每个采样拍都过一遍 [`SafetyMonitor`]；
|
||||
//! - 无论正常结束、触发包络还是被取消，退出前一律 `disable()`（0 电流）。
|
||||
|
||||
use std::future::Future;
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use motor_core::{Disable, MotorFeedback, VelocityDrive};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval};
|
||||
|
||||
use crate::safety::{Abort, SafetyEnvelope, SafetyMonitor};
|
||||
|
||||
/// 一次真机试验的结果。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct TrialOutcome {
|
||||
/// 采集到的轨迹（触发中止时只含中止前的采样）。
|
||||
pub trace: Trace,
|
||||
/// 触发安全包络 / 被取消的原因；`None` 表示按场景跑完。
|
||||
pub abort: Option<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, "取消后不得继续采样");
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
[package]
|
||||
name = "chassis-config"
|
||||
description = "chassis.toml schema: parsing, semantic validation, and mapping to driver configuration"
|
||||
description = "chassis.toml schema and validation"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
@@ -10,9 +10,13 @@ license.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
dji-esc = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
mecanum = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
|
||||
@@ -10,9 +10,10 @@
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{BusOptions, EscConfig, EscModel};
|
||||
use esc_bus::{BusOptions, EscConfig};
|
||||
use mecanum::Mecanum;
|
||||
use motor_core::VelocityGains;
|
||||
use motor_control::VelocityGains;
|
||||
use motor_protocol::EscModel;
|
||||
use serde::{Deserialize, Serialize};
|
||||
use thiserror::Error;
|
||||
|
||||
@@ -411,10 +412,6 @@ impl Config {
|
||||
gear_ratio: m.gear_ratio,
|
||||
direction: m.direction,
|
||||
current_limit: m.current_limit,
|
||||
kp: m.kp,
|
||||
ki: m.ki,
|
||||
kd: m.kd,
|
||||
feedforward: m.feedforward,
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
[package]
|
||||
name = "chassis-tune"
|
||||
description = "Automated PID gain search for the chassis velocity loop: offline sweep/refine plus bench (hardware-in-the-loop) verification"
|
||||
description = "Automated chassis velocity-loop tuning tool"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
@@ -10,32 +10,37 @@ license.workspace = true
|
||||
workspace = true
|
||||
|
||||
[features]
|
||||
default = []
|
||||
# 可选：用 plotters 输出 best.svg（默认关闭，避免 CI 依赖系统字体与更长编译）。
|
||||
plot = ["dep:plotters"]
|
||||
plot = ['dep:plotters']
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
chassis-bench = { workspace = true }
|
||||
bench = { workspace = true }
|
||||
can = { workspace = true }
|
||||
chassis-config = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
motor-model = { workspace = true }
|
||||
sim-harness = { workspace = true }
|
||||
tuning = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
slcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
argmin = { workspace = true }
|
||||
argmin-math = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
rayon = { workspace = true }
|
||||
indicatif = { workspace = true }
|
||||
csv = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
serde_json = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
serde_json = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
csv = { workspace = true }
|
||||
indicatif = { workspace = true }
|
||||
rayon = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
argmin = { workspace = true }
|
||||
argmin-math = { workspace = true }
|
||||
anyhow = { workspace = true }
|
||||
plotters = { workspace = true, optional = true }
|
||||
|
||||
[dev-dependencies]
|
||||
|
||||
@@ -1,173 +0,0 @@
|
||||
//! 评估后端：把"一组增益有多好"这件事抽象出来。
|
||||
//!
|
||||
//! - [`SimEvaluator`]：离线 harness + 被控对象集成（域随机化），CPU 密集、可并行；
|
||||
//! - [`BenchEvaluator`](crate::bench::BenchEvaluator)：真机台架（`chassis-bench`），单轮、串行、有安全包络。
|
||||
//!
|
||||
//! 搜索器只认识 [`Evaluator`] trait，因此"先仿真粗调、再上台架复验"是同一套
|
||||
//! 代码换一个后端，而不是两份实现。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, PlantEnsemble, Scenario, simulate};
|
||||
use motor_core::VelocityGains;
|
||||
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
pub use crate::cost::Weights;
|
||||
|
||||
/// 一次评估的结果。
|
||||
pub type Evaluated = Score;
|
||||
|
||||
/// 被控对象评估后端。
|
||||
///
|
||||
/// 必须 `Send + Sync`：粗扫阶段由 `rayon` 并行调用。
|
||||
pub trait Evaluator: Send + Sync {
|
||||
/// 评估一组增益。
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated;
|
||||
/// 一行描述（写进报告，便于复现）。
|
||||
fn describe(&self) -> String;
|
||||
}
|
||||
|
||||
/// 离线评估：在一组被控对象参数上重放同一条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct SimEvaluator {
|
||||
/// 被控对象集成（域随机化）。
|
||||
pub ensemble: PlantEnsemble,
|
||||
/// 评价场景。
|
||||
pub scenario: Scenario,
|
||||
/// 限幅。
|
||||
pub limits: LoopLimits,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 集成聚合权重：0 = 只看均值，1 = 只看最坏成员。
|
||||
pub robust_weight: f64,
|
||||
}
|
||||
|
||||
impl SimEvaluator {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
ensemble: PlantEnsemble,
|
||||
scenario: Scenario,
|
||||
limits: LoopLimits,
|
||||
objective: Objective,
|
||||
robust_weight: f64,
|
||||
) -> Self {
|
||||
Self { ensemble, scenario, limits, objective, robust_weight: robust_weight.clamp(0.0, 1.0) }
|
||||
}
|
||||
|
||||
/// 集成规模。
|
||||
#[must_use]
|
||||
pub fn ensemble_size(&self) -> usize {
|
||||
self.ensemble.len()
|
||||
}
|
||||
}
|
||||
|
||||
impl Evaluator for SimEvaluator {
|
||||
fn evaluate(&self, gains: &VelocityGains) -> Evaluated {
|
||||
let mut costs = Vec::with_capacity(self.ensemble.len());
|
||||
let mut nominal: Option<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}");
|
||||
}
|
||||
}
|
||||
@@ -1,383 +0,0 @@
|
||||
//! 真机（HIL）评估后端：在台架上逐次试验一组增益。
|
||||
//!
|
||||
//! 与 [`SimEvaluator`](crate::backend::SimEvaluator) 实现同一个 [`Evaluator`]，
|
||||
//! 因此 `sweep` / `validate` 的全部逻辑（粗扫、精修、报告）都不需要改动，
|
||||
//! 只是"评估一组增益有多好"这件事从浮点积分换成了真的让轮子转。
|
||||
//!
|
||||
//! # 安全设计
|
||||
//!
|
||||
//! 每次试验都：① 先确认目标电调反馈新鲜；② 逐拍重发设定值（命令租约）；
|
||||
//! ③ 每拍过一遍 [`SafetyEnvelope`]；④ 无论正常结束、触发包络还是 ctrl-c，
|
||||
//! 退出前一律 `disable()`；⑤ 试验之间插入静默期，让轮子真正停下来再开始下一组。
|
||||
//!
|
||||
//! 评估是**串行**的（真机只有一条总线）。调用方应把搜索放进单线程 rayon 池
|
||||
//! （见 `main`），否则大量线程会在互斥锁上排队、白白浪费时间。
|
||||
|
||||
use std::sync::Mutex;
|
||||
use std::time::Duration;
|
||||
|
||||
use anyhow::Context as _;
|
||||
use chassis_bench::{Abort, SafetyEnvelope, run_step_trial};
|
||||
use dji_esc::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use motor_core::{Disable as _, MotorFeedback, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Objective, Score, Verdict};
|
||||
|
||||
/// 真机后端错误。
|
||||
#[derive(Debug, Error)]
|
||||
pub enum BenchError {
|
||||
/// 无法打开总线。
|
||||
#[error("cannot open CAN link: {0}")]
|
||||
Link(String),
|
||||
/// 电调总线启动失败。
|
||||
#[error("cannot start ESC bus: {0}")]
|
||||
Bus(String),
|
||||
/// 启动超时：目标电调没有反馈。
|
||||
#[error("esc {esc_id} has no fresh feedback after {waited_ms}ms (wiring / power / bitrate?)")]
|
||||
NoFeedback {
|
||||
/// 目标电调 ID。
|
||||
esc_id: u8,
|
||||
/// 等待时长（毫秒）。
|
||||
waited_ms: u64,
|
||||
},
|
||||
/// 搜索期间因安全事件中止且未要求继续。
|
||||
#[error("aborted by safety envelope: {reason}")]
|
||||
Aborted {
|
||||
/// 触发原因。
|
||||
reason: String,
|
||||
},
|
||||
}
|
||||
|
||||
/// 真机评估配置。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct BenchConfig {
|
||||
/// 电调配置（名称、ID、方向、减速比、限流）——增益由搜索覆写。
|
||||
pub esc: EscConfig,
|
||||
/// 总线运行参数。
|
||||
pub bus: BusOptions,
|
||||
/// 评估场景。
|
||||
pub scenario: Scenario,
|
||||
/// 安全包络。
|
||||
pub envelope: SafetyEnvelope,
|
||||
/// 硬约束与权重。
|
||||
pub objective: Objective,
|
||||
/// 试验之间的静默期（等轮子停稳）。
|
||||
pub between_trials: Duration,
|
||||
/// 命令租约续期节拍。
|
||||
pub lease_period: Duration,
|
||||
/// 启动时等待反馈的超时。
|
||||
pub startup_timeout: Duration,
|
||||
/// 安全检查触发后是否继续搜索（默认 false = 立即停）。
|
||||
pub continue_on_abort: bool,
|
||||
}
|
||||
|
||||
impl BenchConfig {
|
||||
/// 合理默认：1 kHz 总线、50 ms 租约、5 s 启动超时、500 ms 试验间隔。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
esc: EscConfig,
|
||||
scenario: Scenario,
|
||||
envelope: SafetyEnvelope,
|
||||
objective: Objective,
|
||||
) -> Self {
|
||||
Self {
|
||||
esc,
|
||||
bus: BusOptions::default(),
|
||||
scenario,
|
||||
envelope,
|
||||
objective,
|
||||
between_trials: Duration::from_millis(500),
|
||||
lease_period: Duration::from_millis(5),
|
||||
startup_timeout: Duration::from_secs(5),
|
||||
continue_on_abort: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 真机评估器（持有总线与运行时）。
|
||||
pub struct BenchEvaluator {
|
||||
session: Mutex<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 接口必须报错");
|
||||
}
|
||||
}
|
||||
@@ -1,302 +0,0 @@
|
||||
//! 成本函数：把指标折成一个标量，并把"不可接受"变成硬约束。
|
||||
//!
|
||||
//! 两个原则：
|
||||
//!
|
||||
//! 1. **硬约束不用权重表达**。过速 / 长时间饱和 / 不收敛不是"差一点"，
|
||||
//! 而是"不能用"——它们直接给罚值并记录判定，绝不允许被其他项的权重
|
||||
//! 抵消掉（否则搜索会主动挑一个会飞车的解）。
|
||||
//! 2. **各项归一化**。时间按轨迹时长归一、误差按 `|目标| × 时长` 归一、
|
||||
//! effort 按限幅归一，因此权重在不同场景 / 不同电机之间仍然可解释。
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, SimError, Trace};
|
||||
|
||||
use crate::metrics::StepMetrics;
|
||||
|
||||
/// 罚值：远大于任何合法成本，但保持有限（避免 NaN 传染给优化器）。
|
||||
pub const PENALTY: f64 = 1.0e9;
|
||||
|
||||
/// 成本权重（各项已归一化到 O(1)）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Weights {
|
||||
/// 调节时间权重。
|
||||
pub settle: f64,
|
||||
/// 超调权重。
|
||||
pub overshoot: f64,
|
||||
/// 绝对误差积分（IAE）权重。
|
||||
pub iae: f64,
|
||||
/// effort（电流）权重。
|
||||
pub effort: f64,
|
||||
}
|
||||
|
||||
impl Default for Weights {
|
||||
fn default() -> Self {
|
||||
// 底盘速度环：跟得上、不过冲最重要；电流只作为次要惩罚
|
||||
// （省电流不是目的，避免烧电调才是）。
|
||||
Self { settle: 1.0, overshoot: 1.0, iae: 0.5, effort: 0.1 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Weights {
|
||||
/// 人类可读描述（日志 / 报告用）。
|
||||
#[must_use]
|
||||
pub fn describe(&self) -> String {
|
||||
format!(
|
||||
"cost={:.2}·settle+{:.2}·overshoot+{:.2}·iae+{:.2}·effort",
|
||||
self.settle, self.overshoot, self.iae, self.effort
|
||||
)
|
||||
}
|
||||
|
||||
/// 权重是否全部有限且非负。
|
||||
#[must_use]
|
||||
pub fn is_valid(&self) -> bool {
|
||||
[self.settle, self.overshoot, self.iae, self.effort]
|
||||
.iter()
|
||||
.all(|w| w.is_finite() && *w >= 0.0)
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的判定。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Verdict {
|
||||
/// 达标（成本 = 加权指标）。
|
||||
Ok,
|
||||
/// 增益含 NaN / inf，或场景退化（被拒绝，不进控制器）。
|
||||
Rejected,
|
||||
/// 数值发散。
|
||||
Diverged,
|
||||
/// 过速。
|
||||
Overspeed,
|
||||
/// 饱和占比过高（积分绕死 / 限幅太紧）。
|
||||
Saturated,
|
||||
/// 在场景内没有收敛。
|
||||
NoSettle,
|
||||
}
|
||||
|
||||
impl Verdict {
|
||||
/// 是否达标。
|
||||
#[must_use]
|
||||
pub fn is_ok(self) -> bool {
|
||||
matches!(self, Self::Ok)
|
||||
}
|
||||
|
||||
/// 简短标签（报告 / CSV 用）。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Ok => "ok",
|
||||
Self::Rejected => "rejected",
|
||||
Self::Diverged => "diverged",
|
||||
Self::Overspeed => "overspeed",
|
||||
Self::Saturated => "saturated",
|
||||
Self::NoSettle => "no-settle",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次评估的结果。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Score {
|
||||
/// 成本（越小越好）。
|
||||
pub cost: f64,
|
||||
/// 判定。
|
||||
pub verdict: Verdict,
|
||||
/// 指标（被拒绝时为 `None`）。
|
||||
pub metrics: Option<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);
|
||||
}
|
||||
}
|
||||
@@ -1,572 +0,0 @@
|
||||
//! 增益参数空间：轴定义、采样与**坐标空间**映射。
|
||||
//!
|
||||
//! # 为什么要有"坐标空间"
|
||||
//!
|
||||
//! 增益的合法取值跨越数量级（`ki` 可能在 0.1 与 100 之间），线性空间的
|
||||
//! 单纯形步长对数量级不敏感；同时 `kp/ki/kd` 必须为正。把每个轴映射到
|
||||
//! **坐标空间**（对数轴取 `ln`，线性轴恒等）后：
|
||||
//!
|
||||
//! - 无约束优化器不会走出正增益区域；
|
||||
//! - 步长在不同数量级上等价（±0.1 的坐标步长 = ×1.1 的比例步长）；
|
||||
//! - `LogOrZero` 轴在**粗扫**阶段可以精确取 0（`ff = 0` 是台架常态），
|
||||
//! 在**精修**阶段退化为对数轴（零是不可导的拐点，本地搜索收益为负）。
|
||||
|
||||
use motor_core::VelocityGains;
|
||||
use rand::Rng;
|
||||
use rand::seq::SliceRandom as _;
|
||||
use thiserror::Error;
|
||||
|
||||
/// 轴数量：`kp`、`ki`、`kd`、`feedforward`。
|
||||
pub const AXIS_COUNT: usize = 4;
|
||||
|
||||
/// 轴的取值范围映射方式。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Scale {
|
||||
/// 线性均匀。
|
||||
Linear,
|
||||
/// 对数均匀（要求 `min > 0`）。
|
||||
Log,
|
||||
/// 以概率 `p_zero` 取精确 0，否则对数均匀（用于 `ki` / `kd` / `ff`）。
|
||||
LogOrZero {
|
||||
/// 取 0 的概率。
|
||||
p_zero: f64,
|
||||
},
|
||||
}
|
||||
|
||||
impl Scale {
|
||||
/// 是否是对数族（`Log` / `LogOrZero`）。
|
||||
#[must_use]
|
||||
pub fn is_log(self) -> bool {
|
||||
matches!(self, Self::Log | Self::LogOrZero { .. })
|
||||
}
|
||||
|
||||
/// 从文本解析（CLI 用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回 [`GainsError::UnknownScale`]。
|
||||
pub fn parse(text: &str) -> Result<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());
|
||||
}
|
||||
}
|
||||
@@ -1,60 +0,0 @@
|
||||
//! 底盘速度环 **PID 自动整定**：参数空间搜索 + 指标评估 + 报告。
|
||||
//!
|
||||
//! # 一条命令背后发生了什么
|
||||
//!
|
||||
//! 1. [`gains`] 定义搜索空间（四轴、对数刻度、可精确取 0 的前馈 / 微分）；
|
||||
//! 2. [`search`] 先做种子化分层粗扫（`rayon` 并行），再以 top-k 为起点做
|
||||
//! 多起点 `argmin` 局部精修；
|
||||
//! 3. 每次评估由 [`backend`] 执行：离线 harness 在**被控对象集成**上重放场景，
|
||||
//! 或（真机后端）在台架上跑一次受安全包络约束的试验；
|
||||
//! 4. [`metrics`] / [`cost`] 把轨迹折成加权成本，硬约束（过速 / 长时饱和 /
|
||||
//! 不收敛）直接给罚值，不参与权重折衷；
|
||||
//! 5. [`report`] 落盘 CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! # 超参数从哪来
|
||||
//!
|
||||
//! "搜什么、怎么打分、什么算达标"由 [`preset`] 定义，优先级为
|
||||
//! **内置默认 < 预设文件（`config/tune.toml`）< 显式命令行参数**。
|
||||
//! 一次评审过的预设可以长期复用，不再靠命令行回忆当时那组数。
|
||||
//!
|
||||
//! # 可复现
|
||||
//!
|
||||
//! 随机性只来自 `--seed`：同一 seed + 同一参数空间 + 同一后端 → 逐位一致的结果。
|
||||
//! 这是 CI 里做冒烟回归的前提。预设只是把参数空间 / 权重 / 场景固化下来，
|
||||
//! 因此"同一个预设 + 同一个 seed"在任何机器上都应当复现同一结果。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 真机后端（`--backend bench`）会**真的驱动电机**。安全由三层保证：
|
||||
//! 搜索前的强制确认标志、每次试验的 `chassis-bench` 安全包络、以及任何退出
|
||||
//! 路径都先归零电流。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod backend;
|
||||
pub mod bench;
|
||||
pub mod cost;
|
||||
pub mod gains;
|
||||
pub mod metrics;
|
||||
#[cfg(feature = "plot")]
|
||||
pub mod plot;
|
||||
pub mod preset;
|
||||
pub mod report;
|
||||
pub mod search;
|
||||
|
||||
pub use backend::{Evaluated, Evaluator, SimEvaluator};
|
||||
pub use bench::{BenchConfig, BenchError, BenchEvaluator};
|
||||
pub use cost::{Objective, PENALTY, Score, Verdict, Weights};
|
||||
pub use gains::{AXIS_COUNT, GainAxis, GainSpace, GainsError, Scale, parse_axis};
|
||||
pub use metrics::StepMetrics;
|
||||
pub use preset::{
|
||||
Acceptance, AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec,
|
||||
DEFAULT_PRESET_PATH, PRESET_VERSION, PlantSpec, Preset, PresetError, PresetFile, ProfileSpec,
|
||||
RangeSpec, ScaleSpec, ScenarioSpec, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning,
|
||||
};
|
||||
pub use report::{ReportContext, ReportError, write_reports};
|
||||
pub use search::{
|
||||
Phase, Ranked, SearchConfig, SearchError, SolverKind, Trial, evaluate_once, sweep,
|
||||
};
|
||||
|
||||
/// 被整定的物理量类型（来自 `motor-core`，与驱动层、台架共用同一个定义）。
|
||||
pub use motor_core::VelocityGains;
|
||||
+1102
-58
File diff suppressed because it is too large
Load Diff
@@ -1,265 +0,0 @@
|
||||
//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。
|
||||
//!
|
||||
//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能
|
||||
//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
|
||||
/// 稳态判据的相对带宽（±2% of target）。
|
||||
pub const SETTLING_BAND: f32 = 0.02;
|
||||
/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。
|
||||
///
|
||||
/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡，
|
||||
/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。
|
||||
pub const STEADY_FRACTION: f32 = 0.75;
|
||||
/// 指标最少需要的采样数（样本太少时一切统计都无意义）。
|
||||
pub const MIN_SAMPLES: usize = 8;
|
||||
|
||||
/// 一条步响应轨迹的指标集合。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct StepMetrics {
|
||||
/// 上升时间 10% → 90%（秒）；未到达时等于轨迹总时长。
|
||||
pub rise_time_s: f32,
|
||||
/// 超调量（%，相对目标；负值截为 0）。
|
||||
pub overshoot_pct: f32,
|
||||
/// 调节时间：最后一次离开 ±2% 带的时刻（秒）。
|
||||
pub settling_time_s: f32,
|
||||
/// 是否在稳态窗口内保持 ±2% 带（即真正收敛）。
|
||||
pub settled: bool,
|
||||
/// 稳态误差（%，相对目标）。
|
||||
pub ss_error_pct: f32,
|
||||
/// 绝对误差积分 ∫|e|dt（归一化前的原始值）。
|
||||
pub iae: f32,
|
||||
/// 误差平方积分 ∫e²dt。
|
||||
pub ise: f32,
|
||||
/// 时间加权绝对误差积分 ∫t|e|dt。
|
||||
pub itae: f32,
|
||||
/// 峰值 effort（raw，绝对值）。
|
||||
pub peak_effort_raw: f32,
|
||||
/// effort 均方根（raw）。
|
||||
pub rms_effort_raw: f32,
|
||||
/// effort 符号翻转次数（抖振）。
|
||||
pub effort_reversals: usize,
|
||||
/// 饱和采样占比（%）。
|
||||
pub saturated_pct: f32,
|
||||
/// 全过程最大转速（rpm，绝对值）。
|
||||
pub max_rpm: f32,
|
||||
/// 稳态窗口内的平均转速（rpm）。
|
||||
pub steady_rpm: f32,
|
||||
}
|
||||
|
||||
impl StepMetrics {
|
||||
/// 从轨迹计算指标；样本不足或目标转速为 0 时返回 `None`。
|
||||
#[must_use]
|
||||
pub fn from_trace(trace: &Trace, scenario: &Scenario) -> Option<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());
|
||||
}
|
||||
}
|
||||
@@ -1,129 +0,0 @@
|
||||
//! 可选（`--features plot`）：把最优步响应画成 SVG。
|
||||
//!
|
||||
//! 为什么是 SVG 而不是 PNG：矢量图在任何缩放级别都能看清起动段的细节，
|
||||
//! 且不需要位图编码器（依赖更小）。默认关闭该 feature——绘图不是整定的
|
||||
//! 必要条件，CSV 已经给了全部数据。
|
||||
|
||||
use std::path::Path;
|
||||
|
||||
use dji_esc_sim::{Scenario, Trace};
|
||||
use plotters::backend::SVGBackend;
|
||||
use plotters::chart::ChartBuilder;
|
||||
use plotters::drawing::IntoDrawingArea as _;
|
||||
use plotters::series::LineSeries;
|
||||
use plotters::style::{BLACK, BLUE, Color as _, RED, WHITE};
|
||||
|
||||
use crate::report::ReportError;
|
||||
|
||||
/// 写出步响应三联图：转速跟随、误差、电流。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 绘图或写文件失败时返回 [`ReportError::Plot`]。
|
||||
pub fn write_step_svg(path: &Path, trace: &Trace, scenario: &Scenario) -> Result<(), ReportError> {
|
||||
let root = SVGBackend::new(path, (1000, 900)).into_drawing_area();
|
||||
root.fill(&WHITE).map_err(plot_err)?;
|
||||
let (upper, lower) = root.split_vertically(600);
|
||||
let (middle, bottom) = lower.split_vertically(150);
|
||||
|
||||
let total = trace.t_s.last().copied().unwrap_or(1.0).max(1.0e-3);
|
||||
let peak = trace.rpm.iter().copied().fold(scenario.target_rpm, f32::max).max(1.0);
|
||||
let effort_peak =
|
||||
trace.effort_raw.iter().map(|&raw| f32::from(raw).abs()).fold(1.0f32, f32::max);
|
||||
|
||||
// 转速跟随。
|
||||
let mut chart = ChartBuilder::on(&upper)
|
||||
.caption("velocity step response (rpm)", ("sans-serif", 24))
|
||||
.margin(12)
|
||||
.x_label_area_size(36)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, 0.0f32..peak * 1.1)
|
||||
.map_err(plot_err)?;
|
||||
chart.configure_mesh().x_desc("t (s)").y_desc("rpm").draw().map_err(plot_err)?;
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.setpoint_rpm.iter().copied()),
|
||||
&BLACK,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("setpoint")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], BLUE));
|
||||
chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().copied().zip(trace.rpm.iter().copied()),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?
|
||||
.label("measured")
|
||||
.legend(|(x, y)| plotters::element::PathElement::new([(x, y), (x + 18, y)], RED));
|
||||
chart.configure_series_labels().background_style(WHITE.mix(0.8)).draw().map_err(plot_err)?;
|
||||
|
||||
// 误差。
|
||||
let mut error_chart = ChartBuilder::on(&middle)
|
||||
.caption("error (rpm)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -peak..peak)
|
||||
.map_err(plot_err)?;
|
||||
error_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
error_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace
|
||||
.t_s
|
||||
.iter()
|
||||
.zip(trace.setpoint_rpm.iter().zip(trace.rpm.iter()))
|
||||
.map(|(&t, (&sp, &rpm))| (t, sp - rpm)),
|
||||
&RED,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
// 电流。
|
||||
let mut effort_chart = ChartBuilder::on(&bottom)
|
||||
.caption("effort (raw)", ("sans-serif", 16))
|
||||
.margin(8)
|
||||
.x_label_area_size(28)
|
||||
.y_label_area_size(64)
|
||||
.build_cartesian_2d(0.0f32..total, -effort_peak..effort_peak)
|
||||
.map_err(plot_err)?;
|
||||
effort_chart.configure_mesh().draw().map_err(plot_err)?;
|
||||
effort_chart
|
||||
.draw_series(LineSeries::new(
|
||||
trace.t_s.iter().zip(trace.effort_raw.iter()).map(|(&t, &raw)| (t, f32::from(raw))),
|
||||
&BLUE,
|
||||
))
|
||||
.map_err(plot_err)?;
|
||||
|
||||
root.present().map_err(plot_err)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn plot_err<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
@@ -1,713 +0,0 @@
|
||||
//! 报告与产物写出：CSV / TOML / JSON / Markdown（可选 SVG）。
|
||||
//!
|
||||
//! 产物布局（`--out-dir`）：
|
||||
//!
|
||||
//! | 文件 | 内容 |
|
||||
//! | --- | --- |
|
||||
//! | `trials.csv` | 全部候选：增益 + 指标 + 成本 + 判定（可用任意表格软件复盘） |
|
||||
//! | `best.toml` | 可直接粘贴进 `chassis.toml` 的 `[[motor]]` 片段 |
|
||||
//! | `best-trace.csv` | 最优增益的完整步响应轨迹 |
|
||||
//! | `report.md` | 人类可读总结：设置、Top-N、结论与注意事项 |
|
||||
//! | `summary.json` | 机器可读结果（CI / 后续工具消费） |
|
||||
//! | `best.svg` | 步响应三联图（仅 `--features plot`） |
|
||||
|
||||
use std::fmt::Write as _;
|
||||
use std::path::{Path, PathBuf};
|
||||
|
||||
use dji_esc_sim::{LoopLimits, Scenario, Trace};
|
||||
use serde::Serialize;
|
||||
|
||||
use crate::gains::GainSpace;
|
||||
use crate::metrics::StepMetrics;
|
||||
use crate::search::{Ranked, SearchConfig, Trial, verdict_hint};
|
||||
|
||||
/// 报告写出错误。
|
||||
#[derive(Debug, thiserror::Error)]
|
||||
pub enum ReportError {
|
||||
/// 目录创建或文件写入失败。
|
||||
#[error("io error at {path}: {source}")]
|
||||
Io {
|
||||
/// 出错路径。
|
||||
path: PathBuf,
|
||||
/// 底层错误。
|
||||
source: std::io::Error,
|
||||
},
|
||||
/// CSV 写出失败。
|
||||
#[error("csv error: {0}")]
|
||||
Csv(#[from] csv::Error),
|
||||
/// JSON 写出失败。
|
||||
#[error("json error: {0}")]
|
||||
Json(#[from] serde_json::Error),
|
||||
/// TOML 写出失败。
|
||||
#[error("toml error: {0}")]
|
||||
Toml(#[from] toml::ser::Error),
|
||||
/// 没有可用的最优结果。
|
||||
#[error("no feasible trial to report")]
|
||||
NoBest,
|
||||
/// SVG 绘图失败（仅 `--features plot`）。
|
||||
#[error("plot error: {0}")]
|
||||
Plot(String),
|
||||
}
|
||||
|
||||
fn io(path: impl Into<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));
|
||||
}
|
||||
}
|
||||
@@ -1,628 +0,0 @@
|
||||
//! 两阶段参数空间搜索：种子化分层粗扫 → `argmin` 多起点局部精修。
|
||||
//!
|
||||
//! # 为什么是两阶段
|
||||
//!
|
||||
//! 成本函数有一大片"罚值平原"（不收敛 / 饱和的候选成本都等于 `PENALTY`），
|
||||
//! 纯局部搜索会陷在平原上；而纯网格搜索在四个数量级跨度上需要天文数字的
|
||||
//! 评估次数。两阶段各取所长：
|
||||
//!
|
||||
//! 1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数把
|
||||
//! 空间铺满，拿到若干个互不相邻的可达解（top-k）；
|
||||
//! 2. **精修**：以每个可达解为起点做多起点 `argmin` 局部搜索（Nelder-Mead
|
||||
//! 或 PSO），在**坐标空间**（对数刻度）里收敛到局部最优。
|
||||
//!
|
||||
//! # 可复现性
|
||||
//!
|
||||
//! 每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，结果与线程
|
||||
//! 调度无关；粗扫结果按下标排序后再取 top-k，因此同 seed 两次运行逐位一致。
|
||||
|
||||
use std::sync::atomic::{AtomicU64, Ordering};
|
||||
|
||||
use argmin::core::{CostFunction, Error as ArgminError, Executor};
|
||||
use argmin::solver::neldermead::NelderMead;
|
||||
use argmin::solver::particleswarm::ParticleSwarm;
|
||||
use dji_esc_sim::Scenario;
|
||||
use indicatif::ProgressBar;
|
||||
use motor_core::VelocityGains;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
use rayon::prelude::*;
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::backend::Evaluator;
|
||||
use crate::cost::{Score, Verdict};
|
||||
use crate::gains::{AXIS_COUNT, GainSpace};
|
||||
|
||||
/// 搜索阶段的错误。
|
||||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||||
pub enum SearchError {
|
||||
/// 参数空间非法。
|
||||
#[error("invalid gain space: {0}")]
|
||||
InvalidSpace(String),
|
||||
/// 粗扫样本数为 0。
|
||||
#[error("samples must be >= 1")]
|
||||
NoSamples,
|
||||
}
|
||||
|
||||
/// 候选来自哪个阶段。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Phase {
|
||||
/// 分层粗扫。
|
||||
Coarse,
|
||||
/// 局部精修。
|
||||
Refine,
|
||||
/// 用户指定 / 基线点。
|
||||
Seed,
|
||||
}
|
||||
|
||||
impl Phase {
|
||||
/// CSV / 报告标签。
|
||||
#[must_use]
|
||||
pub fn label(self) -> &'static str {
|
||||
match self {
|
||||
Self::Coarse => "coarse",
|
||||
Self::Refine => "refine",
|
||||
Self::Seed => "seed",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 一次被评估的候选。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Trial {
|
||||
/// 评估序号（按评估顺序递增，报告可复现）。
|
||||
pub index: u64,
|
||||
/// 所属阶段。
|
||||
pub phase: Phase,
|
||||
/// 增益。
|
||||
pub gains: VelocityGains,
|
||||
/// 评估结果。
|
||||
pub score: Score,
|
||||
}
|
||||
|
||||
impl Trial {
|
||||
/// 成本。
|
||||
#[must_use]
|
||||
pub fn cost(&self) -> f64 {
|
||||
self.score.cost
|
||||
}
|
||||
}
|
||||
|
||||
/// 局部精修求解器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub enum SolverKind {
|
||||
/// Nelder-Mead（无导数、对罚值平原鲁棒），默认。
|
||||
#[default]
|
||||
NelderMead,
|
||||
/// 粒子群（更全局，评估次数更高）。
|
||||
Pso,
|
||||
}
|
||||
|
||||
impl SolverKind {
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::NelderMead => "nelder-mead",
|
||||
Self::Pso => "pso",
|
||||
}
|
||||
}
|
||||
|
||||
/// 从文本解析。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 未知名字时返回错误。
|
||||
pub fn parse(text: &str) -> Result<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
-10
@@ -1,28 +1,28 @@
|
||||
[package]
|
||||
name = "chassisd"
|
||||
description = "Mecanum chassis daemon"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety supervision, and mock mode"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
chassis-config = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
dji-esc-sim = { workspace = true }
|
||||
dji-esc-socketcan = { workspace = true }
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
mecanum = { workspace = true }
|
||||
chassis-config = { workspace = true }
|
||||
sim-harness = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
approx = { workspace = true }
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
approx = { workspace = true }
|
||||
|
||||
@@ -1,197 +0,0 @@
|
||||
//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use mecanum::{Mecanum, Twist};
|
||||
use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive};
|
||||
use tokio::sync::watch;
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval};
|
||||
use tracing::{info, warn};
|
||||
|
||||
/// 底盘控制器。
|
||||
///
|
||||
/// 职责（1kHz 节拍）：
|
||||
/// 1. 读取最新 twist 指令，**指令超时则归零**；
|
||||
/// 2. 速度限幅（保持方向按比例收缩）；
|
||||
/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用；
|
||||
/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。
|
||||
///
|
||||
/// 电机反馈离线安全在驱动层完成（强制 0 电流）。
|
||||
pub struct ChassisController<W> {
|
||||
kinematics: Mecanum,
|
||||
/// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。
|
||||
wheels: [W; 4],
|
||||
/// (max_vx, max_vy, max_omega)。
|
||||
limits: (f32, f32, f32),
|
||||
command_timeout: Duration,
|
||||
}
|
||||
|
||||
impl<W> ChassisController<W>
|
||||
where
|
||||
W: VelocityDrive + MotorFeedback + Disable + 'static,
|
||||
{
|
||||
/// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。
|
||||
#[must_use]
|
||||
pub fn new(
|
||||
kinematics: Mecanum,
|
||||
wheels: [W; 4],
|
||||
limits: (f32, f32, f32),
|
||||
command_timeout: Duration,
|
||||
) -> Self {
|
||||
Self { kinematics, wheels, limits, command_timeout }
|
||||
}
|
||||
|
||||
/// 启动控制循环任务。
|
||||
pub fn spawn(self, commands: watch::Receiver<Twist>, period: Duration) -> JoinHandle<()> {
|
||||
tokio::spawn(self.run(commands, period))
|
||||
}
|
||||
|
||||
async fn run(self, mut commands: watch::Receiver<Twist>, period: Duration) {
|
||||
let mut ticker = interval(period);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut latest = Twist::ZERO;
|
||||
let mut last_command_at: Option<Instant> = None;
|
||||
let mut ticks: u64 = 0;
|
||||
|
||||
loop {
|
||||
ticker.tick().await;
|
||||
let now = Instant::now();
|
||||
|
||||
if commands.has_changed().unwrap_or(false) {
|
||||
latest = *commands.borrow_and_update();
|
||||
last_command_at = Some(now);
|
||||
}
|
||||
let timed_out = match last_command_at {
|
||||
Some(t) => now.duration_since(t) > self.command_timeout,
|
||||
// 尚未收到任何指令：保持静止。
|
||||
None => true,
|
||||
};
|
||||
let twist = if timed_out {
|
||||
Twist::ZERO
|
||||
} else {
|
||||
latest.clamped(self.limits.0, self.limits.1, self.limits.2)
|
||||
};
|
||||
|
||||
let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback());
|
||||
if all_seen_feedback {
|
||||
let wheel_rpms = self.kinematics.wheel_rpms(&twist);
|
||||
for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) {
|
||||
motor.set_velocity(wheel_rpm);
|
||||
}
|
||||
} else {
|
||||
for motor in &self.wheels {
|
||||
motor.disable();
|
||||
}
|
||||
if ticks.is_multiple_of(1000) {
|
||||
warn!("waiting for first feedback from all motors");
|
||||
}
|
||||
}
|
||||
|
||||
ticks += 1;
|
||||
if ticks.is_multiple_of(1000) {
|
||||
self.log_status();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn log_status(&self) {
|
||||
let (rotor_rpm, effort_raw): (Vec<f32>, Vec<i32>) = self
|
||||
.wheels
|
||||
.iter()
|
||||
.map(|m| {
|
||||
let st = m.state();
|
||||
(st.rotor_speed_rpm, i32::from(st.effort_raw))
|
||||
})
|
||||
.unzip();
|
||||
info!(?rotor_rpm, ?effort_raw, "chassis status");
|
||||
}
|
||||
}
|
||||
|
||||
/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。
|
||||
#[must_use]
|
||||
pub fn wrap_wheels<T: Clone, const N: usize>(
|
||||
motors: [T; N],
|
||||
gear_ratios: [f32; N],
|
||||
) -> [OutputShaft<T>; N] {
|
||||
let mut out = Vec::with_capacity(N);
|
||||
let mut gears = gear_ratios.into_iter();
|
||||
for m in motors {
|
||||
let g = gears.next().unwrap_or(1.0);
|
||||
out.push(OutputShaft::new(m, g));
|
||||
}
|
||||
out.try_into().unwrap_or_else(|_| unreachable!())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_core::{Command, MockMotor, MotorState};
|
||||
|
||||
const GEAR: f32 = 19.203;
|
||||
|
||||
fn mock_wheel(id: u8) -> MockMotor {
|
||||
let m = MockMotor::new(id);
|
||||
m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
|
||||
m
|
||||
}
|
||||
|
||||
/// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn controller_drives_wheels_through_capability_traits() {
|
||||
let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
|
||||
let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)];
|
||||
let ctrl = ChassisController::new(
|
||||
kinematics,
|
||||
wrap_wheels(mocks.clone(), [GEAR; 4]),
|
||||
(3.0, 3.0, 6.0),
|
||||
Duration::from_millis(200),
|
||||
);
|
||||
let (tx, rx) = watch::channel(Twist::ZERO);
|
||||
let handle = ctrl.spawn(rx, Duration::from_millis(1));
|
||||
|
||||
let twist = Twist { vx: 1.0, ..Twist::ZERO };
|
||||
tx.send_replace(twist);
|
||||
tokio::time::sleep(Duration::from_millis(5)).await;
|
||||
|
||||
let expected = kinematics.wheel_rpms(&twist);
|
||||
for (i, mock) in mocks.iter().enumerate() {
|
||||
match mock.last_command() {
|
||||
Command::Velocity(rotor_rpm) => {
|
||||
let want = expected[i] * GEAR;
|
||||
assert!(
|
||||
(rotor_rpm - want).abs() < 1e-2,
|
||||
"wheel {i}: rotor_rpm {rotor_rpm} != {want}"
|
||||
);
|
||||
}
|
||||
ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
handle.abort();
|
||||
}
|
||||
|
||||
/// 反馈未到齐时必须全部停用，不得下发速度。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn disables_wheels_until_all_feedback_seen() {
|
||||
let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
|
||||
let ready = mock_wheel(1);
|
||||
let missing = MockMotor::new(2); // 无反馈
|
||||
let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
|
||||
let _ = mocks_ready;
|
||||
let wheels =
|
||||
wrap_wheels([ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [GEAR; 4]);
|
||||
let ctrl =
|
||||
ChassisController::new(kinematics, wheels, (3.0, 3.0, 6.0), Duration::from_millis(200));
|
||||
let (tx, rx) = watch::channel(Twist::ZERO);
|
||||
let handle = ctrl.spawn(rx, Duration::from_millis(1));
|
||||
|
||||
tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
|
||||
tokio::time::sleep(Duration::from_millis(5)).await;
|
||||
|
||||
assert_eq!(ready.last_command(), Command::Disabled);
|
||||
assert_eq!(missing.last_command(), Command::Disabled);
|
||||
|
||||
handle.abort();
|
||||
}
|
||||
}
|
||||
@@ -1,20 +0,0 @@
|
||||
//! `chassisd` 库部分：底盘控制循环与指令源抽象。
|
||||
//!
|
||||
//! 配置 schema 已独立为 [`chassis_config`]（守护进程与整定工具共用一份
|
||||
//! 定义），这里以 `config` 别名重新导出，保持 `chassisd::config::Config`
|
||||
//! 这一历史路径可用。
|
||||
//!
|
||||
//! 二进制入口见 `src/main.rs`。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub use chassis_config as config;
|
||||
|
||||
pub mod controller;
|
||||
pub mod source;
|
||||
|
||||
pub use chassis_config::{
|
||||
CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig,
|
||||
ProtocolFamily,
|
||||
};
|
||||
pub use controller::{ChassisController, wrap_wheels};
|
||||
pub use source::{ChannelSource, TwistSource, ZeroSource, pump_source};
|
||||
+980
-13
File diff suppressed because it is too large
Load Diff
@@ -1,81 +0,0 @@
|
||||
//! 上层速度指令源（twist source）。
|
||||
//!
|
||||
//! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中
|
||||
//! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现
|
||||
//! [`TwistSource`] 即可。
|
||||
|
||||
use std::future::Future;
|
||||
|
||||
use mecanum::Twist;
|
||||
use tokio::sync::{mpsc, watch};
|
||||
|
||||
/// 速度指令源。
|
||||
pub trait TwistSource: Send {
|
||||
/// 取下一条指令；`None` 表示源关闭。
|
||||
fn next_twist(&mut self) -> impl Future<Output = Option<Twist>> + Send;
|
||||
}
|
||||
|
||||
/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。
|
||||
pub struct ZeroSource;
|
||||
|
||||
impl TwistSource for ZeroSource {
|
||||
async fn next_twist(&mut self) -> Option<Twist> {
|
||||
std::future::pending().await
|
||||
}
|
||||
}
|
||||
|
||||
/// 通道指令源（测试 / 简单进程内注入用）。
|
||||
pub struct ChannelSource {
|
||||
rx: mpsc::Receiver<Twist>,
|
||||
}
|
||||
|
||||
impl ChannelSource {
|
||||
/// 由 mpsc 接收端构造。
|
||||
#[must_use]
|
||||
pub const fn new(rx: mpsc::Receiver<Twist>) -> Self {
|
||||
Self { rx }
|
||||
}
|
||||
|
||||
/// 创建一对（发送端, 源）。
|
||||
#[must_use]
|
||||
pub fn pair(bound: usize) -> (mpsc::Sender<Twist>, Self) {
|
||||
let (tx, rx) = mpsc::channel(bound);
|
||||
(tx, Self { rx })
|
||||
}
|
||||
}
|
||||
|
||||
impl TwistSource for ChannelSource {
|
||||
async fn next_twist(&mut self) -> Option<Twist> {
|
||||
self.rx.recv().await
|
||||
}
|
||||
}
|
||||
|
||||
/// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。
|
||||
pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender<Twist>) {
|
||||
while let Some(twist) = source.next_twist().await {
|
||||
if tx.send(twist).is_err() {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn channel_source_pumps_into_watch() {
|
||||
let (tx, source) = ChannelSource::pair(8);
|
||||
let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO);
|
||||
let pump = tokio::spawn(pump_source(source, watch_tx));
|
||||
|
||||
tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap();
|
||||
tokio::time::sleep(std::time::Duration::from_millis(1)).await;
|
||||
assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 });
|
||||
|
||||
// 源关闭 → 泵任务退出。
|
||||
drop(tx);
|
||||
pump.await.unwrap();
|
||||
assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值
|
||||
}
|
||||
}
|
||||
@@ -1,170 +0,0 @@
|
||||
//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机
|
||||
//! → 控制循环 → 麦轮运动学），全平台可跑。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use chassisd::config::Config;
|
||||
use chassisd::controller::{ChassisController, wrap_wheels};
|
||||
use dji_esc::{CanLink, EscBus, EscBusHandle};
|
||||
use dji_esc_sim::spawn_plant;
|
||||
use mecanum::Twist;
|
||||
use motor_core::MotorFeedback;
|
||||
use tokio::sync::watch;
|
||||
use tokio::time::sleep;
|
||||
|
||||
/// 搭建与 `chassisd --mock` 完全同构的软件栈。
|
||||
///
|
||||
/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的
|
||||
/// 指令源），超时测试用配置原值。
|
||||
fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender<Twist>) {
|
||||
let (bus_side, plant_side) = CanLink::pair(512);
|
||||
let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
|
||||
spawn_plant(plant_side, &esc_ids, config.bus_options().period);
|
||||
|
||||
let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap();
|
||||
|
||||
let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
|
||||
let motors: [dji_esc::EscMotor; 4] =
|
||||
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
|
||||
let gears: [f32; 4] = config
|
||||
.motors
|
||||
.iter()
|
||||
.map(|m| m.gear_ratio)
|
||||
.collect::<Vec<_>>()
|
||||
.try_into()
|
||||
.unwrap_or_else(|_| unreachable!("exactly 4 motors"));
|
||||
let controller = ChassisController::new(
|
||||
config.kinematics(),
|
||||
wrap_wheels(motors, gears),
|
||||
(config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
|
||||
Duration::from_millis(command_timeout_ms),
|
||||
);
|
||||
controller.spawn(twist_rx, config.bus_options().period);
|
||||
(bus, twist_tx)
|
||||
}
|
||||
|
||||
async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
|
||||
let mut waited = 0u64;
|
||||
while waited < total_ms {
|
||||
if cond() {
|
||||
return true;
|
||||
}
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
waited += 5;
|
||||
}
|
||||
cond()
|
||||
}
|
||||
|
||||
fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] {
|
||||
bus.motors()
|
||||
.iter()
|
||||
.map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio)
|
||||
.collect::<Vec<_>>()
|
||||
.try_into()
|
||||
.unwrap()
|
||||
}
|
||||
|
||||
/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。
|
||||
const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762);
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn forward_command_drives_all_wheels_equally() {
|
||||
let config = Config::mock_default();
|
||||
let (bus, twist_tx) = stack(&config, 10_000);
|
||||
|
||||
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
|
||||
let target = RPM_PER_MPS;
|
||||
let ok = wait_until(3000, || {
|
||||
wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
|
||||
})
|
||||
.await;
|
||||
let rpm = wheel_rpms(&bus);
|
||||
assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
|
||||
|
||||
// 停车：指令归零后各轮减速到停。
|
||||
twist_tx.send_replace(Twist::ZERO);
|
||||
let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
|
||||
let rpm = wheel_rpms(&bus);
|
||||
assert!(stopped, "未停车: {rpm:?}");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn rotation_command_respects_wheel_signs() {
|
||||
let config = Config::mock_default();
|
||||
let (bus, twist_tx) = stack(&config, 10_000);
|
||||
|
||||
twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
|
||||
// a·ω = (0.2 + 0.25) m/s 的轮线速度。
|
||||
let target = 0.45 * RPM_PER_MPS;
|
||||
let ok = wait_until(3000, || {
|
||||
let rpm = wheel_rpms(&bus);
|
||||
// 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。
|
||||
[
|
||||
(rpm[0] + target).abs(),
|
||||
(rpm[1] - target).abs(),
|
||||
(rpm[2] + target).abs(),
|
||||
(rpm[3] - target).abs(),
|
||||
]
|
||||
.iter()
|
||||
.all(|e| *e < 0.05 * target)
|
||||
})
|
||||
.await;
|
||||
let rpm = wheel_rpms(&bus);
|
||||
assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn command_timeout_stops_chassis() {
|
||||
let config = Config::mock_default();
|
||||
// 指令超时使用配置原值（200ms）。
|
||||
let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
|
||||
|
||||
// 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。
|
||||
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
|
||||
let moving =
|
||||
wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await;
|
||||
assert!(moving, "指令后底盘应当运动");
|
||||
|
||||
sleep(Duration::from_millis(400)).await; // 超过 200ms 超时
|
||||
let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
|
||||
let rpm = wheel_rpms(&bus);
|
||||
assert!(stopped, "指令超时后未停车: {rpm:?}");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn speed_limits_scale_motion() {
|
||||
let mut config = Config::mock_default();
|
||||
config.chassis.max_vx = 1.0; // 限速 1 m/s
|
||||
let (bus, twist_tx) = stack(&config, 10_000);
|
||||
|
||||
twist_tx.send_replace(Twist {
|
||||
vx: 3.0, // 超限指令
|
||||
vy: 2.0,
|
||||
omega: 0.0,
|
||||
});
|
||||
// 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy：
|
||||
// FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。
|
||||
let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
|
||||
let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS;
|
||||
let tol = 0.08 * diag_fast;
|
||||
let ok = wait_until(3000, || {
|
||||
let rpm = wheel_rpms(&bus);
|
||||
[
|
||||
(rpm[0] - diag_slow).abs(),
|
||||
(rpm[1] - diag_fast).abs(),
|
||||
(rpm[2] - diag_fast).abs(),
|
||||
(rpm[3] - diag_slow).abs(),
|
||||
]
|
||||
.iter()
|
||||
.all(|e| *e < tol)
|
||||
})
|
||||
.await;
|
||||
let rpm = wheel_rpms(&bus);
|
||||
assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
[package]
|
||||
name = "delta10-protocol"
|
||||
description = "DETA10 IMU wire protocol"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
imu = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,301 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DETA10 IMU 的 CAN 帧编解码（ARCHITECTURE.md §5.2 / 决策点 **D1**）。
|
||||
//!
|
||||
//! # 为什么这里没有"实现好"的解码
|
||||
//!
|
||||
//! ARCHITECTURE.md §11.4 把「delta10 的 CAN ID 和协议模式」列为**需要人工确认的
|
||||
//! 决策点 D1**（需查阅数据手册），而文档 §12.2 给出的手册链接
|
||||
//! `https://fdisystems.github.io/DETA10_Docs/` **已经 404**，厂商现手册为
|
||||
//! `fdisystems.cn` 上的 PDF。
|
||||
//!
|
||||
//! 在没有手册的前提下臆造字节偏移，比留一个空实现更糟：它会以"看起来能用"
|
||||
//! 的姿态把错误的 roll/pitch/yaw 送进云台闭环。因此本模块：
|
||||
//!
|
||||
//! 1. 把**能确定的部分**做实：帧 ID 匹配、DLC 校验、`ImuData` 组装路径；
|
||||
//! 2. 把**不能确定的部分**参数化为 [`Delta10Layout`]（字段偏移 + 标度）；
|
||||
//! 3. 未配置布局时解码返回 [`ImuError::LayoutUnconfirmed`]（显式、可测、不会
|
||||
//! 静默给错数据）。D1 确认后**只需填一组常量**，无需改动任何调用方。
|
||||
//!
|
||||
//! # 已知的确定事实
|
||||
//!
|
||||
//! - 设备经 CAN 接入，挂在与电机共享的总线上（ARCHITECTURE.md §2.1）；
|
||||
//! - 文档记录了两种协议模式：FDILINK 与「标准」，见 [`Delta10Mode`]；
|
||||
//! - 上层需要的是 [`ImuData`]（accel / gyro / mag / roll / pitch / yaw / 时间戳）。
|
||||
|
||||
use imu::{ImuData, ImuError};
|
||||
use thiserror::Error;
|
||||
|
||||
use can::CanFrame;
|
||||
|
||||
/// 载荷里数值字段的宽度与编码。
|
||||
///
|
||||
/// DETA10 的 FDILink 帧用 `float32`，但一个 8 字节 CAN 帧装不下三个 `f32`，
|
||||
/// 因此真实设备必然是"多帧"或"定点压缩"之一——**这正是 D1 要确认的内容**。
|
||||
/// 把宽度显式建模出来，D1 确认后只需填一张常量表。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum FieldWidth {
|
||||
/// 4 字节小端 `f32`。
|
||||
F32,
|
||||
/// 2 字节小端 `i16`（定点，配合 `scale`）。
|
||||
I16,
|
||||
}
|
||||
|
||||
impl FieldWidth {
|
||||
/// 字节数。
|
||||
#[must_use]
|
||||
pub const fn bytes(self) -> usize {
|
||||
match self {
|
||||
Self::F32 => 4,
|
||||
Self::I16 => 2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 载荷里一个数值字段的定义（偏移 + 宽度 + 标度）。
|
||||
///
|
||||
/// 有了它，"确认手册后要改什么"就只剩一张表。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct FieldLayout {
|
||||
/// 在 8 字节载荷中的起始偏移。
|
||||
pub offset: usize,
|
||||
/// 数值宽度与编码。
|
||||
pub width: FieldWidth,
|
||||
/// 原始值 → 物理量的比例（如 0.001 表示原始值单位是 1/1000）。
|
||||
pub scale: f32,
|
||||
}
|
||||
|
||||
impl FieldLayout {
|
||||
/// 构造。
|
||||
#[must_use]
|
||||
pub const fn new(offset: usize, width: FieldWidth, scale: f32) -> Self {
|
||||
Self { offset, width, scale }
|
||||
}
|
||||
|
||||
/// 从载荷里取一个字段；越界返回 `None`。
|
||||
#[must_use]
|
||||
pub fn read(&self, data: &[u8; 8]) -> Option<f32> {
|
||||
let end = self.offset.checked_add(self.width.bytes())?;
|
||||
if end > data.len() {
|
||||
return None;
|
||||
}
|
||||
let raw = match self.width {
|
||||
FieldWidth::F32 => {
|
||||
let mut bytes = [0u8; 4];
|
||||
bytes.copy_from_slice(&data[self.offset..end]);
|
||||
f32::from_le_bytes(bytes)
|
||||
}
|
||||
FieldWidth::I16 => {
|
||||
let mut bytes = [0u8; 2];
|
||||
bytes.copy_from_slice(&data[self.offset..end]);
|
||||
f32::from(i16::from_le_bytes(bytes))
|
||||
}
|
||||
};
|
||||
Some(raw * self.scale)
|
||||
}
|
||||
}
|
||||
|
||||
/// DETA10 载荷布局（**待 D1 确认**）。
|
||||
///
|
||||
/// 单帧 8 字节装不下 9 个浮点量，因此真实设备要么用多帧、要么用定点压缩。
|
||||
/// 本结构支持"单帧 + 字段子集"的形态；多帧重组待手册确认后按同样思路扩展。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Delta10Layout {
|
||||
/// 横滚角（度）字段。
|
||||
pub roll: FieldLayout,
|
||||
/// 俯仰角（度）字段。
|
||||
pub pitch: FieldLayout,
|
||||
/// 偏航角（度）字段。
|
||||
pub yaw: FieldLayout,
|
||||
}
|
||||
|
||||
/// 协议模式（ARCHITECTURE.md §2.2 记录了两者，先按哪种接需 D1 确认）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum Delta10Mode {
|
||||
/// FDILINK 协议模式。
|
||||
Fdilink,
|
||||
/// 标准模式。
|
||||
Standard,
|
||||
}
|
||||
|
||||
/// DETA10 接入配置。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Delta10Config {
|
||||
/// 反馈帧的 CAN ID（**待 D1 确认**，默认取 `0x100` 占位）。
|
||||
pub can_id: u32,
|
||||
/// 协议模式。
|
||||
pub mode: Delta10Mode,
|
||||
/// 载荷布局；`None` 表示尚未确认，解码会返回
|
||||
/// [`ImuError::LayoutUnconfirmed`]。
|
||||
pub layout: Option<Delta10Layout>,
|
||||
}
|
||||
|
||||
impl Default for Delta10Config {
|
||||
fn default() -> Self {
|
||||
// 0x100 只是占位（ARCHITECTURE.md §10.3 的 robot.toml 里也把它写成
|
||||
// `imu_can_id = 0x100`，同样标注为待确认）。它**不应**被当作事实使用：
|
||||
// 布局为 None，因此默认配置下解码一定报 LayoutUnconfirmed。
|
||||
Self { can_id: 0x100, mode: Delta10Mode::Fdilink, layout: None }
|
||||
}
|
||||
}
|
||||
|
||||
impl Delta10Config {
|
||||
/// 换一个 CAN ID（D1 确认后由配置注入）。
|
||||
#[must_use]
|
||||
pub const fn with_can_id(mut self, can_id: u32) -> Self {
|
||||
self.can_id = can_id;
|
||||
self
|
||||
}
|
||||
|
||||
/// 换协议模式。
|
||||
#[must_use]
|
||||
pub const fn with_mode(mut self, mode: Delta10Mode) -> Self {
|
||||
self.mode = mode;
|
||||
self
|
||||
}
|
||||
|
||||
/// 装上载荷布局（D1 确认后调用）。
|
||||
#[must_use]
|
||||
pub const fn with_layout(mut self, layout: Delta10Layout) -> Self {
|
||||
self.layout = Some(layout);
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// 解码结果：`Ok(None)` = 不是 IMU 帧（正常情况，总线上大部分帧是电机反馈）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// - [`ImuError::BadFrame`]：ID 匹配但长度不足；
|
||||
/// - [`ImuError::LayoutUnconfirmed`]：ID 匹配但布局未确认（D1）；
|
||||
/// - [`ImuError::Checksum`]：字段读取越界（布局与载荷不匹配）。
|
||||
pub fn try_decode(
|
||||
config: &Delta10Config,
|
||||
frame: &CanFrame,
|
||||
timestamp_ns: u64,
|
||||
) -> Result<Option<ImuData>, ImuError> {
|
||||
if frame.id != config.can_id {
|
||||
return Ok(None);
|
||||
}
|
||||
if !frame.is_complete() {
|
||||
return Err(ImuError::BadFrame);
|
||||
}
|
||||
let Some(layout) = config.layout else {
|
||||
return Err(ImuError::LayoutUnconfirmed);
|
||||
};
|
||||
let roll = layout.roll.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
let pitch = layout.pitch.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
let yaw = layout.yaw.read(&frame.data).ok_or(ImuError::Checksum)?;
|
||||
Ok(Some(ImuData {
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
timestamp_ns,
|
||||
// 本帧只承载姿态角；加速度 / 角速度需要额外的帧（待手册确认）
|
||||
..ImuData::default()
|
||||
}))
|
||||
}
|
||||
|
||||
/// 配置错误：把"不确定"和"确定错"分开。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum Delta10ConfigError {
|
||||
/// 布局字段越过 8 字节载荷。
|
||||
#[error("DETA10 字段偏移 {offset} 超出 8 字节载荷")]
|
||||
FieldOutOfRange {
|
||||
/// 越界的偏移。
|
||||
offset: usize,
|
||||
},
|
||||
/// 布局非有限标度。
|
||||
#[error("DETA10 字段标度必须为有限正值")]
|
||||
NonFiniteScale,
|
||||
}
|
||||
|
||||
/// 校验布局能否落在载荷内（D1 确认后填入常量时用）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 字段越界或标度非法时返回错误。
|
||||
pub fn validate_layout(layout: &Delta10Layout) -> Result<(), Delta10ConfigError> {
|
||||
for field in [layout.roll, layout.pitch, layout.yaw] {
|
||||
if !(field.scale.is_finite() && field.scale > 0.0) {
|
||||
return Err(Delta10ConfigError::NonFiniteScale);
|
||||
}
|
||||
if field.offset + field.width.bytes() > 8 {
|
||||
return Err(Delta10ConfigError::FieldOutOfRange { offset: field.offset });
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// 一个**能装进 8 字节**的合成布局：3 × i16 定点（0.001 度/LSB）。
|
||||
///
|
||||
/// 它只用来证明管道可用；真实 DETA10 的偏移/宽度/标度待 D1 确认。
|
||||
fn fitted_layout() -> Delta10Layout {
|
||||
Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::I16, 0.001),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 0.001),
|
||||
yaw: FieldLayout::new(4, FieldWidth::I16, 0.001),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_imu_frames_are_ignored_not_errors() {
|
||||
let config = Delta10Config::default().with_can_id(0x100);
|
||||
let motor = CanFrame::new(0x201, [0; 8]);
|
||||
assert_eq!(try_decode(&config, &motor, 0), Ok(None));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_config_refuses_to_guess_the_layout() {
|
||||
let config = Delta10Config::default().with_can_id(0x100);
|
||||
let imu_frame = CanFrame::new(0x100, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||||
assert_eq!(try_decode(&config, &imu_frame, 7), Err(ImuError::LayoutUnconfirmed));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn short_imu_frames_are_rejected() {
|
||||
let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout());
|
||||
let short = CanFrame::new(0x100, [0; 8]).with_dlc(4);
|
||||
assert_eq!(try_decode(&config, &short, 0), Err(ImuError::BadFrame));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn configured_layout_decodes_attitude_and_stamps_time() {
|
||||
let config = Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout());
|
||||
let mut data = [0u8; 8];
|
||||
// roll = 1.5°, pitch = -2.25°（原始值为毫度）
|
||||
data[0..2].copy_from_slice(&1500i16.to_le_bytes());
|
||||
data[2..4].copy_from_slice(&(-2250i16).to_le_bytes());
|
||||
let frame = CanFrame::new(0x100, data);
|
||||
let decoded = try_decode(&config, &frame, 42).unwrap().unwrap();
|
||||
assert!((decoded.roll - 1.5).abs() < 1e-6, "{}", decoded.roll);
|
||||
assert!((decoded.pitch + 2.25).abs() < 1e-6, "{}", decoded.pitch);
|
||||
assert_eq!(decoded.timestamp_ns, 42);
|
||||
assert!(decoded.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn layout_validation_catches_out_of_range_offsets() {
|
||||
// offset 6 + 4 字节 f32 = 10 > 8：越界
|
||||
let bad = Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::F32, 1.0),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 1.0),
|
||||
yaw: FieldLayout::new(6, FieldWidth::F32, 1.0),
|
||||
};
|
||||
assert!(matches!(
|
||||
validate_layout(&bad),
|
||||
Err(Delta10ConfigError::FieldOutOfRange { offset: 6 })
|
||||
));
|
||||
let zero_scale = Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::I16, 0.0),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 1.0),
|
||||
yaw: FieldLayout::new(4, FieldWidth::I16, 1.0),
|
||||
};
|
||||
assert_eq!(validate_layout(&zero_scale), Err(Delta10ConfigError::NonFiniteScale));
|
||||
assert!(validate_layout(&fitted_layout()).is_ok());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
[package]
|
||||
name = "delta10"
|
||||
description = "DETA10 IMU driver"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
imu = { workspace = true }
|
||||
delta10-protocol = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
@@ -0,0 +1,193 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DETA10 IMU 驱动：把总线上的 IMU 帧变成 [`ImuData`]（ARCHITECTURE.md §5.3）。
|
||||
//!
|
||||
//! 本驱动只负责**收发与筛选**：帧格式与载荷布局的解析在
|
||||
//! [`motor_protocol::delta10`]，那里也记录了为什么载荷布局仍是待确认的决策点 D1。
|
||||
//! 因此本模块在默认配置下会返回 [`ImuError::LayoutUnconfirmed`]——
|
||||
//! 这是**有意的**：宁可明确报错，也不把猜出来的姿态角送进云台闭环。
|
||||
|
||||
use can::CanBus;
|
||||
use delta10_protocol::{self as delta10, Delta10Config};
|
||||
use imu::{Imu, ImuData, ImuError};
|
||||
|
||||
fn now_ns() -> u64 {
|
||||
use std::time::{SystemTime, UNIX_EPOCH};
|
||||
SystemTime::now().duration_since(UNIX_EPOCH).map(|d| d.as_nanos() as u64).unwrap_or_default()
|
||||
}
|
||||
|
||||
/// 挂在任意 [`CanBus`] 上的 DETA10 驱动。
|
||||
///
|
||||
/// 与电机共享总线（ARCHITECTURE.md §2.1），因此 [`Delta10Driver::read`] 会
|
||||
/// **跳过**所有不是 IMU 的帧（总线上大部分帧是电机反馈）。
|
||||
#[derive(Debug)]
|
||||
pub struct Delta10Driver<B> {
|
||||
bus: B,
|
||||
config: Delta10Config,
|
||||
}
|
||||
|
||||
impl<B> Delta10Driver<B> {
|
||||
/// 组装。
|
||||
#[must_use]
|
||||
pub fn new(bus: B, config: Delta10Config) -> Self {
|
||||
Self { bus, config }
|
||||
}
|
||||
|
||||
/// 当前配置（D1 确认后由上层换成带布局的配置）。
|
||||
#[must_use]
|
||||
pub const fn config(&self) -> &Delta10Config {
|
||||
&self.config
|
||||
}
|
||||
|
||||
/// 热更新配置（例如运行期从配置中心注入确认后的布局）。
|
||||
pub fn set_config(&mut self, config: Delta10Config) {
|
||||
self.config = config;
|
||||
}
|
||||
|
||||
/// 取回底层总线。
|
||||
#[must_use]
|
||||
pub fn into_inner(self) -> B {
|
||||
self.bus
|
||||
}
|
||||
}
|
||||
|
||||
impl<B: CanBus> Delta10Driver<B> {
|
||||
/// 阻塞读取下一帧**姿态**数据，跳过非 IMU 帧。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// - 总线关闭 → [`ImuError::Closed`]；
|
||||
/// - 帧 ID 命中但布局未确认 → [`ImuError::LayoutUnconfirmed`]（D1）；
|
||||
/// - 帧不合法 → [`ImuError::BadFrame`]。
|
||||
pub async fn read(&mut self) -> Result<ImuData, ImuError> {
|
||||
loop {
|
||||
let frame = self.bus.recv().await.map_err(|_| ImuError::Closed)?;
|
||||
// 非 IMU 帧（总线上大部分是电机反馈）直接丢弃，继续等下一帧。
|
||||
if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? {
|
||||
return Ok(data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 非阻塞读取：本轮没有 IMU 帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 同 [`Delta10Driver::read`]（只是不会等待）。
|
||||
pub fn try_read(&mut self) -> Result<Option<ImuData>, ImuError> {
|
||||
loop {
|
||||
let frame = match self.bus.try_recv() {
|
||||
Ok(Some(frame)) => frame,
|
||||
Ok(None) => return Ok(None),
|
||||
Err(_) => return Err(ImuError::Closed),
|
||||
};
|
||||
if let Some(data) = delta10::try_decode(&self.config, &frame, now_ns())? {
|
||||
return Ok(Some(data));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<B: CanBus> Imu for Delta10Driver<B> {
|
||||
type Error = ImuError;
|
||||
|
||||
async fn read(&mut self) -> Result<ImuData, Self::Error> {
|
||||
Self::read(self).await
|
||||
}
|
||||
|
||||
fn try_read(&mut self) -> Result<Option<ImuData>, Self::Error> {
|
||||
Self::try_read(self)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use can::CanFrame;
|
||||
use delta10_protocol::{Delta10Layout, FieldLayout, FieldWidth};
|
||||
|
||||
use can::MockBus;
|
||||
|
||||
fn fitted_layout() -> Delta10Layout {
|
||||
Delta10Layout {
|
||||
roll: FieldLayout::new(0, FieldWidth::I16, 0.001),
|
||||
pitch: FieldLayout::new(2, FieldWidth::I16, 0.001),
|
||||
yaw: FieldLayout::new(4, FieldWidth::I16, 0.001),
|
||||
}
|
||||
}
|
||||
|
||||
fn imu_frame(roll_mdeg: i16) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&roll_mdeg.to_le_bytes());
|
||||
CanFrame::new(0x100, data)
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn read_skips_motor_frames_and_returns_attitude() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(CanFrame::new(0x201, [0; 8])); // 电机反馈：必须被跳过
|
||||
bus.inject(CanFrame::new(0x202, [0; 8]));
|
||||
bus.inject(imu_frame(2500));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
let data = driver.read().await.unwrap();
|
||||
assert!((data.roll - 2.5).abs() < 1e-6, "{}", data.roll);
|
||||
assert!(data.is_finite());
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn default_config_refuses_to_guess() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(1000));
|
||||
let mut driver = Delta10Driver::new(bus, Delta10Config::default().with_can_id(0x100));
|
||||
assert_eq!(driver.read().await.unwrap_err(), ImuError::LayoutUnconfirmed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn try_read_polls_without_blocking() {
|
||||
let bus = MockBus::new();
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
assert_eq!(driver.try_read().unwrap(), None, "空总线不是错误");
|
||||
|
||||
driver.into_inner().inject(imu_frame(-1500));
|
||||
// 重新组装以取回总线（inject 需要 &mut）。
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(-1500));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
let data = driver.try_read().unwrap().unwrap();
|
||||
assert!((data.roll + 1.5).abs() < 1e-6, "{}", data.roll);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn closed_bus_is_reported_as_closed() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.close();
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
assert_eq!(driver.read().await.unwrap_err(), ImuError::Closed);
|
||||
assert_eq!(driver.try_read().unwrap_err(), ImuError::Closed);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn works_through_the_imu_trait() {
|
||||
let mut bus = MockBus::new();
|
||||
bus.inject(imu_frame(3000));
|
||||
let mut driver = Delta10Driver::new(
|
||||
bus,
|
||||
Delta10Config::default().with_can_id(0x100).with_layout(fitted_layout()),
|
||||
);
|
||||
// 经 `Imu` trait 调用（证明驱动满足 L1 的抽象边界）。
|
||||
let data: ImuData = Imu::read(&mut driver).await.unwrap();
|
||||
assert!((data.roll - 3.0).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
@@ -1,186 +0,0 @@
|
||||
//! 被控对象集成：用同一组参数的**抖动样本**表达模型不确定性。
|
||||
//!
|
||||
//! 单点模型整定出来的增益往往对增益 / 时间常数 / 延迟 / 摩擦高度敏感
|
||||
//! （真车轮子之间、电量与温度变化都会漂移）。整定应针对一个**集成**做
|
||||
//! 鲁棒优化，而不是针对一条理想曲线做最优化。
|
||||
|
||||
use rand::Rng as _;
|
||||
use rand::SeedableRng as _;
|
||||
use rand::rngs::StdRng;
|
||||
|
||||
use crate::plant::PlantParams;
|
||||
|
||||
/// 集成相对基准参数的抖动幅度（比例，0 表示不抖动）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct PlantTolerances {
|
||||
/// 空载增益 ± 比例。
|
||||
pub gain_pct: f32,
|
||||
/// 时间常数 ± 比例。
|
||||
pub tau_pct: f32,
|
||||
/// 命令延迟在 `0..=delay_ticks_max` 拍内均匀取整。
|
||||
pub delay_ticks_max: u32,
|
||||
/// 摩擦三件套 ± 比例。
|
||||
pub friction_pct: f32,
|
||||
}
|
||||
|
||||
impl Default for PlantTolerances {
|
||||
fn default() -> Self {
|
||||
Self { gain_pct: 0.2, tau_pct: 0.3, delay_ticks_max: 3, friction_pct: 0.5 }
|
||||
}
|
||||
}
|
||||
|
||||
/// 一组被控对象参数（成员之间只有参数差异，模型结构相同）。
|
||||
#[derive(Debug, Clone, PartialEq)]
|
||||
pub struct PlantEnsemble {
|
||||
members: Vec<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));
|
||||
}
|
||||
}
|
||||
@@ -1,258 +0,0 @@
|
||||
//! 离线场景评估：同步、无 I/O、无 async 的闭环重放。
|
||||
//!
|
||||
//! 本模块是自动整定的**求值内核**：给定一组速度环增益、一个被控对象与一条
|
||||
//! 场景，逐拍推进「控制器 → 限幅 → 被控对象」，返回完整 [`Trace`]。
|
||||
//!
|
||||
//! 与 `dji-esc` 的 `EscBus` 刻意保持**同一套物理帧语义**（见
|
||||
//! `tests/harness_parity.rs`）：
|
||||
//!
|
||||
//! ```text
|
||||
//! out = velocity_controller.update(setpoint, measured, dt) // 已按 ±current_limit 限幅
|
||||
//! raw = (out * direction) as i16 // 物理帧（direction = +1）
|
||||
//! cmd = clamp_current(model, raw.clamp(±current_limit)) // 电调满量程再夹一次
|
||||
//! ```
|
||||
//!
|
||||
//! 单次评估约 10^3~10^4 拍纯浮点运算，因此可以在 `rayon` 下并行搜索
|
||||
//! 上千组参数——这是"参数空间搜索"能落地的前提。
|
||||
|
||||
use dji_esc::{EscModel, clamp_current};
|
||||
use motor_core::{VelocityController, VelocityGains};
|
||||
use thiserror::Error;
|
||||
|
||||
use crate::ensemble::PlantEnsemble;
|
||||
use crate::plant::{PlantMotor, PlantParams};
|
||||
use crate::scenario::Scenario;
|
||||
use crate::trace::Trace;
|
||||
|
||||
/// 闭环限幅与电调型号（决定 effort 的可达范围）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct LoopLimits {
|
||||
/// effort 限幅（raw）——同时是 PID 输出钳位点。
|
||||
pub current_limit_raw: i16,
|
||||
/// 电调型号（决定满量程）。
|
||||
pub model: EscModel,
|
||||
}
|
||||
|
||||
impl LoopLimits {
|
||||
/// `GM3508 + C620`、限流 12000 raw（≈14.6 A）——与 `EscConfig::gm3508` 一致。
|
||||
#[must_use]
|
||||
pub const fn gm3508() -> Self {
|
||||
Self { current_limit_raw: 12_000, model: EscModel::C620 }
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for LoopLimits {
|
||||
fn default() -> Self {
|
||||
Self::gm3508()
|
||||
}
|
||||
}
|
||||
|
||||
/// 离线评估失败原因。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum SimError {
|
||||
/// 增益含 NaN / inf（`Pid::new` 会 panic，必须在此拦下）。
|
||||
#[error("gains contain NaN or infinity")]
|
||||
NonFiniteGains,
|
||||
/// 场景或限幅退化（`dt <= 0`、总时长为 0、限流为 0 等）。
|
||||
#[error("degenerate scenario or limits")]
|
||||
DegenerateScenario,
|
||||
/// 数值发散（转速超出被控对象量程 100 倍）。
|
||||
#[error("simulation diverged at step {step}")]
|
||||
Diverged {
|
||||
/// 发散发生的采样序号。
|
||||
step: usize,
|
||||
},
|
||||
}
|
||||
|
||||
/// 在单台被控对象上重放一条场景。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 增益非有限、场景退化或数值发散时返回 [`SimError`]——调用方（整定器）
|
||||
/// 应把它折算成罚值，而不是把非法参数交给控制器。
|
||||
pub fn simulate(
|
||||
gains: VelocityGains,
|
||||
plant: &PlantParams,
|
||||
scenario: &Scenario,
|
||||
limits: LoopLimits,
|
||||
) -> Result<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(),
|
||||
"摩擦 + 延迟只会拖慢跟随"
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -1,39 +0,0 @@
|
||||
//! DJI 电调被控对象仿真：模型、场景与**离线评估 harness**。
|
||||
//!
|
||||
//! 本 crate 从 `dji-esc` 剥离而来，只负责"电调对面的那台电机"以及如何对它
|
||||
//! 做实验，不碰任何真实 I/O——总线接口是一对 `CanLink` 通道。
|
||||
//!
|
||||
//! # 模块职责
|
||||
//!
|
||||
//! - [`plant`]：被控对象模型（一阶转速 + 静/库仑/黏滞摩擦 + 命令延迟 + 负载）；
|
||||
//! - [`ensemble`]：参数集成（域随机化），表达模型不确定性；
|
||||
//! - [`scenario`]：设定值曲线与采样节拍（阶跃 / 斜坡 / 阶梯 / 升降）；
|
||||
//! - [`trace`]：试验轨迹（设定值、转速、effort、饱和标志）；
|
||||
//! - [`harness`]：**同步**闭环重放——自动整定的求值内核（纯 CPU、可并行）；
|
||||
//! - [`rig`]：**异步**假电调组，挂在 [`CanLink`] 上供 `EscBus` 联调。
|
||||
//!
|
||||
//! # 两条路径为什么都要有
|
||||
//!
|
||||
//! - `rig` 用于端到端联调（`chassisd --mock`、闭环集成测试），语义是
|
||||
//! "挂在总线上的假电调"；
|
||||
//! - `harness` 用于参数搜索，语义是"一条场景的数值积分"，单次评估只有
|
||||
//! 浮点运算、没有 tokio 任务与通道，因此可以在 `rayon` 下并行跑上千组
|
||||
//! 参数。两者的物理帧语义由 `tests/harness_parity.rs` 对齐。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod ensemble;
|
||||
pub mod harness;
|
||||
pub mod plant;
|
||||
pub mod rig;
|
||||
pub mod scenario;
|
||||
pub mod trace;
|
||||
|
||||
pub use ensemble::{PlantEnsemble, PlantTolerances};
|
||||
pub use harness::{LoopLimits, SimError, simulate, simulate_ensemble};
|
||||
pub use plant::{PlantMotor, PlantParams};
|
||||
pub use rig::{spawn_plant, spawn_plant_with};
|
||||
pub use scenario::{Scenario, SetpointProfile};
|
||||
pub use trace::{Trace, Window};
|
||||
|
||||
/// 复用的链路类型（`dji-esc` 的协议层与假电调通过它对接）。
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
@@ -1,361 +0,0 @@
|
||||
//! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。
|
||||
//!
|
||||
//! 电调坐标系（正电流 → 正转速），逐拍离散（`dt` 秒）：
|
||||
//!
|
||||
//! 1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才真正作用（模拟
|
||||
//! 链路 + 采样延迟；`0` 表示立即作用）。
|
||||
//! 2. **摩擦**：先由 `u` 与当前运动状态算出净转矩输入
|
||||
//! （静摩擦死区 → 库仑摩擦 → 黏滞阻尼，见 [`PlantMotor::step`]）。
|
||||
//! 3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
|
||||
//! 转速以时间常数 `tau` 一阶趋近（`alpha = 1 − exp(−dt/tau)`）。
|
||||
//! 4. **编码器积分与量化**：机械角按转速积分并对 8192 取模；反馈帧是
|
||||
//! `EscFeedback`，因此角度被截断为 `u16`、转速被截断为 `i16`——量化是
|
||||
//! 协议类型自带的，不需要额外开关。
|
||||
//!
|
||||
//! [`PlantParams::gm3508`] 复现最初的**无摩擦一阶模型**（`chassisd --mock`
|
||||
//! 与闭环测试依赖它，行为逐位不变）；[`PlantParams::gm3508_bench`] 是带摩擦
|
||||
//! 与延迟的**台架量级**预设，供自动整定使用。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use dji_esc::{ENCODER_TICKS, EscFeedback};
|
||||
|
||||
/// 转速绝对值低于该值（rpm）视为静止——静摩擦判别用。
|
||||
const STICK_RPM_EPS: f32 = 1.0;
|
||||
|
||||
/// 被控对象参数（电调坐标系）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct PlantParams {
|
||||
/// 空载增益（rpm / raw）：`GM3508` 实测约 0.565（≈ 9254 / 16384）。
|
||||
pub gain_rpm_per_raw: f32,
|
||||
/// 机电时间常数（秒）。
|
||||
pub tau_s: f32,
|
||||
/// 转速上限（rpm）。
|
||||
pub max_rpm: f32,
|
||||
/// 静摩擦阈值（raw）：静止且 |u| ≤ 该值时不产生任何运动（stick）。
|
||||
pub static_raw: f32,
|
||||
/// 库仑摩擦（raw）：与运动方向相反的恒定阻力。
|
||||
pub coulomb_raw: f32,
|
||||
/// 黏滞阻尼（raw / rpm）。
|
||||
pub viscous_raw_per_rpm: f32,
|
||||
/// 命令传输延迟（拍，`dt` 的整数倍）。
|
||||
pub delay_ticks: u32,
|
||||
/// 恒定外载（折算成 rpm，直接被目标转速减去）。
|
||||
pub load_rpm: f32,
|
||||
}
|
||||
|
||||
impl PlantParams {
|
||||
/// 无摩擦、无延迟的理想一阶模型（与历史 `PlantMotor::gm3508` 逐位一致）。
|
||||
#[must_use]
|
||||
pub const fn gm3508() -> Self {
|
||||
Self {
|
||||
gain_rpm_per_raw: 9254.0 / 16384.0,
|
||||
tau_s: 0.12,
|
||||
max_rpm: 9254.0,
|
||||
static_raw: 0.0,
|
||||
coulomb_raw: 0.0,
|
||||
viscous_raw_per_rpm: 0.0,
|
||||
delay_ticks: 0,
|
||||
load_rpm: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 台架量级预设：在 [`Self::gm3508`] 上叠加静摩擦 / 库仑摩擦 / 黏滞阻尼
|
||||
/// 与 2 拍命令延迟。
|
||||
///
|
||||
/// 摩擦量级取自 `docs/architecture.md` 与台架记录的空载实测（起动需
|
||||
/// 400~600 raw），**是量级估计而非辨识结果**；需要更贴合实车时可用
|
||||
/// [`Self::with_friction`] / [`Self::with_delay_ticks`] 覆盖。
|
||||
#[must_use]
|
||||
pub const fn gm3508_bench() -> Self {
|
||||
let mut params = Self::gm3508();
|
||||
params.static_raw = 450.0;
|
||||
params.coulomb_raw = 500.0;
|
||||
params.viscous_raw_per_rpm = 0.05;
|
||||
params.delay_ticks = 2;
|
||||
params
|
||||
}
|
||||
|
||||
/// 设置摩擦三件套（静摩擦阈值 / 库仑 / 黏滞）。
|
||||
#[must_use]
|
||||
pub const fn with_friction(
|
||||
mut self,
|
||||
static_raw: f32,
|
||||
coulomb_raw: f32,
|
||||
viscous_raw_per_rpm: f32,
|
||||
) -> Self {
|
||||
self.static_raw = static_raw;
|
||||
self.coulomb_raw = coulomb_raw;
|
||||
self.viscous_raw_per_rpm = viscous_raw_per_rpm;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置命令延迟（拍）。
|
||||
#[must_use]
|
||||
pub const fn with_delay_ticks(mut self, ticks: u32) -> Self {
|
||||
self.delay_ticks = ticks;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置恒定外载（rpm）。
|
||||
#[must_use]
|
||||
pub const fn with_load_rpm(mut self, rpm: f32) -> Self {
|
||||
self.load_rpm = rpm;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PlantMotor {
|
||||
params: PlantParams,
|
||||
speed_rpm: f32,
|
||||
angle_raw: f32,
|
||||
/// 命令延迟 FIFO（`delay_ticks == 0` 时恒为空）。
|
||||
delay_line: VecDeque<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, "复位后角度归零");
|
||||
}
|
||||
}
|
||||
@@ -1,147 +0,0 @@
|
||||
//! 异步 plant rig：把被控对象挂到一对 [`CanLink`] 通道上。
|
||||
//!
|
||||
//! 与真实电调对调位置的"假电调"：消费命令帧（`0x200` / `0x1FF`），按
|
||||
//! `tick` 节拍产出反馈帧。`EscBus` 完全看不出区别，因此
|
||||
//! **协议 → 驱动 → 控制回路**的整条链路都能在无硬件环境下跑通
|
||||
//! （`chassisd --mock`、闭环集成测试）。
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{
|
||||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink, encode_feedback_frame,
|
||||
};
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
use tracing::trace;
|
||||
|
||||
use crate::plant::{PlantMotor, PlantParams};
|
||||
|
||||
/// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。
|
||||
///
|
||||
/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可（丢弃句柄 = 让任务独立运行）。
|
||||
pub fn spawn_plant(link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
|
||||
let specs = esc_ids.iter().map(|&id| (id, PlantParams::gm3508()));
|
||||
spawn_plant_with(link, specs, tick)
|
||||
}
|
||||
|
||||
/// 用逐电调的参数启动假电调组（异构轮子 / 域随机化场景）。
|
||||
pub fn spawn_plant_with(
|
||||
mut link: CanLink,
|
||||
specs: impl IntoIterator<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();
|
||||
}
|
||||
}
|
||||
@@ -1,236 +0,0 @@
|
||||
//! 场景定义：设定值曲线 + 采样节拍。
|
||||
//!
|
||||
//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、
|
||||
//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放，
|
||||
//! 因此整定结果可以跨后端比较。
|
||||
|
||||
/// 设定值曲线。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum SetpointProfile {
|
||||
/// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。
|
||||
Step,
|
||||
/// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。
|
||||
RampAndHold,
|
||||
/// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。
|
||||
Staircase {
|
||||
/// 阶梯数（≥ 1）。
|
||||
levels: u32,
|
||||
},
|
||||
/// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。
|
||||
UpDown,
|
||||
}
|
||||
|
||||
/// 一次试验的设定值与时间规格。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Scenario {
|
||||
/// 控制 / 采样节拍（秒）。
|
||||
pub dt_s: f32,
|
||||
/// 斜坡时间（秒）；`0` 表示瞬时。
|
||||
pub ramp_s: f32,
|
||||
/// 保持时间（秒）。
|
||||
pub hold_s: f32,
|
||||
/// 目标转子转速（rpm）。
|
||||
pub target_rpm: f32,
|
||||
/// 设定值曲线。
|
||||
pub profile: SetpointProfile,
|
||||
}
|
||||
|
||||
impl Scenario {
|
||||
/// 默认控制节拍（1 kHz，与 C620 命令周期一致）。
|
||||
pub const DEFAULT_DT_S: f32 = 0.001;
|
||||
|
||||
/// 阶跃 + 保持。
|
||||
#[must_use]
|
||||
pub const fn step(target_rpm: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s: 0.0,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::Step,
|
||||
}
|
||||
}
|
||||
|
||||
/// 斜坡 + 保持（台架默认整定场景）。
|
||||
#[must_use]
|
||||
pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::RampAndHold,
|
||||
}
|
||||
}
|
||||
|
||||
/// 换一个采样节拍。
|
||||
#[must_use]
|
||||
pub const fn with_dt_s(mut self, dt_s: f32) -> Self {
|
||||
self.dt_s = dt_s;
|
||||
self
|
||||
}
|
||||
|
||||
/// 换一条设定值曲线。
|
||||
#[must_use]
|
||||
pub const fn with_profile(mut self, profile: SetpointProfile) -> Self {
|
||||
self.profile = profile;
|
||||
self
|
||||
}
|
||||
|
||||
/// 场景总时长（秒）。
|
||||
#[must_use]
|
||||
pub fn total_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => self.hold_s,
|
||||
SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => {
|
||||
self.ramp_s + self.hold_s
|
||||
}
|
||||
SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s),
|
||||
}
|
||||
}
|
||||
|
||||
/// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。
|
||||
#[must_use]
|
||||
pub fn samples(&self) -> usize {
|
||||
let total = self.total_s();
|
||||
let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0;
|
||||
let total_ok = total.is_finite() && total > 0.0;
|
||||
if !dt_ok || !total_ok {
|
||||
return 1;
|
||||
}
|
||||
((total / self.dt_s).round() as usize).max(1)
|
||||
}
|
||||
|
||||
/// `t_s` 时刻的设定值（rpm）。
|
||||
#[must_use]
|
||||
pub fn setpoint_at(&self, t_s: f32) -> f32 {
|
||||
let target = self.target_rpm;
|
||||
let ramp = self.ramp_s;
|
||||
match self.profile {
|
||||
SetpointProfile::Step => target,
|
||||
SetpointProfile::RampAndHold => {
|
||||
if ramp > 0.0 && t_s < ramp {
|
||||
target * (t_s / ramp)
|
||||
} else {
|
||||
target
|
||||
}
|
||||
}
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let levels = levels.max(1);
|
||||
let total = ramp + self.hold_s;
|
||||
if total <= 0.0 {
|
||||
return target;
|
||||
}
|
||||
let rung_s = total / levels as f32;
|
||||
let index = (t_s / rung_s).floor().max(0.0) as u32;
|
||||
target * ((index.min(levels - 1) + 1) as f32 / levels as f32)
|
||||
}
|
||||
SetpointProfile::UpDown => {
|
||||
let up_end = ramp;
|
||||
let hold_end = up_end + self.hold_s;
|
||||
let down_end = hold_end + ramp;
|
||||
if t_s < up_end {
|
||||
if ramp > 0.0 { target * (t_s / ramp) } else { target }
|
||||
} else if t_s < hold_end {
|
||||
target
|
||||
} else if t_s < down_end {
|
||||
let progress = (t_s - hold_end) / ramp;
|
||||
target * (1.0 - progress)
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。
|
||||
#[must_use]
|
||||
pub fn steady_start_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => 0.0,
|
||||
SetpointProfile::RampAndHold => self.ramp_s,
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let level_s = self.level_s(levels);
|
||||
((levels.max(1) - 1) as f32) * level_s
|
||||
}
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的终点（秒）。
|
||||
#[must_use]
|
||||
pub fn steady_end_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
// 升降场景只评价上升段后的保持段。
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
_ => self.total_s(),
|
||||
}
|
||||
}
|
||||
|
||||
/// 阶梯的单级时长（秒）。
|
||||
fn level_s(&self, levels: u32) -> f32 {
|
||||
let levels = levels.max(1);
|
||||
let total = self.ramp_s + self.hold_s;
|
||||
if total > 0.0 { total / levels as f32 } else { 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
|
||||
#[track_caller]
|
||||
fn assert_close(actual: f32, expected: f32) {
|
||||
assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn step_is_immediate_and_lasts_hold() {
|
||||
let s = Scenario::step(1000.0, 0.5);
|
||||
assert!((s.total_s() - 0.5).abs() < 1e-6);
|
||||
assert_close(s.setpoint_at(0.0), 1000.0);
|
||||
assert_close(s.setpoint_at(0.4), 1000.0);
|
||||
assert_eq!(s.samples(), 500);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ramp_is_linear_then_holds() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0);
|
||||
assert_close(s.setpoint_at(0.0), 0.0);
|
||||
assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 1000.0);
|
||||
assert_close(s.setpoint_at(1.1), 1000.0);
|
||||
assert!((s.total_s() - 1.2).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn staircase_rises_in_equal_levels() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0)
|
||||
.with_profile(SetpointProfile::Staircase { levels: 4 });
|
||||
assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3);
|
||||
// 越界采样被夹在最后一级。
|
||||
assert_close(s.setpoint_at(5.0), 1000.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn up_down_returns_to_zero() {
|
||||
let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown);
|
||||
assert!((s.total_s() - 0.6).abs() < 1e-6);
|
||||
assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 600.0);
|
||||
assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.59), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_scenario_reports_one_sample() {
|
||||
let s = Scenario::step(1.0, 0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
let s = Scenario::step(1.0, 1.0).with_dt_s(0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
}
|
||||
}
|
||||
@@ -1,230 +0,0 @@
|
||||
//! 试验轨迹：一次场景重放的全部采样。
|
||||
|
||||
/// 一条完整试验轨迹（各列等长，索引即采样序号）。
|
||||
#[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);
|
||||
}
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
[package]
|
||||
name = "dji-esc-slcan"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "SLCAN serial backend (e.g. /dev/ttyACM0) bridging to can-link channels"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
slcan = "0.1.3"
|
||||
serial = "0.4"
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
[package]
|
||||
name = "dji-esc-socketcan"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "SocketCAN backend bridging real Linux CAN interfaces to can-link channels"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
socketcan = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dev-dependencies]
|
||||
can-link = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
@@ -1,96 +0,0 @@
|
||||
//! 真实总线桥接实现（Linux）。
|
||||
|
||||
use std::io;
|
||||
use std::sync::Arc;
|
||||
|
||||
use can_link::{CanFrame as LinkFrame, CanLink};
|
||||
use socketcan::tokio::CanSocket;
|
||||
use socketcan::{
|
||||
CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _,
|
||||
SocketOptions as _,
|
||||
};
|
||||
use tokio::sync::mpsc;
|
||||
use tracing::{debug, error};
|
||||
|
||||
/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。
|
||||
const CHANNEL_BOUND: usize = 512;
|
||||
|
||||
/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。
|
||||
///
|
||||
/// 内部：
|
||||
/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208，
|
||||
/// 非反馈帧再由协议层解码拒绝，双保险）；
|
||||
/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 权限不足等 `io::Error`。
|
||||
pub fn open_link(interface: &str) -> io::Result<CanLink> {
|
||||
let socket = CanSocket::open(interface)?;
|
||||
socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?;
|
||||
let socket = Arc::new(socket);
|
||||
|
||||
let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
|
||||
let CanLink { outgoing_tx, incoming_rx } = bridge_side;
|
||||
|
||||
tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx));
|
||||
tokio::spawn(tx_bridge(socket, incoming_rx));
|
||||
Ok(bus_side)
|
||||
}
|
||||
|
||||
/// socket → 通道：真实总线反馈进入协议 bus。
|
||||
async fn rx_bridge(socket: Arc<CanSocket>, tx: mpsc::Sender<LinkFrame>) {
|
||||
loop {
|
||||
match socket.read_frame().await {
|
||||
Ok(frame) => {
|
||||
if let Some(frame) = to_link_frame(&frame) {
|
||||
if tx.send(frame).await.is_err() {
|
||||
debug!("bus side closed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(e) => {
|
||||
// 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程，
|
||||
// 由 systemd 拉起重连。
|
||||
error!(error = %e, "CAN read failed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 通道 → socket：协议 bus 命令写入真实总线。
|
||||
async fn tx_bridge(socket: Arc<CanSocket>, mut rx: mpsc::Receiver<LinkFrame>) {
|
||||
while let Some(frame) = rx.recv().await {
|
||||
match to_socketcan_frame(&frame) {
|
||||
Some(sc_frame) => {
|
||||
if let Err(e) = socket.write_frame(sc_frame).await {
|
||||
error!(error = %e, "CAN write failed, tx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
None => {
|
||||
debug!(id = frame.id, "drop frame with non-standard CAN id");
|
||||
}
|
||||
}
|
||||
}
|
||||
debug!("bus side closed, tx bridge exiting");
|
||||
}
|
||||
|
||||
/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。
|
||||
fn to_link_frame(frame: &ScFrame) -> Option<LinkFrame> {
|
||||
match frame {
|
||||
ScFrame::Data(df) if df.data().len() == 8 => {
|
||||
Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?))
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。
|
||||
fn to_socketcan_frame(frame: &LinkFrame) -> Option<ScFrame> {
|
||||
let id = socketcan::StandardId::new(frame.id as u16)?;
|
||||
let df = CanDataFrame::new(id, &frame.data)?;
|
||||
Some(ScFrame::Data(df))
|
||||
}
|
||||
@@ -1,11 +0,0 @@
|
||||
//! SocketCAN 桥接：把真实 Linux CAN 接口接入 [`can_link::CanLink`] 通道对。
|
||||
//!
|
||||
//! 本 crate 主体仅在 Linux 下编译（依赖 socketcan）。其他平台编译为空壳，
|
||||
//! 使整个 workspace 可以在任意开发机上构建与测试；实车部署目标板均为
|
||||
//! Linux。
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
mod bridge;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
pub use bridge::open_link;
|
||||
@@ -1,249 +0,0 @@
|
||||
//! DJI 电调（C620 / C610）CAN 协议编解码。
|
||||
//!
|
||||
//! 帧格式（大端）：
|
||||
//!
|
||||
//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）：
|
||||
//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令，
|
||||
//! 槽位按组内电调顺序。
|
||||
//! - 反馈（`0x200 + 电调ID`）：
|
||||
//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。
|
||||
//!
|
||||
//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在
|
||||
//! [`crate::bus`] 中按节拍调用。
|
||||
|
||||
use crate::frame::{
|
||||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE,
|
||||
};
|
||||
use thiserror::Error;
|
||||
|
||||
/// 电调 ID 下限。
|
||||
pub const MIN_ESC_ID: u8 = 1;
|
||||
/// 电调 ID 上限。
|
||||
pub const MAX_ESC_ID: u8 = 8;
|
||||
|
||||
/// 电调型号决定电流标度。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum EscModel {
|
||||
/// C620（配 GM3508）：±16384 ↔ ±20 A。
|
||||
C620,
|
||||
/// C610（配 M2006）：±10000 ↔ ±10 A。
|
||||
///
|
||||
/// 接入前需实测核对。
|
||||
C610,
|
||||
}
|
||||
|
||||
impl EscModel {
|
||||
/// 满量程电流命令（raw）。
|
||||
#[must_use]
|
||||
pub const fn max_current_raw(self) -> i16 {
|
||||
match self {
|
||||
Self::C620 => 16384,
|
||||
Self::C610 => 10_000,
|
||||
}
|
||||
}
|
||||
|
||||
/// 满量程电流（安培）。
|
||||
#[must_use]
|
||||
pub const fn max_current_amp(self) -> f32 {
|
||||
match self {
|
||||
Self::C620 => 20.0,
|
||||
Self::C610 => 10.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 安培 → raw（限幅）。
|
||||
#[must_use]
|
||||
pub fn raw_from_amp(self, amp: f32) -> i16 {
|
||||
let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp();
|
||||
(raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16)
|
||||
.clamp(-self.max_current_raw(), self.max_current_raw())
|
||||
}
|
||||
|
||||
/// raw → 安培。
|
||||
#[must_use]
|
||||
pub fn amp_from_raw(self, raw: i16) -> f32 {
|
||||
f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw())
|
||||
}
|
||||
}
|
||||
|
||||
/// 一帧电调反馈（原始、电调侧，未经方向修正）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct EscFeedback {
|
||||
/// 转子机械角原始值（0..8191，对应 0..360°）。
|
||||
pub angle_raw: u16,
|
||||
/// 转子转速（rpm，电调侧符号）。
|
||||
pub speed_rpm: i16,
|
||||
/// 转矩电流 raw（电调侧符号）。
|
||||
pub current_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
}
|
||||
|
||||
/// 帧解码错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum DecodeError {
|
||||
/// 帧不属于电调反馈范围（0x201..=0x208）。
|
||||
#[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")]
|
||||
NotFeedback {
|
||||
/// 收到的帧 ID。
|
||||
id: u32,
|
||||
},
|
||||
}
|
||||
|
||||
/// 电调 ID → 其电流命令所在的组控制帧 ID。
|
||||
///
|
||||
/// 返回 `None` 表示 ID 越界。
|
||||
#[must_use]
|
||||
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
|
||||
match esc_id {
|
||||
1..=4 => Some(COMMAND_FRAME_ID_PRIMARY),
|
||||
5..=8 => Some(COMMAND_FRAME_ID_SECONDARY),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 将 raw 电流限幅到电调满量程。
|
||||
#[must_use]
|
||||
pub const fn clamp_current(model: EscModel, raw: i16) -> i16 {
|
||||
if raw > model.max_current_raw() {
|
||||
model.max_current_raw()
|
||||
} else if raw < -model.max_current_raw() {
|
||||
-model.max_current_raw()
|
||||
} else {
|
||||
raw
|
||||
}
|
||||
}
|
||||
|
||||
/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。
|
||||
///
|
||||
/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。
|
||||
/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成
|
||||
/// （见 [`clamp_current`]）。
|
||||
#[must_use]
|
||||
pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
for (slot, raw) in currents.iter().enumerate() {
|
||||
data[slot * 2] = (raw >> 8) as u8;
|
||||
data[slot * 2 + 1] = (raw & 0xFF) as u8;
|
||||
}
|
||||
CanFrame::new(frame_id, data)
|
||||
}
|
||||
|
||||
/// 组装一帧电调反馈（仿真器 / 测试用）。
|
||||
#[must_use]
|
||||
pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
|
||||
data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
|
||||
data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
|
||||
data[6] = feedback.temperature_c as u8;
|
||||
CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
|
||||
}
|
||||
|
||||
/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。
|
||||
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
|
||||
if frame.id <= FEEDBACK_FRAME_ID_BASE
|
||||
|| frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID)
|
||||
{
|
||||
return Err(DecodeError::NotFeedback { id: frame.id });
|
||||
}
|
||||
let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8;
|
||||
Ok((
|
||||
esc_id,
|
||||
EscFeedback {
|
||||
angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]),
|
||||
speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]),
|
||||
current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]),
|
||||
temperature_c: frame.data[6] as i8,
|
||||
},
|
||||
))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn command_frame_id_routing() {
|
||||
assert_eq!(command_frame_id(1), Some(0x200));
|
||||
assert_eq!(command_frame_id(4), Some(0x200));
|
||||
assert_eq!(command_frame_id(5), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(8), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(0), None);
|
||||
assert_eq!(command_frame_id(9), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encode_current_frame_bytes() {
|
||||
let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]);
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]);
|
||||
|
||||
let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]);
|
||||
assert_eq!(frame.id, 0x1FF);
|
||||
assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_known_bytes() {
|
||||
let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||||
let (esc_id, fb) = decode_feedback(&frame).unwrap();
|
||||
assert_eq!(esc_id, 5);
|
||||
assert_eq!(fb.angle_raw, 8191);
|
||||
assert_eq!(fb.speed_rpm, -364);
|
||||
assert_eq!(fb.current_raw, 123);
|
||||
assert_eq!(fb.temperature_c, 45);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_rejects_non_feedback_ids() {
|
||||
for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] {
|
||||
let frame = CanFrame::new(id, [0; 8]);
|
||||
assert_eq!(
|
||||
decode_feedback(&frame),
|
||||
Err(DecodeError::NotFeedback { id }),
|
||||
"id = 0x{id:03X}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_roundtrip_all_ids() {
|
||||
let fb = EscFeedback {
|
||||
angle_raw: 8191,
|
||||
speed_rpm: -9254,
|
||||
current_raw: -16384,
|
||||
temperature_c: -40,
|
||||
};
|
||||
for esc_id in MIN_ESC_ID..=MAX_ESC_ID {
|
||||
let frame = encode_feedback_frame(esc_id, &fb);
|
||||
assert_eq!(frame.id, 0x200 + u32::from(esc_id));
|
||||
assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn amp_conversion() {
|
||||
let c620 = EscModel::C620;
|
||||
assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4);
|
||||
assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4);
|
||||
assert_eq!(c620.raw_from_amp(20.0), 16384);
|
||||
assert_eq!(c620.raw_from_amp(-5.0), -4096);
|
||||
// 超量程限幅。
|
||||
assert_eq!(c620.raw_from_amp(99.0), 16384);
|
||||
let c610 = EscModel::C610;
|
||||
assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4);
|
||||
assert_eq!(c610.raw_from_amp(10.0), 10000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn clamp_current_respects_model() {
|
||||
assert_eq!(clamp_current(EscModel::C620, 20000), 16384);
|
||||
assert_eq!(clamp_current(EscModel::C610, 15000), 10000);
|
||||
assert_eq!(clamp_current(EscModel::C620, 1000), 1000);
|
||||
}
|
||||
}
|
||||
@@ -1,13 +0,0 @@
|
||||
//! DJI 电调协议帧 ID 常量。
|
||||
//!
|
||||
//! 帧与链路类型（[`CanFrame`] / [`CanLink`]）由 [`can_link`] 提供，
|
||||
//! 本模块只保留 **C620/C610 电流协议族**的 ID 约定。
|
||||
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
|
||||
/// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。
|
||||
pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200;
|
||||
/// 控制帧 ID：电调 5–8 的电流命令。
|
||||
pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF;
|
||||
/// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。
|
||||
pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200;
|
||||
@@ -1,29 +0,0 @@
|
||||
//! DJI RoboMaster 电调（C620/C610）CAN 协议与异步驱动。
|
||||
//!
|
||||
//! 分层：
|
||||
//!
|
||||
//! - [`codec`]：纯函数帧编解码（C620 手册协议）；
|
||||
//! - [`turn`]：单圈编码器 → 连续多圈角；
|
||||
//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）；
|
||||
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全。
|
||||
//!
|
||||
//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction`
|
||||
//! 完成方向修正。
|
||||
//!
|
||||
//! 被控对象仿真（假电调）**不在本 crate**：见 `dji-esc-sim`。本 crate 只描述
|
||||
//! 真实电调在总线上的行为，不假设对面是谁。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod bus;
|
||||
pub mod codec;
|
||||
pub mod frame;
|
||||
pub mod turn;
|
||||
|
||||
pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor};
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
pub use codec::{
|
||||
DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id,
|
||||
decode_feedback, encode_current_frame, encode_feedback_frame,
|
||||
};
|
||||
pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE};
|
||||
pub use turn::{ENCODER_TICKS, TurnTracker};
|
||||
@@ -1,134 +0,0 @@
|
||||
//! 多圈累计：单圈编码器原始角 → 连续圈数。
|
||||
|
||||
/// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// 半圈阈值：帧间角变化超过它视为发生回绕。
|
||||
const HALF_TURN_TICKS: i32 = 4096;
|
||||
|
||||
/// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。
|
||||
///
|
||||
/// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约
|
||||
/// 9300 rpm ≈ 0.155 圈/ms，余量充足。
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct TurnTracker {
|
||||
revolutions: i32,
|
||||
prev_raw: Option<u16>,
|
||||
last_angle_rev: f32,
|
||||
}
|
||||
|
||||
impl TurnTracker {
|
||||
/// 创建跟踪器。
|
||||
#[must_use]
|
||||
pub const fn new() -> Self {
|
||||
Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 }
|
||||
}
|
||||
|
||||
/// 送入新的原始角，返回连续累计角（圈）。
|
||||
pub fn update(&mut self, raw: u16) -> f32 {
|
||||
if let Some(prev) = self.prev_raw {
|
||||
let delta = i32::from(raw) - i32::from(prev);
|
||||
if delta > HALF_TURN_TICKS {
|
||||
// 正向跨越 8191 → 0。
|
||||
self.revolutions -= 1;
|
||||
} else if delta < -HALF_TURN_TICKS {
|
||||
// 反向跨越 0 → 8191。
|
||||
self.revolutions += 1;
|
||||
}
|
||||
}
|
||||
self.prev_raw = Some(raw);
|
||||
self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0;
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 最近一次 [`TurnTracker::update`] 的返回值。
|
||||
#[must_use]
|
||||
pub const fn last_angle_rev(&self) -> f32 {
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 清零回到初始状态。
|
||||
pub fn reset(&mut self) {
|
||||
*self = Self::new();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn first_reading_is_raw_fraction() {
|
||||
let mut t = TurnTracker::new();
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn forward_wrap_counts_up() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
let after = t.update(100); // 8000 → 8191 → 0 → 100
|
||||
assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reverse_wrap_counts_down() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(100);
|
||||
let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向）
|
||||
assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn small_deltas_do_not_wrap() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(0);
|
||||
assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4);
|
||||
assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn multi_turn_forward_then_reverse_returns() {
|
||||
let mut t = TurnTracker::new();
|
||||
// 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。
|
||||
for _ in 0..3 {
|
||||
t.update(0);
|
||||
t.update(4000);
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
}
|
||||
let fwd = t.last_angle_rev();
|
||||
let expected_fwd = 3.0 + 100.0 / 8192.0;
|
||||
assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}");
|
||||
|
||||
// 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。
|
||||
for _ in 0..3 {
|
||||
t.update(8000);
|
||||
t.update(4000);
|
||||
t.update(0);
|
||||
}
|
||||
let back = t.last_angle_rev();
|
||||
assert!(back.abs() < 1e-4, "should return to 0, got {back}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn jitter_at_boundary_stays_put() {
|
||||
// 0 附近抖动不产生虚假圈数。
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8191);
|
||||
assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次
|
||||
assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4);
|
||||
assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回
|
||||
assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_forgets_history() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
t.reset();
|
||||
assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits());
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
[package]
|
||||
name = "esc-bus"
|
||||
description = "Asynchronous ESC frame pump, state, leases, and safety"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
sim-harness = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
motor-model = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
approx = { workspace = true }
|
||||
@@ -1,3 +1,5 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。
|
||||
//!
|
||||
//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务：
|
||||
@@ -12,21 +14,21 @@
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::Duration;
|
||||
|
||||
use motor_core::{
|
||||
Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController,
|
||||
PositionDrive, VelocityController, VelocityDrive, VelocityGains,
|
||||
use can::{CanFrame, CanLink};
|
||||
use motor_protocol::common::{
|
||||
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
|
||||
};
|
||||
use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker};
|
||||
use thiserror::Error;
|
||||
use tokio::sync::{mpsc, watch};
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{Instant, Interval, MissedTickBehavior};
|
||||
use tracing::{debug, info, warn};
|
||||
|
||||
use crate::codec::{
|
||||
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
|
||||
use motor::{
|
||||
Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput,
|
||||
PolicySample, PositionDrive, VelocityDrive,
|
||||
};
|
||||
use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
|
||||
use crate::turn::TurnTracker;
|
||||
|
||||
/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌，
|
||||
/// 状态本身仍一致，驱动机不应因此停摆）。
|
||||
@@ -45,7 +47,7 @@ pub struct EscConfig {
|
||||
pub model: EscModel,
|
||||
/// 减速比（电机转子 : 输出轴）。
|
||||
///
|
||||
/// 驱动层**不使用**此字段；由上层 [`motor_core::OutputShaft`] 应用。
|
||||
/// 驱动层**不使用**此字段；由上层 `crate::OutputShaft` 应用。
|
||||
/// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。
|
||||
pub gear_ratio: f32,
|
||||
/// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
|
||||
@@ -53,14 +55,6 @@ pub struct EscConfig {
|
||||
pub direction: f32,
|
||||
/// effort 限幅（raw，`1..=型号满量程`）。
|
||||
pub current_limit: i16,
|
||||
/// 速度环比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 速度环积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 速度环微分增益。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
impl EscConfig {
|
||||
@@ -75,10 +69,6 @@ impl EscConfig {
|
||||
gear_ratio: 3591.0 / 187.0,
|
||||
direction,
|
||||
current_limit: 12_000,
|
||||
kp: 10.0,
|
||||
ki: 0.5,
|
||||
kd: 0.0,
|
||||
feedforward: 1.5,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -95,19 +85,9 @@ impl EscConfig {
|
||||
gear_ratio: 36.0,
|
||||
direction,
|
||||
current_limit: 8_000,
|
||||
kp: 8.0,
|
||||
ki: 0.4,
|
||||
kd: 0.0,
|
||||
feedforward: 1.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self) -> VelocityGains {
|
||||
VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward)
|
||||
}
|
||||
|
||||
fn validate(&self) -> Result<(), String> {
|
||||
if command_frame_id(self.esc_id).is_none() {
|
||||
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
|
||||
@@ -187,9 +167,6 @@ struct MotorShared {
|
||||
cmd: Mutex<Command>,
|
||||
/// 命令写入时刻（看门狗用）。
|
||||
cmd_at: Mutex<Option<Instant>>,
|
||||
last_mode: Mutex<CommandMode>,
|
||||
vel_ctrl: Mutex<VelocityController>,
|
||||
pos_ctrl: Mutex<PositionController>,
|
||||
turn: Mutex<TurnTracker>,
|
||||
last_feedback: Mutex<Option<Instant>>,
|
||||
offline_warned: std::sync::atomic::AtomicBool,
|
||||
@@ -197,17 +174,11 @@ struct MotorShared {
|
||||
|
||||
impl MotorShared {
|
||||
fn new(cfg: EscConfig) -> Self {
|
||||
let limit = cfg.current_limit;
|
||||
let pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
|
||||
let pos_pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
|
||||
Self {
|
||||
vel_ctrl: Mutex::new(VelocityController::new(pid, cfg.feedforward)),
|
||||
pos_ctrl: Mutex::new(PositionController::direct(pos_pid)),
|
||||
cfg,
|
||||
state: Mutex::new(MotorState::default()),
|
||||
cmd: Mutex::new(Command::Disabled),
|
||||
cmd_at: Mutex::new(None),
|
||||
last_mode: Mutex::new(CommandMode::Disabled),
|
||||
turn: Mutex::new(TurnTracker::new()),
|
||||
last_feedback: Mutex::new(None),
|
||||
offline_warned: std::sync::atomic::AtomicBool::new(false),
|
||||
@@ -219,21 +190,6 @@ impl MotorShared {
|
||||
*lock(&self.cmd_at) = Some(Instant::now());
|
||||
}
|
||||
|
||||
/// 当前速度环增益（来自控制器，而非启动时的 `cfg`）。
|
||||
fn velocity_gains(&self) -> VelocityGains {
|
||||
lock(&self.vel_ctrl).gains()
|
||||
}
|
||||
|
||||
/// 热更新速度环增益：重建控制器（等价于 reset，避免积分冲击）。
|
||||
///
|
||||
/// 只替换增益，不动限幅 / 型号 / 方向 —— 那些是安全与机械约束，
|
||||
/// 不属于可在线试错的整定量。
|
||||
fn set_velocity_gains(&self, gains: VelocityGains) {
|
||||
let limit = self.cfg.current_limit;
|
||||
let mut controller = lock(&self.vel_ctrl);
|
||||
*controller = VelocityController::from_gains(limit, gains);
|
||||
}
|
||||
|
||||
/// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
|
||||
fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
|
||||
let esc_rev = lock(&self.turn).update(fb.angle_raw);
|
||||
@@ -251,68 +207,26 @@ impl MotorShared {
|
||||
self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
|
||||
}
|
||||
|
||||
/// 复位主机侧控制器（离线或模式切换时）。
|
||||
fn reset_controllers(&self) {
|
||||
lock(&self.vel_ctrl).reset();
|
||||
lock(&self.pos_ctrl).reset();
|
||||
}
|
||||
|
||||
/// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。
|
||||
fn compute_effort(
|
||||
/// Produce one policy sample while enforcing feedback and command leases.
|
||||
fn sample(
|
||||
&self,
|
||||
dt: f32,
|
||||
feedback_timeout: Duration,
|
||||
command_timeout: Duration,
|
||||
now: Instant,
|
||||
) -> i16 {
|
||||
let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout);
|
||||
if !online {
|
||||
if !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
|
||||
warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current");
|
||||
}
|
||||
// 复位控制器，避免重新上线时旧积分冲击。
|
||||
self.reset_controllers();
|
||||
*lock(&self.last_mode) = CommandMode::Disabled;
|
||||
return 0;
|
||||
) -> PolicySample {
|
||||
let online = matches!(
|
||||
*lock(&self.last_feedback),
|
||||
Some(t) if now.saturating_duration_since(t) <= feedback_timeout
|
||||
);
|
||||
if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
|
||||
warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current");
|
||||
}
|
||||
// 命令看门狗：上层任务死亡/卡死后旧指令不得持续输出。
|
||||
let cmd_fresh =
|
||||
matches!(*lock(&self.cmd_at), Some(t) if now.duration_since(t) <= command_timeout);
|
||||
if !cmd_fresh {
|
||||
self.reset_controllers();
|
||||
*lock(&self.last_mode) = CommandMode::Disabled;
|
||||
return 0;
|
||||
}
|
||||
let cmd = *lock(&self.cmd);
|
||||
// 模式切换复位：避免积分/微分记忆冲击。
|
||||
{
|
||||
let mut last_mode = lock(&self.last_mode);
|
||||
if *last_mode != cmd.mode() {
|
||||
debug!(motor = %self.cfg.name, from = ?*last_mode, to = ?cmd.mode(), "command mode switch, resetting controllers");
|
||||
self.reset_controllers();
|
||||
*last_mode = cmd.mode();
|
||||
}
|
||||
}
|
||||
let dir = self.cfg.direction;
|
||||
let raw = match cmd {
|
||||
Command::Disabled => 0,
|
||||
Command::EffortRaw(raw) => (f32::from(raw) * dir) as i16,
|
||||
Command::Velocity(target) => {
|
||||
let measured = lock(&self.state).rotor_speed_rpm;
|
||||
let out = lock(&self.vel_ctrl).update(target, measured, dt);
|
||||
(out * dir) as i16
|
||||
}
|
||||
Command::Position(target) => {
|
||||
let (measured_rev, measured_rpm) = {
|
||||
let st = lock(&self.state);
|
||||
(st.rotor_revolutions, st.rotor_speed_rpm)
|
||||
};
|
||||
let out = lock(&self.pos_ctrl).update(target, measured_rev, measured_rpm, dt);
|
||||
(out * dir) as i16
|
||||
}
|
||||
};
|
||||
let limit = self.cfg.current_limit;
|
||||
clamp_current(self.cfg.model, raw.clamp(-limit, limit))
|
||||
let fresh = matches!(
|
||||
*lock(&self.cmd_at),
|
||||
Some(t) if now.saturating_duration_since(t) <= command_timeout
|
||||
);
|
||||
let command = if online && fresh { *lock(&self.cmd) } else { Command::Disabled };
|
||||
PolicySample { id: self.cfg.esc_id, state: *lock(&self.state), command, online }
|
||||
}
|
||||
}
|
||||
|
||||
@@ -353,25 +267,6 @@ impl EscMotor {
|
||||
pub fn last_feedback(&self) -> Option<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 {
|
||||
@@ -426,11 +321,12 @@ impl EscBus {
|
||||
/// # Errors
|
||||
///
|
||||
/// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
|
||||
pub fn spawn(
|
||||
pub fn spawn<P: EffortPolicy>(
|
||||
link: CanLink,
|
||||
configs: Vec<EscConfig>,
|
||||
options: BusOptions,
|
||||
) -> Result<EscBusHandle, EscBusError> {
|
||||
policy: P,
|
||||
) -> Result<EscBusHandle<P>, EscBusError> {
|
||||
if configs.is_empty() {
|
||||
return Err(EscBusError::NoMotors);
|
||||
}
|
||||
@@ -448,23 +344,30 @@ impl EscBus {
|
||||
let (shutdown_tx, shutdown_rx) = watch::channel(false);
|
||||
let CanLink { outgoing_tx, incoming_rx } = link;
|
||||
let motors = Arc::new(motors);
|
||||
let policy = Arc::new(Mutex::new(policy));
|
||||
let rx_handle =
|
||||
tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
|
||||
let tx_handle =
|
||||
tokio::spawn(tx_loop(outgoing_tx, Arc::clone(&motors), options, shutdown_rx));
|
||||
let tx_handle = tokio::spawn(tx_loop(
|
||||
outgoing_tx,
|
||||
Arc::clone(&motors),
|
||||
Arc::clone(&policy),
|
||||
options,
|
||||
shutdown_rx,
|
||||
));
|
||||
|
||||
Ok(EscBusHandle { motors, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
|
||||
Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
|
||||
}
|
||||
}
|
||||
|
||||
/// `EscBus` 的控制句柄。
|
||||
pub struct EscBusHandle {
|
||||
pub struct EscBusHandle<P> {
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
policy: Arc<Mutex<P>>,
|
||||
shutdown_tx: watch::Sender<bool>,
|
||||
tasks: Vec<JoinHandle<()>>,
|
||||
}
|
||||
|
||||
impl EscBusHandle {
|
||||
impl<P: EffortPolicy> EscBusHandle<P> {
|
||||
/// 按电调 ID 取电机句柄。
|
||||
#[must_use]
|
||||
pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> {
|
||||
@@ -477,6 +380,11 @@ impl EscBusHandle {
|
||||
&self.motors
|
||||
}
|
||||
|
||||
/// Mutate the composed control policy between bus ticks.
|
||||
pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R {
|
||||
update(&mut lock(&self.policy))
|
||||
}
|
||||
|
||||
/// 停止收发任务并等待退出。
|
||||
pub async fn shutdown(self) {
|
||||
let _ = self.shutdown_tx.send(true);
|
||||
@@ -512,15 +420,15 @@ async fn rx_loop(
|
||||
}
|
||||
}
|
||||
|
||||
async fn tx_loop(
|
||||
async fn tx_loop<P: EffortPolicy>(
|
||||
outgoing: mpsc::Sender<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
policy: Arc<Mutex<P>>,
|
||||
options: BusOptions,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
let nominal_dt = options.period.as_secs_f32();
|
||||
let mut ticker = tick_every(options.period);
|
||||
// 实测 dt：丢拍/负载抖动时 PID 不得按名义 1ms 硬算（F-1）。
|
||||
let mut last_tick = Instant::now();
|
||||
loop {
|
||||
tokio::select! {
|
||||
@@ -530,8 +438,21 @@ async fn tx_loop(
|
||||
let now = Instant::now();
|
||||
let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32();
|
||||
last_tick = now;
|
||||
// 夹在 [0.5, 4] × 名义周期，避免首拍/长卡死把积分或微分打飞。
|
||||
let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
|
||||
let mut samples: [PolicySample; 8] = std::array::from_fn(|index| PolicySample {
|
||||
id: u8::try_from(index + 1).unwrap_or(0),
|
||||
state: MotorState::default(),
|
||||
command: Command::Disabled,
|
||||
online: false,
|
||||
});
|
||||
for motor in motors.iter() {
|
||||
let sample =
|
||||
motor.shared.sample(options.feedback_timeout, options.command_timeout, now);
|
||||
if let Some(slot) = samples.get_mut(usize::from(sample.id.saturating_sub(1))) {
|
||||
*slot = sample;
|
||||
}
|
||||
}
|
||||
let efforts = lock(&policy).step(&PolicyInput { samples, dt });
|
||||
for (frame_id, group_start) in
|
||||
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
|
||||
{
|
||||
@@ -545,11 +466,12 @@ async fn tx_loop(
|
||||
let mut currents = [0i16; 4];
|
||||
for motor in &group {
|
||||
let slot = usize::from(motor.id() - group_start);
|
||||
currents[slot] = motor.shared.compute_effort(
|
||||
dt,
|
||||
options.feedback_timeout,
|
||||
options.command_timeout,
|
||||
now,
|
||||
let policy_index = usize::from(motor.id().saturating_sub(1));
|
||||
let physical = efforts.get(policy_index).copied().unwrap_or(0);
|
||||
let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16;
|
||||
currents[slot] = clamp_current(
|
||||
motor.shared.cfg.model,
|
||||
directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit),
|
||||
);
|
||||
}
|
||||
if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
|
||||
@@ -6,24 +6,35 @@
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{
|
||||
BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback,
|
||||
encode_feedback_frame,
|
||||
};
|
||||
use dji_esc_sim::spawn_plant;
|
||||
use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive, VelocityGains};
|
||||
use can::{CanFrame, CanLink};
|
||||
use esc_bus::{BusOptions, EscBus, EscBusError, EscConfig};
|
||||
use motor::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use motor_protocol::{EscFeedback, encode_feedback_frame};
|
||||
use sim_harness::spawn_plant;
|
||||
use tokio::sync::mpsc;
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval, sleep, timeout};
|
||||
|
||||
/// 与 `PlantParams::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
|
||||
fn tuned(esc_id: u8, direction: f32) -> EscConfig {
|
||||
let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction);
|
||||
cfg.kp = 8.0;
|
||||
cfg.ki = 2.0;
|
||||
cfg.kd = 0.0;
|
||||
cfg.feedforward = 16384.0 / 9254.0;
|
||||
cfg
|
||||
EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction)
|
||||
}
|
||||
|
||||
fn tuned_gains() -> VelocityGains {
|
||||
VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0)
|
||||
}
|
||||
|
||||
fn spawn_bus(
|
||||
link: CanLink,
|
||||
configs: Vec<EscConfig>,
|
||||
options: BusOptions,
|
||||
) -> Result<esc_bus::EscBusHandle<MotorPolicy>, EscBusError> {
|
||||
let mut policy = MotorPolicy::new();
|
||||
for config in &configs {
|
||||
policy.configure(config.esc_id, config.current_limit, tuned_gains());
|
||||
}
|
||||
EscBus::spawn(link, configs, options, policy)
|
||||
}
|
||||
|
||||
/// 轮询直到条件满足或超时（毫秒，仿真时间）。
|
||||
@@ -93,7 +104,7 @@ fn slot_current(frame: &CanFrame, slot: usize) -> i16 {
|
||||
async fn velocity_loop_converges() {
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
motor.set_velocity(3000.0);
|
||||
@@ -121,7 +132,7 @@ async fn reversed_motor_converges_in_physical_frame() {
|
||||
// direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1));
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(2).unwrap().clone();
|
||||
|
||||
motor.set_velocity(2000.0);
|
||||
@@ -143,7 +154,7 @@ async fn reversed_motor_converges_in_physical_frame() {
|
||||
async fn motor_without_feedback_commands_zero_current() {
|
||||
let (bus_link, mut peer) = CanLink::pair(64);
|
||||
// 不接 plant → 永远没有反馈。
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
|
||||
bus.motor(1).unwrap().set_velocity(5000.0);
|
||||
for _ in 0..5 {
|
||||
@@ -159,7 +170,7 @@ async fn current_limit_clamps_saturation() {
|
||||
let (bus_link, mut peer) = CanLink::pair(64);
|
||||
let mut cfg = tuned(1, 1.0);
|
||||
cfg.current_limit = 5000;
|
||||
let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![cfg], BusOptions::default()).unwrap();
|
||||
|
||||
send_feedback(&peer, &[1]).await;
|
||||
bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和
|
||||
@@ -203,7 +214,7 @@ fn spawn_feedback(yield_tx: mpsc::Sender<CanFrame>, esc_ids: Vec<u8>) -> JoinHan
|
||||
async fn command_lease_expires_and_renews() {
|
||||
let (bus_link, mut peer) = CanLink::pair(256);
|
||||
let feeder = spawn_feedback(peer.outgoing_tx.clone(), vec![1]);
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。
|
||||
@@ -248,7 +259,7 @@ async fn command_lease_expires_and_renews() {
|
||||
async fn current_commands_route_to_group_slots() {
|
||||
let (bus_link, mut peer) = CanLink::pair(256);
|
||||
let configs = (1..=5).map(|id| tuned(id, 1.0)).collect();
|
||||
let bus = EscBus::spawn(bus_link, configs, BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, configs, BusOptions::default()).unwrap();
|
||||
|
||||
bus.motor(3).unwrap().set_effort_raw(1234);
|
||||
bus.motor(5).unwrap().set_effort_raw(-567);
|
||||
@@ -306,7 +317,7 @@ fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool
|
||||
#[tokio::test]
|
||||
async fn feedback_updates_state_with_direction() {
|
||||
let (bus_link, peer) = CanLink::pair(64);
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
|
||||
// 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。
|
||||
let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 };
|
||||
@@ -337,7 +348,7 @@ async fn feedback_updates_state_with_direction() {
|
||||
async fn shutdown_stops_tasks_and_closes_link() {
|
||||
let (bus_link, mut peer) = CanLink::pair(128);
|
||||
// 不接 plant，直接验证任务生命周期。
|
||||
let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
bus.motor(1).unwrap().set_velocity(1000.0);
|
||||
sleep(Duration::from_millis(20)).await;
|
||||
// 有命令帧在流动。
|
||||
@@ -358,17 +369,14 @@ async fn shutdown_stops_tasks_and_closes_link() {
|
||||
#[test]
|
||||
fn spawn_rejects_bad_configs() {
|
||||
let link = || CanLink::pair(8).0;
|
||||
assert_eq!(spawn_bus(link(), vec![], BusOptions::default()).err(), Some(EscBusError::NoMotors));
|
||||
assert_eq!(
|
||||
EscBus::spawn(link(), vec![], BusOptions::default()).err(),
|
||||
Some(EscBusError::NoMotors)
|
||||
);
|
||||
assert_eq!(
|
||||
EscBus::spawn(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(),
|
||||
spawn_bus(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(),
|
||||
Some(EscBusError::DuplicateEscId(2))
|
||||
);
|
||||
let mut bad = tuned(1, 1.0);
|
||||
bad.direction = 0.0;
|
||||
let err = EscBus::spawn(link(), vec![bad], BusOptions::default()).err();
|
||||
let err = spawn_bus(link(), vec![bad], BusOptions::default()).err();
|
||||
assert!(
|
||||
matches!(err, Some(EscBusError::InvalidConfig { .. })),
|
||||
"expected InvalidConfig, got {err:?}"
|
||||
@@ -386,22 +394,22 @@ async fn velocity_gains_can_be_hot_swapped() {
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let cfg = tuned(1, 1.0);
|
||||
let limit = i32::from(cfg.current_limit);
|
||||
let bus = EscBus::spawn(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap();
|
||||
let bus = spawn_bus(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// ① 初始增益来自 EscConfig（含前馈）。
|
||||
assert_eq!(motor.velocity_gains(), cfg.velocity_gains());
|
||||
assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), tuned_gains());
|
||||
|
||||
// 让旧增益的积分先积累起来（大静差 + 深度饱和）。
|
||||
motor.set_velocity(9000.0);
|
||||
hold(&motor, 9000.0, 200).await;
|
||||
let stale = motor.velocity_gains();
|
||||
let stale = bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap());
|
||||
|
||||
// ② 热更新：控制器被重建。
|
||||
let swapped = VelocityGains::new(1.0, 0.0, 0.0, 0.0);
|
||||
motor.set_velocity_gains(swapped);
|
||||
assert_eq!(motor.velocity_gains(), swapped);
|
||||
assert_ne!(motor.velocity_gains(), stale);
|
||||
bus.with_policy(|policy| policy.set_velocity_gains(motor.id(), swapped));
|
||||
assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), swapped);
|
||||
assert_ne!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), stale);
|
||||
|
||||
// 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。
|
||||
motor.set_velocity(30_000.0);
|
||||
@@ -412,7 +420,11 @@ async fn velocity_gains_can_be_hot_swapped() {
|
||||
// ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。
|
||||
let effort = i32::from(motor.state().effort_raw);
|
||||
assert_eq!(effort, limit, "饱和值必须正好是 current_limit");
|
||||
assert_eq!(motor.velocity_gains(), swapped, "热更新后增益保持不变");
|
||||
assert_eq!(
|
||||
bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()),
|
||||
swapped,
|
||||
"热更新后增益保持不变"
|
||||
);
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
+12
-9
@@ -1,6 +1,6 @@
|
||||
//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。
|
||||
//!
|
||||
//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是
|
||||
//! 自动整定跑在 [`sim_harness::simulate`]（同步、无通道）上，而实车跑的是
|
||||
//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移，
|
||||
//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与
|
||||
//! 同一条场景下比较稳态与 effort。
|
||||
@@ -12,9 +12,13 @@
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{BusOptions, CanLink, EscBus, EscConfig};
|
||||
use dji_esc_sim::{LoopLimits, PlantParams, Scenario, simulate, spawn_plant};
|
||||
use motor_core::{MotorFeedback, VelocityDrive, VelocityGains};
|
||||
use can::CanLink;
|
||||
use esc_bus::{BusOptions, EscBus, EscConfig};
|
||||
use experiment::Scenario;
|
||||
use motor::{MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use motor_model::PlantParams;
|
||||
use sim_harness::{LoopLimits, simulate, spawn_plant};
|
||||
use tokio::time::sleep;
|
||||
|
||||
/// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。
|
||||
@@ -40,12 +44,11 @@ async fn async_steady_state() -> (f32, f32) {
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let mut cfg = EscConfig::gm3508("front_left", 1, 1.0);
|
||||
cfg.kp = GAINS.kp;
|
||||
cfg.ki = GAINS.ki;
|
||||
cfg.kd = GAINS.kd;
|
||||
cfg.feedforward = GAINS.feedforward;
|
||||
cfg.current_limit = LoopLimits::gm3508().current_limit_raw;
|
||||
let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).expect("bus must start");
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(cfg.esc_id, cfg.current_limit, GAINS);
|
||||
let bus =
|
||||
EscBus::spawn(bus_link, vec![cfg], BusOptions::default(), policy).expect("bus must start");
|
||||
let motor = bus.motor(1).expect("motor 1 exists").clone();
|
||||
|
||||
// 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。
|
||||
@@ -0,0 +1,16 @@
|
||||
[package]
|
||||
name = "experiment"
|
||||
description = "Shared experiment scenarios, traces, windows, and metrics"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,747 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod scenario {
|
||||
//! 场景定义：设定值曲线 + 采样节拍。
|
||||
//!
|
||||
//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、
|
||||
//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放，
|
||||
//! 因此整定结果可以跨后端比较。
|
||||
|
||||
/// 设定值曲线。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum SetpointProfile {
|
||||
/// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。
|
||||
Step,
|
||||
/// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。
|
||||
RampAndHold,
|
||||
/// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。
|
||||
Staircase {
|
||||
/// 阶梯数（≥ 1）。
|
||||
levels: u32,
|
||||
},
|
||||
/// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。
|
||||
UpDown,
|
||||
}
|
||||
|
||||
/// 一次试验的设定值与时间规格。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Scenario {
|
||||
/// 控制 / 采样节拍（秒）。
|
||||
pub dt_s: f32,
|
||||
/// 斜坡时间（秒）；`0` 表示瞬时。
|
||||
pub ramp_s: f32,
|
||||
/// 保持时间（秒）。
|
||||
pub hold_s: f32,
|
||||
/// 目标转子转速（rpm）。
|
||||
pub target_rpm: f32,
|
||||
/// 设定值曲线。
|
||||
pub profile: SetpointProfile,
|
||||
}
|
||||
|
||||
impl Scenario {
|
||||
/// 默认控制节拍（1 kHz，与 C620 命令周期一致）。
|
||||
pub const DEFAULT_DT_S: f32 = 0.001;
|
||||
|
||||
/// 阶跃 + 保持。
|
||||
#[must_use]
|
||||
pub const fn step(target_rpm: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s: 0.0,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::Step,
|
||||
}
|
||||
}
|
||||
|
||||
/// 斜坡 + 保持（台架默认整定场景）。
|
||||
#[must_use]
|
||||
pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self {
|
||||
Self {
|
||||
dt_s: Self::DEFAULT_DT_S,
|
||||
ramp_s,
|
||||
hold_s,
|
||||
target_rpm,
|
||||
profile: SetpointProfile::RampAndHold,
|
||||
}
|
||||
}
|
||||
|
||||
/// 换一个采样节拍。
|
||||
#[must_use]
|
||||
pub const fn with_dt_s(mut self, dt_s: f32) -> Self {
|
||||
self.dt_s = dt_s;
|
||||
self
|
||||
}
|
||||
|
||||
/// 换一条设定值曲线。
|
||||
#[must_use]
|
||||
pub const fn with_profile(mut self, profile: SetpointProfile) -> Self {
|
||||
self.profile = profile;
|
||||
self
|
||||
}
|
||||
|
||||
/// 场景总时长（秒）。
|
||||
#[must_use]
|
||||
pub fn total_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => self.hold_s,
|
||||
SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => {
|
||||
self.ramp_s + self.hold_s
|
||||
}
|
||||
SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s),
|
||||
}
|
||||
}
|
||||
|
||||
/// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。
|
||||
#[must_use]
|
||||
pub fn samples(&self) -> usize {
|
||||
let total = self.total_s();
|
||||
let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0;
|
||||
let total_ok = total.is_finite() && total > 0.0;
|
||||
if !dt_ok || !total_ok {
|
||||
return 1;
|
||||
}
|
||||
((total / self.dt_s).round() as usize).max(1)
|
||||
}
|
||||
|
||||
/// `t_s` 时刻的设定值（rpm）。
|
||||
#[must_use]
|
||||
pub fn setpoint_at(&self, t_s: f32) -> f32 {
|
||||
let target = self.target_rpm;
|
||||
let ramp = self.ramp_s;
|
||||
match self.profile {
|
||||
SetpointProfile::Step => target,
|
||||
SetpointProfile::RampAndHold => {
|
||||
if ramp > 0.0 && t_s < ramp {
|
||||
target * (t_s / ramp)
|
||||
} else {
|
||||
target
|
||||
}
|
||||
}
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let levels = levels.max(1);
|
||||
let total = ramp + self.hold_s;
|
||||
if total <= 0.0 {
|
||||
return target;
|
||||
}
|
||||
let rung_s = total / levels as f32;
|
||||
let index = (t_s / rung_s).floor().max(0.0) as u32;
|
||||
target * ((index.min(levels - 1) + 1) as f32 / levels as f32)
|
||||
}
|
||||
SetpointProfile::UpDown => {
|
||||
let up_end = ramp;
|
||||
let hold_end = up_end + self.hold_s;
|
||||
let down_end = hold_end + ramp;
|
||||
if t_s < up_end {
|
||||
if ramp > 0.0 { target * (t_s / ramp) } else { target }
|
||||
} else if t_s < hold_end {
|
||||
target
|
||||
} else if t_s < down_end {
|
||||
let progress = (t_s - hold_end) / ramp;
|
||||
target * (1.0 - progress)
|
||||
} else {
|
||||
0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。
|
||||
#[must_use]
|
||||
pub fn steady_start_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
SetpointProfile::Step => 0.0,
|
||||
SetpointProfile::RampAndHold => self.ramp_s,
|
||||
SetpointProfile::Staircase { levels } => {
|
||||
let level_s = self.level_s(levels);
|
||||
((levels.max(1) - 1) as f32) * level_s
|
||||
}
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
}
|
||||
}
|
||||
|
||||
/// 稳态评价窗口的终点（秒）。
|
||||
#[must_use]
|
||||
pub fn steady_end_s(&self) -> f32 {
|
||||
match self.profile {
|
||||
// 升降场景只评价上升段后的保持段。
|
||||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||||
_ => self.total_s(),
|
||||
}
|
||||
}
|
||||
|
||||
/// 阶梯的单级时长（秒）。
|
||||
fn level_s(&self, levels: u32) -> f32 {
|
||||
let levels = levels.max(1);
|
||||
let total = self.ramp_s + self.hold_s;
|
||||
if total > 0.0 { total / levels as f32 } else { 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
/// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
|
||||
#[track_caller]
|
||||
fn assert_close(actual: f32, expected: f32) {
|
||||
assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn step_is_immediate_and_lasts_hold() {
|
||||
let s = Scenario::step(1000.0, 0.5);
|
||||
assert!((s.total_s() - 0.5).abs() < 1e-6);
|
||||
assert_close(s.setpoint_at(0.0), 1000.0);
|
||||
assert_close(s.setpoint_at(0.4), 1000.0);
|
||||
assert_eq!(s.samples(), 500);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ramp_is_linear_then_holds() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0);
|
||||
assert_close(s.setpoint_at(0.0), 0.0);
|
||||
assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 1000.0);
|
||||
assert_close(s.setpoint_at(1.1), 1000.0);
|
||||
assert!((s.total_s() - 1.2).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn staircase_rises_in_equal_levels() {
|
||||
let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0)
|
||||
.with_profile(SetpointProfile::Staircase { levels: 4 });
|
||||
assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3);
|
||||
assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3);
|
||||
// 越界采样被夹在最后一级。
|
||||
assert_close(s.setpoint_at(5.0), 1000.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn up_down_returns_to_zero() {
|
||||
let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown);
|
||||
assert!((s.total_s() - 0.6).abs() < 1e-6);
|
||||
assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.2), 600.0);
|
||||
assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3);
|
||||
assert_close(s.setpoint_at(0.59), 0.0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_scenario_reports_one_sample() {
|
||||
let s = Scenario::step(1.0, 0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
let s = Scenario::step(1.0, 1.0).with_dt_s(0.0);
|
||||
assert_eq!(s.samples(), 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use scenario::*;
|
||||
|
||||
pub mod trace {
|
||||
//! 试验轨迹：一次场景重放的全部采样。
|
||||
|
||||
/// 一条完整试验轨迹（各列等长，索引即采样序号）。
|
||||
#[derive(Debug, Clone, Default, PartialEq)]
|
||||
pub struct Trace {
|
||||
/// 采样时刻（秒）。
|
||||
pub t_s: Vec<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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use trace::*;
|
||||
|
||||
pub mod metrics {
|
||||
//! 步响应指标：把一条 [`Trace`] 折成可比较、可解释的标量。
|
||||
//!
|
||||
//! 指标口径对**所有后端**（离线 harness、真机台架）一致——整定结果能不能
|
||||
//! 从仿真迁移到实车，取决于两条路径是否用同一把尺子量。
|
||||
|
||||
use crate::{Scenario, Trace};
|
||||
|
||||
/// 稳态判据的相对带宽（±2% of target）。
|
||||
pub const SETTLING_BAND: f32 = 0.02;
|
||||
/// 稳态窗口占轨迹总时长的比例（从 75% 处开始）。
|
||||
///
|
||||
/// 与场景自带的 `steady_start_s()` 取**更晚**者：阶跃场景没有显式斜坡，
|
||||
/// 若不设下限就会把整条上升段也算进"稳态"，任何真实响应都会被判成未收敛。
|
||||
pub const STEADY_FRACTION: f32 = 0.75;
|
||||
/// 指标最少需要的采样数（样本太少时一切统计都无意义）。
|
||||
pub const MIN_SAMPLES: usize = 8;
|
||||
|
||||
/// 一条步响应轨迹的指标集合。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct StepMetrics {
|
||||
/// 上升时间 10% → 90%（秒）；未到达时等于轨迹总时长。
|
||||
pub rise_time_s: f32,
|
||||
/// 超调量（%，相对目标；负值截为 0）。
|
||||
pub overshoot_pct: f32,
|
||||
/// 调节时间：最后一次离开 ±2% 带的时刻（秒）。
|
||||
pub settling_time_s: f32,
|
||||
/// 是否在稳态窗口内保持 ±2% 带（即真正收敛）。
|
||||
pub settled: bool,
|
||||
/// 稳态误差（%，相对目标）。
|
||||
pub ss_error_pct: f32,
|
||||
/// 绝对误差积分 ∫|e|dt（归一化前的原始值）。
|
||||
pub iae: f32,
|
||||
/// 误差平方积分 ∫e²dt。
|
||||
pub ise: f32,
|
||||
/// 时间加权绝对误差积分 ∫t|e|dt。
|
||||
pub itae: f32,
|
||||
/// 峰值 effort（raw，绝对值）。
|
||||
pub peak_effort_raw: f32,
|
||||
/// effort 均方根（raw）。
|
||||
pub rms_effort_raw: f32,
|
||||
/// effort 符号翻转次数（抖振）。
|
||||
pub effort_reversals: usize,
|
||||
/// 饱和采样占比（%）。
|
||||
pub saturated_pct: f32,
|
||||
/// 全过程最大转速（rpm，绝对值）。
|
||||
pub max_rpm: f32,
|
||||
/// 稳态窗口内的平均转速（rpm）。
|
||||
pub steady_rpm: f32,
|
||||
}
|
||||
|
||||
impl StepMetrics {
|
||||
/// 从轨迹计算指标；样本不足或目标转速为 0 时返回 `None`。
|
||||
#[must_use]
|
||||
pub fn from_trace(trace: &Trace, scenario: &Scenario) -> Option<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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use metrics::*;
|
||||
@@ -1,13 +1,13 @@
|
||||
[package]
|
||||
name = "can-link"
|
||||
description = "CAN 传输抽象：帧与链路通道（无协议知识）"
|
||||
name = "imu"
|
||||
description = "IMU data contract"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[dependencies]
|
||||
tokio = { workspace = true }
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! IMU 抽象：姿态数据与读取接口（ARCHITECTURE.md §5.1）。
|
||||
//!
|
||||
//! `driver crates::delta10` 为 DETA10 实现 [`Imu`]；测试可用
|
||||
//! `motor::testing` 风格的替身。
|
||||
//!
|
||||
//! # 单位与坐标系
|
||||
//!
|
||||
//! 与 ARCHITECTURE.md §5.1 的 [`ImuData`] 一致：
|
||||
//!
|
||||
//! | 字段 | 单位 | 坐标系 |
|
||||
//! | --- | --- | --- |
|
||||
//! | `accel` | m/s² | 机体系（x 前、y 左、z 上） |
|
||||
//! | `gyro` | rad/s | 机体系 |
|
||||
//! | `mag` | µT | 机体系 |
|
||||
//! | `roll` / `pitch` / `yaw` | 度 | 机体 → 世界（yaw 逆时针为正） |
|
||||
//!
|
||||
//! 时间戳为单调时钟纳秒（见 [`crate::time`]），用于和电机反馈对齐。
|
||||
|
||||
/// 一帧 IMU 数据。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct ImuData {
|
||||
/// 加速度（m/s²，机体系 x/y/z）。
|
||||
pub accel: [f32; 3],
|
||||
/// 角速度（rad/s，机体系 x/y/z）。
|
||||
pub gyro: [f32; 3],
|
||||
/// 磁力计（µT，机体系 x/y/z）；无磁力计时为 `[0.0; 3]`。
|
||||
pub mag: [f32; 3],
|
||||
/// 横滚角（度）。
|
||||
pub roll: f32,
|
||||
/// 俯仰角（度）。
|
||||
pub pitch: f32,
|
||||
/// 偏航角（度）。
|
||||
pub yaw: f32,
|
||||
/// 单调时钟时间戳（纳秒）。
|
||||
pub timestamp_ns: u64,
|
||||
}
|
||||
|
||||
impl ImuData {
|
||||
/// 所有字段是否有限（NaN / inf 不得进入控制律）。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.accel.iter().chain(&self.gyro).chain(&self.mag).all(|v| v.is_finite())
|
||||
&& self.roll.is_finite()
|
||||
&& self.pitch.is_finite()
|
||||
&& self.yaw.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// IMU 读取错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum ImuError {
|
||||
/// 链路关闭。
|
||||
Closed,
|
||||
/// 帧 ID / 长度不符（多半是总线接错或协议模式不对）。
|
||||
BadFrame,
|
||||
/// **帧载荷布局尚未确认**（ARCHITECTURE.md 决策点 D1）。
|
||||
///
|
||||
/// 见 `delta10-protocol`：DETA10 手册链接已失效，载荷偏移在
|
||||
/// 拿到数据手册前不得臆造，因此解码器会显式返回本错误。
|
||||
LayoutUnconfirmed,
|
||||
/// 校验和 / 校验失败。
|
||||
Checksum,
|
||||
/// 底层 IO 失败。
|
||||
Io,
|
||||
}
|
||||
|
||||
impl core::fmt::Display for ImuError {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
match self {
|
||||
Self::Closed => write!(f, "IMU 链路已关闭"),
|
||||
Self::BadFrame => write!(f, "IMU 帧 ID 或长度不符"),
|
||||
Self::LayoutUnconfirmed => {
|
||||
write!(f, "IMU 载荷布局未确认（ARCHITECTURE.md 决策点 D1：需 DETA10 数据手册）")
|
||||
}
|
||||
Self::Checksum => write!(f, "IMU 校验失败"),
|
||||
Self::Io => write!(f, "IMU 底层 IO 失败"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for ImuError {}
|
||||
|
||||
/// 硬件无关的 IMU 边界。
|
||||
pub trait Imu {
|
||||
/// 底层错误类型。
|
||||
type Error: core::error::Error + Send + Sync + 'static;
|
||||
|
||||
/// 读取一帧姿态数据（阻塞直到有帧或链路关闭）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 链路关闭、帧不合法、载荷布局未确认时返回错误。
|
||||
fn read(&mut self) -> impl core::future::Future<Output = Result<ImuData, Self::Error>>;
|
||||
|
||||
/// 非阻塞读取：本轮没有新帧时返回 `Ok(None)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 仅在链路确定损坏时返回错误。
|
||||
fn try_read(&mut self) -> Result<Option<ImuData>, Self::Error> {
|
||||
Ok(None)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn default_data_is_zero_and_finite() {
|
||||
let data = ImuData::default();
|
||||
assert!((data.yaw).abs() < 1e-6);
|
||||
assert!(data.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_finite_attitude_is_detected() {
|
||||
let data = ImuData { pitch: f32::NAN, ..ImuData::default() };
|
||||
assert!(!data.is_finite());
|
||||
let data = ImuData { gyro: [0.0, f32::INFINITY, 0.0], ..ImuData::default() };
|
||||
assert!(!data.is_finite());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn layout_unconfirmed_error_names_the_decision_point() {
|
||||
let text = ImuError::LayoutUnconfirmed.to_string();
|
||||
assert!(text.contains("D1"), "{text}");
|
||||
}
|
||||
}
|
||||
@@ -1,16 +1,17 @@
|
||||
[package]
|
||||
name = "mecanum"
|
||||
description = "Mecanum chassis kinematics"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "Mecanum wheel chassis forward/inverse kinematics"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
approx = { workspace = true }
|
||||
|
||||
@@ -1,7 +0,0 @@
|
||||
# Seeds for failure cases proptest has generated in the past. It is
|
||||
# automatically read and these particular cases re-run before any
|
||||
# novel cases are generated.
|
||||
#
|
||||
# It is recommended to check this file in to source control so that
|
||||
# everyone who runs the test benefits from these saved cases.
|
||||
cc 698ba63bf948a3702145ece17ed0f014133de0d0b3797fd8bdda514c5d2480a7 # shrinks to vx = 0.0, vy = 0.0, omega = -2.3901656
|
||||
@@ -1,3 +1,5 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 麦克纳姆轮底盘正 / 逆运动学。
|
||||
//!
|
||||
//! # 约定
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
[package]
|
||||
name = "motor-control"
|
||||
description = "Motor velocity, position, cascade, and feedforward control"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
pid = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,798 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod feedforward {
|
||||
//! 前馈补偿（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 速度环的前馈是**比例前馈**：稳态下所需控制量与目标转速近似成正比，
|
||||
//! 因此 `u_ff = k_ff · setpoint` 可以直接把积分项从"慢慢顶"变成"一开始就到位"。
|
||||
//!
|
||||
//! # 为什么前馈在空载台架上必须为 0
|
||||
//!
|
||||
//! `k_ff ≈ 1/K`，其中 `K` 是被控对象的稳态增益（GM3508 空载实测
|
||||
//! `K ≈ 0.57 rpm/raw` → `k_ff ≈ 1.77`）。空载时维持转速只需要克服摩擦的极小
|
||||
//! 电流，任何接近 1.77 的前馈都会把轮子直接推到最高转速（见 `docs/chassis.md` §7.2）。
|
||||
|
||||
/// 比例前馈：`输出 = k_ff · setpoint`。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct ProportionalFeedforward {
|
||||
/// 前馈系数（控制量 / 目标量）。
|
||||
pub k_ff: f32,
|
||||
}
|
||||
|
||||
impl ProportionalFeedforward {
|
||||
/// 构造。
|
||||
#[must_use]
|
||||
pub const fn new(k_ff: f32) -> Self {
|
||||
Self { k_ff }
|
||||
}
|
||||
|
||||
/// 由被控对象稳态增益换算：`k_ff = 1 / K`。
|
||||
///
|
||||
/// `K` 非有限或 ≤ 0 时返回 0（宁可不前馈，也不要一个发散的系数）。
|
||||
#[must_use]
|
||||
pub fn from_plant_gain(plant_gain: f32) -> Self {
|
||||
if plant_gain.is_finite() && plant_gain > 0.0 {
|
||||
Self { k_ff: 1.0 / plant_gain }
|
||||
} else {
|
||||
Self { k_ff: 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
/// 计算前馈量。
|
||||
#[must_use]
|
||||
pub fn compute(&self, setpoint: f32) -> f32 {
|
||||
if setpoint.is_finite() { self.k_ff * setpoint } else { 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn inverse_plant_gain_matches_the_documented_bench_feedforward() {
|
||||
// GM3508 空载 K ≈ 0.5648 rpm/raw → k_ff ≈ 1.77 raw/rpm
|
||||
let feedforward = ProportionalFeedforward::from_plant_gain(9254.0 / 16384.0);
|
||||
assert!((feedforward.k_ff - 1.77).abs() < 0.02, "{}", feedforward.k_ff);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn degenerate_plant_gain_yields_no_feedforward() {
|
||||
assert!((ProportionalFeedforward::from_plant_gain(0.0).k_ff).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::from_plant_gain(-1.0).k_ff).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::from_plant_gain(f32::NAN).k_ff).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn compute_is_linear_and_safe() {
|
||||
let feedforward = ProportionalFeedforward::new(2.0);
|
||||
assert!((feedforward.compute(3.0) - 6.0).abs() < 1e-6);
|
||||
assert!((feedforward.compute(f32::INFINITY)).abs() < 1e-6);
|
||||
assert!((ProportionalFeedforward::default().compute(100.0)).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use feedforward::*;
|
||||
|
||||
pub mod plant {
|
||||
//! 被控对象抽象：让**仿真与实机共用同一套 PID**（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 这是整定能否迁移到实机的关键 seam：整定器只认识 [`ControlPlant`]，
|
||||
//! 因此"在仿真里搜出来的增益"与"在台架上验出来的增益"评估的是同一条接口。
|
||||
//!
|
||||
//! 实现方：
|
||||
//! - 仿真：`sim-harness::SimPlant`（浮点积分，无 IO，可并行上千次）；
|
||||
//! - 实机/台架：`bench` 的台架采样（每次试验 = 真机跑一条场景）。
|
||||
|
||||
/// 单输入单输出的被控对象。
|
||||
///
|
||||
/// 约定：`step` 推进 `dt` 秒并返回**本次的测量输出**；输入是控制量
|
||||
/// （`motor-control` 里即 effort raw）。实现必须保证 `dt <= 0` 时不推进状态。
|
||||
pub trait ControlPlant {
|
||||
/// 推进一拍的误差类型（平台相关实现可以自定义）。
|
||||
type Error;
|
||||
|
||||
/// 推进一步，返回测量输出。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 数值发散或输入非法时返回错误，由调用方折算成硬约束罚值。
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error>;
|
||||
|
||||
/// 复位到初始状态（两次试验之间必须调用，否则结果依赖历史）。
|
||||
fn reset(&mut self);
|
||||
|
||||
/// 人类可读描述（写进报告，便于复现实验条件）。
|
||||
fn describe(&self) -> String;
|
||||
}
|
||||
|
||||
/// 无错误的被控对象（纯代数模型 / 已在内部处理发散的实现）用它的适配器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||||
pub struct Infallible;
|
||||
|
||||
impl core::fmt::Display for Infallible {
|
||||
fn fmt(&self, _f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl core::error::Error for Infallible {}
|
||||
|
||||
/// 一阶惯性环节：`y' = (k·u − y) / tau`，显式欧拉。
|
||||
///
|
||||
/// 这是最小可用的 [`ControlPlant`] 实现，用来给控制律 / 整定器做单元测试；
|
||||
/// 带摩擦与延迟的完整电机模型在 `sim-harness`。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct FirstOrderPlant {
|
||||
/// 稳态增益。
|
||||
pub gain: f32,
|
||||
/// 时间常数（秒）。
|
||||
pub tau_s: f32,
|
||||
/// 当前输出。
|
||||
pub output: f32,
|
||||
}
|
||||
|
||||
impl FirstOrderPlant {
|
||||
/// 构造（`tau_s` 必须为正，否则退化为直通）。
|
||||
#[must_use]
|
||||
pub fn new(gain: f32, tau_s: f32) -> Self {
|
||||
Self { gain, tau_s: if tau_s > 0.0 { tau_s } else { f32::EPSILON }, output: 0.0 }
|
||||
}
|
||||
}
|
||||
|
||||
impl ControlPlant for FirstOrderPlant {
|
||||
type Error = Infallible;
|
||||
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
|
||||
if dt > 0.0 && input.is_finite() {
|
||||
let alpha = (dt / self.tau_s).clamp(0.0, 1.0);
|
||||
let target = self.gain * input;
|
||||
self.output += alpha * (target - self.output);
|
||||
}
|
||||
Ok(self.output)
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.output = 0.0;
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
format!("first-order(gain={}, tau={}s)", self.gain, self.tau_s)
|
||||
}
|
||||
}
|
||||
|
||||
/// 标准二阶系统：`G(s) = gain·wn² / (s² + 2ζ·wn·s + wn²)`，显式欧拉。
|
||||
///
|
||||
/// 用作"整定算法是否真的收敛"的试金石——ARCHITECTURE.md §5.5 的验收标准就要求
|
||||
/// 继电整定在标准二阶系统上收敛。
|
||||
///
|
||||
/// # 相频特性（做继电实验时必须知道）
|
||||
///
|
||||
/// `∠G(jω) = −atan2(2ζ·wn·ω, wn² − ω²)`：在 `ω = wn` 处只有 **−90°**，
|
||||
/// 随 `ω → ∞` **渐近**趋于 −180°，**不存在有限的 −180° 穿越点**。
|
||||
/// 因此纯二阶对象在继电反馈下的极限环由**滞环宽度**决定（幅值 ≈ 滞环量级、
|
||||
/// 周期远小于 `2π/wn`），而不是由 `Tu = 2π/wn` 决定。
|
||||
/// 真实电机带延迟（`sim-harness` 的模型有 2 拍命令延迟 + 摩擦），相频会真正穿过
|
||||
/// −180°，那时的 `Tu` 才是有物理意义的临界周期。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct SecondOrderPlant {
|
||||
/// 自然频率（rad/s）。
|
||||
pub wn: f32,
|
||||
/// 阻尼比。
|
||||
pub zeta: f32,
|
||||
/// 直流增益。
|
||||
pub gain: f32,
|
||||
/// 位置状态。
|
||||
pub position: f32,
|
||||
/// 速度状态。
|
||||
pub velocity: f32,
|
||||
}
|
||||
|
||||
impl SecondOrderPlant {
|
||||
/// 构造（`wn > 0`；`zeta` 取绝对值）。
|
||||
#[must_use]
|
||||
pub fn new(wn: f32, zeta: f32, gain: f32) -> Self {
|
||||
Self {
|
||||
wn: if wn > 0.0 { wn } else { f32::EPSILON },
|
||||
zeta: zeta.abs(),
|
||||
gain,
|
||||
position: 0.0,
|
||||
velocity: 0.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ControlPlant for SecondOrderPlant {
|
||||
type Error = Infallible;
|
||||
|
||||
fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
|
||||
if dt > 0.0 && input.is_finite() {
|
||||
let accel = self.gain * self.wn * self.wn * input
|
||||
- 2.0 * self.zeta * self.wn * self.velocity
|
||||
- self.wn * self.wn * self.position;
|
||||
self.velocity += accel * dt;
|
||||
self.position += self.velocity * dt;
|
||||
}
|
||||
Ok(self.position)
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.position = 0.0;
|
||||
self.velocity = 0.0;
|
||||
}
|
||||
|
||||
fn describe(&self) -> String {
|
||||
format!("second-order(wn={} rad/s, zeta={}, gain={})", self.wn, self.zeta, self.gain)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn second_order_settles_at_dc_gain() {
|
||||
let mut plant = SecondOrderPlant::new(20.0, 0.7, 2.0);
|
||||
for _ in 0..20_000 {
|
||||
plant.step(1.0, 0.0001).unwrap();
|
||||
}
|
||||
assert!((plant.position - 2.0).abs() < 5e-2, "{}", plant.position);
|
||||
plant.reset();
|
||||
assert!((plant.position).abs() < 1e-6);
|
||||
assert!(plant.describe().contains("second-order"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn second_order_ignores_non_positive_dt() {
|
||||
let mut plant = SecondOrderPlant::new(20.0, 0.5, 1.0);
|
||||
assert!((plant.step(1.0, 0.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.velocity).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn first_order_converges_to_gain_times_input() {
|
||||
let mut plant = FirstOrderPlant::new(2.0, 0.1);
|
||||
for _ in 0..2000 {
|
||||
plant.step(1.0, 0.001).unwrap();
|
||||
}
|
||||
assert!((plant.output - 2.0).abs() < 1e-3, "{}", plant.output);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn non_positive_dt_does_not_advance() {
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.1);
|
||||
assert!((plant.step(5.0, 0.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.step(5.0, -1.0).unwrap()).abs() < 1e-6);
|
||||
assert!((plant.step(f32::NAN, 0.001).unwrap()).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_history() {
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.05);
|
||||
for _ in 0..100 {
|
||||
plant.step(1.0, 0.001).unwrap();
|
||||
}
|
||||
assert!(plant.output > 0.0);
|
||||
plant.reset();
|
||||
assert!((plant.output).abs() < 1e-6);
|
||||
assert!(plant.describe().contains("first-order"));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use plant::*;
|
||||
|
||||
pub mod position {
|
||||
//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。
|
||||
|
||||
use crate::velocity::VelocityController;
|
||||
use pid::Pid;
|
||||
|
||||
/// 位置环拓扑。
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum PositionLoop {
|
||||
/// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。
|
||||
Direct(Pid),
|
||||
/// 级联：位置 → 目标转速 → 速度环 → effort_raw。
|
||||
Cascade {
|
||||
/// 外环位置 PID（输出限幅 = 最大 rpm）。
|
||||
pos: Pid,
|
||||
/// 内环速度控制器（输出限幅 = 最大 effort）。
|
||||
vel: VelocityController,
|
||||
},
|
||||
}
|
||||
|
||||
/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PositionController {
|
||||
inner: PositionLoop,
|
||||
}
|
||||
|
||||
impl PositionController {
|
||||
/// 单环位置控制器。
|
||||
#[must_use]
|
||||
pub fn direct(pid: Pid) -> Self {
|
||||
Self { inner: PositionLoop::Direct(pid) }
|
||||
}
|
||||
|
||||
/// 级联位置控制器（限速）。
|
||||
#[must_use]
|
||||
pub fn cascade(pos: Pid, vel: VelocityController) -> Self {
|
||||
Self { inner: PositionLoop::Cascade { pos, vel } }
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含限幅）。
|
||||
///
|
||||
/// `measured_rpm` 仅级联内环使用；单环忽略该参数。
|
||||
pub fn update(
|
||||
&mut self,
|
||||
target_rev: f32,
|
||||
measured_rev: f32,
|
||||
measured_rpm: f32,
|
||||
dt: f32,
|
||||
) -> f32 {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
let vel_target = pos.update(target_rev, measured_rev, dt);
|
||||
vel.update(vel_target, measured_rpm, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 清空内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.reset(),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
pos.reset();
|
||||
vel.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const DT: f32 = 0.001;
|
||||
|
||||
#[test]
|
||||
fn direct_outputs_pid() {
|
||||
let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0));
|
||||
let out = pc.update(1.0, 0.5, 0.0, DT);
|
||||
assert!((out - 1.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cascade_limits_velocity_then_effort() {
|
||||
// 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。
|
||||
let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0);
|
||||
let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0);
|
||||
let mut pc = PositionController::cascade(pos, vel);
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 外环饱和到 5 rpm，内环 ff=2 → 10 raw。
|
||||
assert!((out - 10.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_state() {
|
||||
let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6));
|
||||
pc.update(1.0, 0.0, 0.0, DT);
|
||||
pc.reset();
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 重置后只剩本步积分：10 * 1 * 0.001。
|
||||
assert!((out - 0.01).abs() < 1e-5, "out = {out}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use position::*;
|
||||
|
||||
pub mod velocity {
|
||||
//! 速度环控制器：PID + 速度前馈。
|
||||
|
||||
use pid::Pid;
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
///
|
||||
/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
|
||||
/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
|
||||
/// 传递、落盘与热更新，而无需重建整个速度环。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct VelocityGains {
|
||||
/// 比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 微分增益（raw / (rpm/s)）。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityGains {
|
||||
/// 全零增益。
|
||||
pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
|
||||
|
||||
/// 由四个分量构造。
|
||||
#[must_use]
|
||||
pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
|
||||
Self { kp, ki, kd, feedforward }
|
||||
}
|
||||
|
||||
/// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.kp.is_finite()
|
||||
&& self.ki.is_finite()
|
||||
&& self.kd.is_finite()
|
||||
&& self.feedforward.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
|
||||
///
|
||||
/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
|
||||
/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。
|
||||
/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量
|
||||
/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。
|
||||
///
|
||||
/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct VelocityController {
|
||||
pid: Pid,
|
||||
feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityController {
|
||||
/// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。
|
||||
#[must_use]
|
||||
pub fn new(pid: Pid, feedforward: f32) -> Self {
|
||||
Self { pid, feedforward }
|
||||
}
|
||||
|
||||
/// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
|
||||
#[must_use]
|
||||
pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
|
||||
let limit = f32::from(output_limit_raw);
|
||||
let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
|
||||
Self { pid, feedforward: gains.feedforward }
|
||||
}
|
||||
|
||||
/// 当前增益组（含前馈），用于诊断与热更新前的读取。
|
||||
#[must_use]
|
||||
pub fn gains(&self) -> VelocityGains {
|
||||
VelocityGains {
|
||||
kp: self.pid.kp(),
|
||||
ki: self.pid.ki(),
|
||||
kd: self.pid.kd(),
|
||||
feedforward: self.feedforward,
|
||||
}
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
|
||||
pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
|
||||
let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
|
||||
(pid_out + self.feedforward * target_rpm).clamp(
|
||||
// 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。
|
||||
self.pid.output_min(),
|
||||
self.pid.output_max(),
|
||||
)
|
||||
}
|
||||
|
||||
/// 清空 PID 内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
self.pid.reset();
|
||||
}
|
||||
|
||||
/// 前馈系数（raw / rpm）。
|
||||
#[must_use]
|
||||
pub fn feedforward(&self) -> f32 {
|
||||
self.feedforward
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn pure_feedforward_tracks_target() {
|
||||
let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0);
|
||||
assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3);
|
||||
// 前馈 -18000 被限幅到 -16384。
|
||||
assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedforward_plus_pid_respects_limit() {
|
||||
let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0);
|
||||
// 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。
|
||||
let out = vc.update(1000.0, 0.0, 0.001);
|
||||
assert!((out - 12000.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_error_yields_pure_feedforward() {
|
||||
// 误差为 0 时 PID 贡献为 0，输出只剩前馈。
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0);
|
||||
let out = vc.update(1000.0, 1000.0, 0.001);
|
||||
assert!((out - 9000.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_pid_state() {
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0);
|
||||
for _ in 0..10 {
|
||||
vc.update(100.0, 0.0, 0.001);
|
||||
}
|
||||
vc.reset();
|
||||
// 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。
|
||||
let out = vc.update(100.0, 0.0, 0.001);
|
||||
assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_round_trips_through_gains() {
|
||||
let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
|
||||
let mut from_data = VelocityController::from_gains(12_000, gains);
|
||||
let mut by_hand =
|
||||
VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
|
||||
|
||||
assert_eq!(from_data.gains(), gains);
|
||||
// 两条构造路径必须逐位一致。
|
||||
for step in 0..16 {
|
||||
let target = 1000.0 + step as f32;
|
||||
let measured = 900.0 + step as f32 * 3.0;
|
||||
assert_eq!(
|
||||
from_data.update(target, measured, 0.001).to_bits(),
|
||||
by_hand.update(target, measured, 0.001).to_bits()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_clamps_to_output_limit() {
|
||||
let mut vc =
|
||||
VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
|
||||
assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gains_finiteness_is_checkable() {
|
||||
assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
|
||||
assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use velocity::*;
|
||||
|
||||
pub mod cascade {
|
||||
//! 串级控制：位置环（外）+ 速度环（内）（ARCHITECTURE.md §5.5）。
|
||||
//!
|
||||
//! 外环输出的是**速度目标**而不是控制量，因此内环必须在每个节拍都跑一次——
|
||||
//! 这也是云台（位置）与底盘（速度）共用的结构：底盘只用内环，云台两环都用。
|
||||
//!
|
||||
//! ```text
|
||||
//! 位置目标 ──►[ 外环 PID ]──► 速度目标 ──►[ 内环 PID ]──► effort
|
||||
//! ▲ ▲
|
||||
//! 位置反馈 速度反馈
|
||||
//! ```
|
||||
|
||||
use pid::Pid;
|
||||
|
||||
/// 位置-速度串级控制器。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct CascadeController {
|
||||
/// 外环：位置 → 速度目标。
|
||||
outer: Pid,
|
||||
/// 内环：速度 → 控制量。
|
||||
inner: Pid,
|
||||
}
|
||||
|
||||
impl CascadeController {
|
||||
/// 由两个已配置好的 PID 组装。
|
||||
#[must_use]
|
||||
pub const fn new(outer: Pid, inner: Pid) -> Self {
|
||||
Self { outer, inner }
|
||||
}
|
||||
|
||||
/// 单步更新，返回内环输出（控制量）。
|
||||
///
|
||||
/// 非有限输入会被内环防御性忽略（见 [`Pid::update`]），因此本函数不会 panic。
|
||||
pub fn compute(
|
||||
&mut self,
|
||||
position_setpoint: f32,
|
||||
position_feedback: f32,
|
||||
velocity_feedback: f32,
|
||||
dt: f32,
|
||||
) -> f32 {
|
||||
let velocity_setpoint = self.outer.update(position_setpoint, position_feedback, dt);
|
||||
self.inner.update(velocity_setpoint, velocity_feedback, dt)
|
||||
}
|
||||
|
||||
/// 外环当前建议的速度目标（诊断 / 限幅用）。
|
||||
#[must_use]
|
||||
pub fn last_velocity_setpoint(&self) -> f32 {
|
||||
self.outer.last_output()
|
||||
}
|
||||
|
||||
/// 清空两环的积分与微分记忆。
|
||||
pub fn reset(&mut self) {
|
||||
self.outer.reset();
|
||||
self.inner.reset();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::plant::{ControlPlant, FirstOrderPlant};
|
||||
|
||||
#[test]
|
||||
fn cascade_drives_a_first_order_plant_to_the_position_setpoint() {
|
||||
// 内环（速度）与对象同量纲简化：直接把外环输出当对象输入，
|
||||
// 用真实的一阶对象验证"两环串联能收敛"。
|
||||
let mut cascade = CascadeController::new(
|
||||
Pid::new(4.0, 8.0, 0.0, -100.0, 100.0),
|
||||
Pid::new(6.0, 20.0, 0.0, -100.0, 100.0),
|
||||
);
|
||||
let mut plant = FirstOrderPlant::new(1.0, 0.05);
|
||||
let dt = 0.001;
|
||||
let mut position = 0.0f32;
|
||||
for _ in 0..4000 {
|
||||
let effort = cascade.compute(1.0, position, 0.0, dt);
|
||||
let velocity = plant.step(effort, dt).unwrap();
|
||||
position += velocity * dt;
|
||||
}
|
||||
assert!((position - 1.0).abs() < 0.05, "position = {position}");
|
||||
assert!(plant.describe().contains("first-order"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_integral_windup() {
|
||||
let mut cascade = CascadeController::new(
|
||||
Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
|
||||
Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
|
||||
);
|
||||
for _ in 0..100 {
|
||||
cascade.compute(100.0, 0.0, 0.0, 0.001);
|
||||
}
|
||||
assert!(cascade.last_velocity_setpoint().abs() > 0.0);
|
||||
cascade.reset();
|
||||
assert!((cascade.last_velocity_setpoint()).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use cascade::*;
|
||||
|
||||
/// Per-motor controller state used by [`MotorPolicy`].
|
||||
struct PolicyLoop {
|
||||
current_limit: i16,
|
||||
gains: VelocityGains,
|
||||
velocity: VelocityController,
|
||||
position: PositionController,
|
||||
last_mode: motor::CommandMode,
|
||||
}
|
||||
|
||||
impl PolicyLoop {
|
||||
fn new(limit: i16, gains: VelocityGains) -> Self {
|
||||
let bound = f32::from(limit);
|
||||
Self {
|
||||
current_limit: limit,
|
||||
gains,
|
||||
velocity: VelocityController::from_gains(limit, gains),
|
||||
position: PositionController::direct(pid::Pid::new(
|
||||
gains.kp, gains.ki, gains.kd, -bound, bound,
|
||||
)),
|
||||
last_mode: motor::CommandMode::Disabled,
|
||||
}
|
||||
}
|
||||
|
||||
fn reset(&mut self) {
|
||||
self.velocity.reset();
|
||||
self.position.reset();
|
||||
self.last_mode = motor::CommandMode::Disabled;
|
||||
}
|
||||
|
||||
fn effort(&mut self, sample: motor::PolicySample, dt: f32) -> i16 {
|
||||
if !sample.online {
|
||||
self.reset();
|
||||
return 0;
|
||||
}
|
||||
if self.last_mode != sample.command.mode() {
|
||||
self.reset();
|
||||
self.last_mode = sample.command.mode();
|
||||
}
|
||||
match sample.command {
|
||||
motor::Command::Disabled => {
|
||||
self.reset();
|
||||
0
|
||||
}
|
||||
motor::Command::EffortRaw(raw) => raw,
|
||||
motor::Command::Velocity(target) => {
|
||||
self.velocity.update(target, sample.state.rotor_speed_rpm, dt) as i16
|
||||
}
|
||||
motor::Command::Position(target) => self.position.update(
|
||||
target,
|
||||
sample.state.rotor_revolutions,
|
||||
sample.state.rotor_speed_rpm,
|
||||
dt,
|
||||
) as i16,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Concrete velocity/position policy for up to eight ESCs.
|
||||
pub struct MotorPolicy {
|
||||
loops: [Option<PolicyLoop>; 8],
|
||||
}
|
||||
|
||||
impl MotorPolicy {
|
||||
/// Create an empty policy.
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self { loops: std::array::from_fn(|_| None) }
|
||||
}
|
||||
|
||||
/// Configure one ESC controller.
|
||||
pub fn configure(&mut self, esc_id: u8, current_limit: i16, gains: VelocityGains) {
|
||||
if let Some(slot) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
*slot = Some(PolicyLoop::new(current_limit, gains));
|
||||
}
|
||||
}
|
||||
|
||||
/// Read one controller's current gains.
|
||||
#[must_use]
|
||||
pub fn velocity_gains(&self, esc_id: u8) -> Option<VelocityGains> {
|
||||
self.loops
|
||||
.get(usize::from(esc_id.saturating_sub(1)))
|
||||
.and_then(Option::as_ref)
|
||||
.map(|loop_| loop_.gains)
|
||||
}
|
||||
|
||||
/// Replace one controller's gains without changing its output limit.
|
||||
pub fn set_velocity_gains(&mut self, esc_id: u8, gains: VelocityGains) {
|
||||
if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
let limit = loop_.current_limit;
|
||||
*loop_ = PolicyLoop::new(limit, gains);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for MotorPolicy {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl motor::EffortPolicy for MotorPolicy {
|
||||
fn step(&mut self, input: &motor::PolicyInput) -> [i16; 8] {
|
||||
std::array::from_fn(|index| {
|
||||
let sample = input.samples[index];
|
||||
match self.loops[index].as_mut() {
|
||||
Some(loop_) if sample.id == u8::try_from(index + 1).unwrap_or(0) => {
|
||||
loop_.effort(sample, input.dt)
|
||||
}
|
||||
_ => 0,
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
fn reset(&mut self, esc_id: u8) {
|
||||
if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
|
||||
loop_.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,41 +0,0 @@
|
||||
//! 驱动层命令寄存器（最后写入胜出）。
|
||||
|
||||
/// 发给驱动层的电机命令。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Command {
|
||||
/// 停用：目标清零，下发 0 输出。
|
||||
Disabled,
|
||||
/// 直接下发开环 raw（协议量程，物理方向），绕过闭环。
|
||||
/// ESC = 电流 raw；GM6020 = 电压 raw。
|
||||
EffortRaw(i16),
|
||||
/// 速度环跟踪目标转子转速（rpm，物理方向）。
|
||||
Velocity(f32),
|
||||
/// 位置环跟踪目标转子多圈位置（圈，物理方向）。
|
||||
Position(f32),
|
||||
}
|
||||
|
||||
/// 命令模式标签（不含设定值）——用于切换时复位控制器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum CommandMode {
|
||||
/// 停用。
|
||||
Disabled,
|
||||
/// 开环 raw。
|
||||
EffortRaw,
|
||||
/// 速度环。
|
||||
Velocity,
|
||||
/// 位置环。
|
||||
Position,
|
||||
}
|
||||
|
||||
impl Command {
|
||||
/// 模式标签。
|
||||
#[must_use]
|
||||
pub const fn mode(self) -> CommandMode {
|
||||
match self {
|
||||
Self::Disabled => CommandMode::Disabled,
|
||||
Self::EffortRaw(_) => CommandMode::EffortRaw,
|
||||
Self::Velocity(_) => CommandMode::Velocity,
|
||||
Self::Position(_) => CommandMode::Position,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,182 +0,0 @@
|
||||
//! 输出轴单位适配器：减速比换算。
|
||||
//!
|
||||
//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
|
||||
//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
|
||||
|
||||
/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
|
||||
pub struct OutputShaft<T> {
|
||||
inner: T,
|
||||
gear_ratio: f32,
|
||||
}
|
||||
|
||||
impl<T> OutputShaft<T> {
|
||||
/// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `gear_ratio` 非有限或 ≤ 0 时 panic。
|
||||
#[must_use]
|
||||
pub fn new(inner: T, gear_ratio: f32) -> Self {
|
||||
assert!(
|
||||
gear_ratio.is_finite() && gear_ratio > 0.0,
|
||||
"gear_ratio must be > 0, got {gear_ratio}"
|
||||
);
|
||||
Self { inner, gear_ratio }
|
||||
}
|
||||
|
||||
/// 减速比。
|
||||
#[must_use]
|
||||
pub fn gear_ratio(&self) -> f32 {
|
||||
self.gear_ratio
|
||||
}
|
||||
|
||||
/// 底层电机引用。
|
||||
#[must_use]
|
||||
pub fn inner(&self) -> &T {
|
||||
&self.inner
|
||||
}
|
||||
|
||||
/// 转子量 → 输出轴量。
|
||||
#[must_use]
|
||||
pub fn to_output(&self, rotor: f32) -> f32 {
|
||||
rotor / self.gear_ratio
|
||||
}
|
||||
|
||||
/// 输出轴量 → 转子量。
|
||||
#[must_use]
|
||||
pub fn to_rotor(&self, output: f32) -> f32 {
|
||||
output * self.gear_ratio
|
||||
}
|
||||
|
||||
/// 由反馈快照推算输出轴转速（rpm）。
|
||||
#[must_use]
|
||||
pub fn output_speed_rpm(&self, state: &MotorState) -> f32 {
|
||||
state.rotor_speed_rpm / self.gear_ratio
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
|
||||
fn id(&self) -> u8 {
|
||||
self.inner.id()
|
||||
}
|
||||
|
||||
fn state(&self) -> MotorState {
|
||||
// 只透传转子量；不伪造输出轴字段。
|
||||
self.inner.state()
|
||||
}
|
||||
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
self.inner.feedback_age()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
/// `output_rpm` 为**输出轴**转速。
|
||||
fn set_velocity(&self, output_rpm: f32) {
|
||||
self.inner.set_velocity(self.to_rotor(output_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: EffortDrive> EffortDrive for OutputShaft<T> {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
self.inner.set_effort_raw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: PositionDrive> PositionDrive for OutputShaft<T> {
|
||||
/// `output_rev` 为**输出轴**圈数。
|
||||
fn set_position(&self, output_rev: f32) {
|
||||
self.inner.set_position(self.to_rotor(output_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: Disable> Disable for OutputShaft<T> {
|
||||
fn disable(&self) {
|
||||
self.inner.disable();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::Command;
|
||||
|
||||
struct Stub {
|
||||
cmd: std::sync::Mutex<Command>,
|
||||
}
|
||||
|
||||
impl Stub {
|
||||
fn new() -> Self {
|
||||
Self { cmd: std::sync::Mutex::new(Command::Disabled) }
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for Stub {
|
||||
fn id(&self) -> u8 {
|
||||
1
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
MotorState::default()
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
Some(Duration::ZERO)
|
||||
}
|
||||
}
|
||||
impl VelocityDrive for Stub {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
impl EffortDrive for Stub {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
*self.cmd.lock().unwrap() = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
impl PositionDrive for Stub {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
impl Disable for Stub {
|
||||
fn disable(&self) {
|
||||
*self.cmd.lock().unwrap() = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn velocity_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 19.203);
|
||||
shaft.set_velocity(100.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn position_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 36.0);
|
||||
shaft.set_position(2.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn effort_passes_through() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 10.0);
|
||||
shaft.set_effort_raw(-1234);
|
||||
assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234));
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic = "gear_ratio"]
|
||||
fn rejects_nonpositive_gear() {
|
||||
let _ = OutputShaft::new(Stub::new(), 0.0);
|
||||
}
|
||||
}
|
||||
@@ -1,21 +0,0 @@
|
||||
//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。
|
||||
//!
|
||||
//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。
|
||||
//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。
|
||||
|
||||
use std::future::Future;
|
||||
|
||||
/// 绝对航向观测源。
|
||||
pub trait HeadingSource: Send {
|
||||
/// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send;
|
||||
}
|
||||
|
||||
/// 云台目标角的参考系。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum HeadingFrame {
|
||||
/// 电机转子多圈（上电相对）。
|
||||
RotorMultiTurn,
|
||||
/// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。
|
||||
AbsoluteField,
|
||||
}
|
||||
@@ -1,93 +0,0 @@
|
||||
//! 协议无关的电机抽象层。
|
||||
//!
|
||||
//! 本 crate 不做任何 I/O：它定义电机能力 trait（[`MotorFeedback`] /
|
||||
//! [`EffortDrive`] / [`VelocityDrive`] / [`PositionDrive`] / [`Disable`]）、
|
||||
//! 公共数据类型（[`MotorState`]、[`Command`]）以及纯控制算法（[`Pid`]、
|
||||
//! [`VelocityController`]、[`PositionController`]）。具体总线协议在其上实现。
|
||||
//!
|
||||
//! # 单一职责
|
||||
//!
|
||||
//! - [`traits`]：能力面（小而正交）；
|
||||
//! - [`state`] / [`command`]：反馈快照与命令寄存器；
|
||||
//! - [`pid`] / [`velocity`] / [`position`]：纯控制律；
|
||||
//! - [`gearbox`]：减速比单位适配（输出轴 ↔ 转子）；
|
||||
//! - [`heading`]：航向观测 seam（云台绝对角融合用）；
|
||||
//! - [`testing`]：测试替身。
|
||||
//!
|
||||
//! # 单位与方向约定
|
||||
//!
|
||||
//! - 所有呈现给上层的量都是**安装方向修正后**的物理量：`direction = -1`
|
||||
//! 的电机由驱动层在读写两个方向上统一翻转符号。
|
||||
//! - 转速一律为电机**转子**转速（rpm）；输出轴换算见 [`OutputShaft`]。
|
||||
//! - 开环控制量以协议原始值（raw）为主（C620 电流：±16384 ↔ ±20 A；
|
||||
//! GM6020 电压：±30000 ≈ 满电压）。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod command;
|
||||
pub mod gearbox;
|
||||
pub mod heading;
|
||||
pub mod pid;
|
||||
pub mod position;
|
||||
pub mod state;
|
||||
pub mod testing;
|
||||
pub mod traits;
|
||||
pub mod velocity;
|
||||
|
||||
pub use command::{Command, CommandMode};
|
||||
pub use gearbox::OutputShaft;
|
||||
pub use heading::{HeadingFrame, HeadingSource};
|
||||
pub use pid::Pid;
|
||||
pub use position::{PositionController, PositionLoop};
|
||||
pub use state::MotorState;
|
||||
pub use testing::{MockHeading, MockMotor};
|
||||
pub use traits::{Disable, EffortDrive, Motor, MotorFeedback, PositionDrive, VelocityDrive};
|
||||
pub use velocity::{VelocityController, VelocityGains};
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn default_state_has_no_feedback() {
|
||||
let state = MotorState::default();
|
||||
assert!(!state.has_feedback());
|
||||
assert_eq!(state.feedback_count, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_gate_flips_on_first_frame() {
|
||||
let state = MotorState { feedback_count: 1, ..MotorState::default() };
|
||||
assert!(state.has_feedback());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn command_variants_roundtrip_by_value() {
|
||||
let cmds = [
|
||||
Command::Disabled,
|
||||
Command::EffortRaw(-16384),
|
||||
Command::Velocity(4820.5),
|
||||
Command::Position(2.5),
|
||||
];
|
||||
assert_eq!(cmds[0], Command::Disabled);
|
||||
assert_eq!(cmds[1], Command::EffortRaw(-16384));
|
||||
assert_eq!(cmds[2], Command::Velocity(4820.5));
|
||||
assert_eq!(cmds[3], Command::Position(2.5));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn command_mode_labels() {
|
||||
assert_eq!(Command::Disabled.mode(), CommandMode::Disabled);
|
||||
assert_eq!(Command::EffortRaw(0).mode(), CommandMode::EffortRaw);
|
||||
assert_eq!(Command::Velocity(0.0).mode(), CommandMode::Velocity);
|
||||
assert_eq!(Command::Position(0.0).mode(), CommandMode::Position);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_is_full_motor() {
|
||||
fn takes_motor(m: &impl Motor) {
|
||||
m.set_velocity(1.0);
|
||||
m.disable();
|
||||
}
|
||||
takes_motor(&MockMotor::new(1));
|
||||
}
|
||||
}
|
||||
@@ -1,103 +0,0 @@
|
||||
//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。
|
||||
|
||||
use crate::pid::Pid;
|
||||
use crate::velocity::VelocityController;
|
||||
|
||||
/// 位置环拓扑。
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum PositionLoop {
|
||||
/// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。
|
||||
Direct(Pid),
|
||||
/// 级联：位置 → 目标转速 → 速度环 → effort_raw。
|
||||
Cascade {
|
||||
/// 外环位置 PID（输出限幅 = 最大 rpm）。
|
||||
pos: Pid,
|
||||
/// 内环速度控制器（输出限幅 = 最大 effort）。
|
||||
vel: VelocityController,
|
||||
},
|
||||
}
|
||||
|
||||
/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PositionController {
|
||||
inner: PositionLoop,
|
||||
}
|
||||
|
||||
impl PositionController {
|
||||
/// 单环位置控制器。
|
||||
#[must_use]
|
||||
pub fn direct(pid: Pid) -> Self {
|
||||
Self { inner: PositionLoop::Direct(pid) }
|
||||
}
|
||||
|
||||
/// 级联位置控制器（限速）。
|
||||
#[must_use]
|
||||
pub fn cascade(pos: Pid, vel: VelocityController) -> Self {
|
||||
Self { inner: PositionLoop::Cascade { pos, vel } }
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含限幅）。
|
||||
///
|
||||
/// `measured_rpm` 仅级联内环使用；单环忽略该参数。
|
||||
pub fn update(
|
||||
&mut self,
|
||||
target_rev: f32,
|
||||
measured_rev: f32,
|
||||
measured_rpm: f32,
|
||||
dt: f32,
|
||||
) -> f32 {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
let vel_target = pos.update(target_rev, measured_rev, dt);
|
||||
vel.update(vel_target, measured_rpm, dt)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 清空内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
match &mut self.inner {
|
||||
PositionLoop::Direct(pid) => pid.reset(),
|
||||
PositionLoop::Cascade { pos, vel } => {
|
||||
pos.reset();
|
||||
vel.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
const DT: f32 = 0.001;
|
||||
|
||||
#[test]
|
||||
fn direct_outputs_pid() {
|
||||
let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0));
|
||||
let out = pc.update(1.0, 0.5, 0.0, DT);
|
||||
assert!((out - 1.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cascade_limits_velocity_then_effort() {
|
||||
// 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。
|
||||
let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0);
|
||||
let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0);
|
||||
let mut pc = PositionController::cascade(pos, vel);
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 外环饱和到 5 rpm，内环 ff=2 → 10 raw。
|
||||
assert!((out - 10.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_state() {
|
||||
let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6));
|
||||
pc.update(1.0, 0.0, 0.0, DT);
|
||||
pc.reset();
|
||||
let out = pc.update(1.0, 0.0, 0.0, DT);
|
||||
// 重置后只剩本步积分：10 * 1 * 0.001。
|
||||
assert!((out - 0.01).abs() < 1e-5, "out = {out}");
|
||||
}
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
//! 电机反馈快照（协议无关，方向修正后的物理量）。
|
||||
//!
|
||||
//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`]
|
||||
//! 或上层完成，不在此泄漏底盘语义。
|
||||
|
||||
/// 电机反馈快照。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct MotorState {
|
||||
/// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。
|
||||
pub rotor_angle_raw: u16,
|
||||
/// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。
|
||||
pub rotor_revolutions: f32,
|
||||
/// 转子转速（rpm，物理方向）。
|
||||
pub rotor_speed_rpm: f32,
|
||||
/// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。
|
||||
pub effort_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
/// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。
|
||||
pub feedback_count: u64,
|
||||
}
|
||||
|
||||
impl MotorState {
|
||||
/// 是否已收到过反馈（上电后首帧门控）。
|
||||
#[must_use]
|
||||
pub fn has_feedback(&self) -> bool {
|
||||
self.feedback_count > 0
|
||||
}
|
||||
}
|
||||
@@ -1,145 +0,0 @@
|
||||
//! 测试替身：内存电机与航向源。
|
||||
|
||||
use std::future::Future;
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use crate::{
|
||||
Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
|
||||
VelocityDrive,
|
||||
};
|
||||
|
||||
/// 锁辅助：中毒锁直接恢复内部数据。
|
||||
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
|
||||
/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。
|
||||
///
|
||||
/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。
|
||||
#[derive(Clone)]
|
||||
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))),
|
||||
last_feedback: Arc::new(Mutex::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// 注入反馈快照（记为「刚刚收到」）。
|
||||
pub fn inject_state(&self, state: MotorState) {
|
||||
self.inject_state_with_age(state, Duration::ZERO);
|
||||
}
|
||||
|
||||
/// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。
|
||||
pub fn inject_state_with_age(&self, state: MotorState, age: Duration) {
|
||||
lock(&self.published).0 = state;
|
||||
let at = Instant::now().checked_sub(age);
|
||||
*lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now));
|
||||
}
|
||||
|
||||
/// 最近一次命令。
|
||||
#[must_use]
|
||||
pub fn last_command(&self) -> Command {
|
||||
lock(&self.published).1
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for MockMotor {
|
||||
fn id(&self) -> u8 {
|
||||
self.id
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
lock(&self.published).0
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
let at = *lock(&self.last_feedback);
|
||||
Some(at?.elapsed())
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for MockMotor {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
lock(&self.published).1 = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
|
||||
impl EffortDrive for MockMotor {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
lock(&self.published).1 = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionDrive for MockMotor {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
lock(&self.published).1 = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
|
||||
impl Disable for MockMotor {
|
||||
fn disable(&self) {
|
||||
lock(&self.published).1 = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
/// 恒定航向测试源。
|
||||
pub struct MockHeading {
|
||||
rad: f32,
|
||||
}
|
||||
|
||||
impl MockHeading {
|
||||
/// 创建固定航向源。
|
||||
#[must_use]
|
||||
pub const fn new(rad: f32) -> Self {
|
||||
Self { rad }
|
||||
}
|
||||
}
|
||||
|
||||
impl HeadingSource for MockHeading {
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send {
|
||||
std::future::ready(self.rad)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn mock_motor_records_commands() {
|
||||
let m = MockMotor::new(3);
|
||||
m.set_velocity(1000.0);
|
||||
assert_eq!(m.last_command(), Command::Velocity(1000.0));
|
||||
m.disable();
|
||||
assert_eq!(m.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_injects_state() {
|
||||
let m = MockMotor::new(1);
|
||||
assert!(!m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_none());
|
||||
m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
|
||||
assert!(m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_feedback_age_can_be_stale() {
|
||||
let m = MockMotor::new(1);
|
||||
m.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(250),
|
||||
);
|
||||
assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200)));
|
||||
}
|
||||
}
|
||||
@@ -1,55 +0,0 @@
|
||||
//! 能力面 trait：小而正交，按硬件事实拆分。
|
||||
//!
|
||||
//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）：
|
||||
//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`]
|
||||
//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]）
|
||||
//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
|
||||
//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::MotorState;
|
||||
|
||||
/// 读反馈。所有可观察设备的最小面。
|
||||
pub trait MotorFeedback: Send + Sync {
|
||||
/// 控制器内的电机 ID（对 DJI 电调即电调 ID）。
|
||||
fn id(&self) -> u8;
|
||||
|
||||
/// 最新反馈快照。
|
||||
fn state(&self) -> MotorState;
|
||||
|
||||
/// 距最近一帧反馈的时长；`None` 表示从未收到。
|
||||
///
|
||||
/// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`]
|
||||
/// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。
|
||||
fn feedback_age(&self) -> Option<Duration>;
|
||||
}
|
||||
|
||||
/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。
|
||||
pub trait EffortDrive: Send + Sync {
|
||||
/// 直接下发开环 raw（协议量程，物理方向）。
|
||||
fn set_effort_raw(&self, raw: i16);
|
||||
}
|
||||
|
||||
/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。
|
||||
pub trait VelocityDrive: Send + Sync {
|
||||
/// 设定目标转子转速。
|
||||
fn set_velocity(&self, rotor_rpm: f32);
|
||||
}
|
||||
|
||||
/// 位置目标（转子多圈，圈；物理方向）。
|
||||
pub trait PositionDrive: Send + Sync {
|
||||
/// 设定目标转子多圈位置。
|
||||
fn set_position(&self, rotor_rev: f32);
|
||||
}
|
||||
|
||||
/// 停用（零输出）。
|
||||
pub trait Disable: Send + Sync {
|
||||
/// 停用电机。
|
||||
fn disable(&self);
|
||||
}
|
||||
|
||||
/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。
|
||||
pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
|
||||
impl<T> Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
@@ -1,180 +0,0 @@
|
||||
//! 速度环控制器：PID + 速度前馈。
|
||||
|
||||
use crate::pid::Pid;
|
||||
|
||||
/// 速度环增益组（可整定参数）。
|
||||
///
|
||||
/// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
|
||||
/// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
|
||||
/// 传递、落盘与热更新，而无需重建整个速度环。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct VelocityGains {
|
||||
/// 比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 微分增益（raw / (rpm/s)）。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityGains {
|
||||
/// 全零增益。
|
||||
pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
|
||||
|
||||
/// 由四个分量构造。
|
||||
#[must_use]
|
||||
pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
|
||||
Self { kp, ki, kd, feedforward }
|
||||
}
|
||||
|
||||
/// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
|
||||
#[must_use]
|
||||
pub fn is_finite(&self) -> bool {
|
||||
self.kp.is_finite()
|
||||
&& self.ki.is_finite()
|
||||
&& self.kd.is_finite()
|
||||
&& self.feedforward.is_finite()
|
||||
}
|
||||
}
|
||||
|
||||
/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
|
||||
///
|
||||
/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
|
||||
/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。
|
||||
/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量
|
||||
/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。
|
||||
///
|
||||
/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct VelocityController {
|
||||
pid: Pid,
|
||||
feedforward: f32,
|
||||
}
|
||||
|
||||
impl VelocityController {
|
||||
/// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。
|
||||
#[must_use]
|
||||
pub fn new(pid: Pid, feedforward: f32) -> Self {
|
||||
Self { pid, feedforward }
|
||||
}
|
||||
|
||||
/// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
|
||||
#[must_use]
|
||||
pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
|
||||
let limit = f32::from(output_limit_raw);
|
||||
let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
|
||||
Self { pid, feedforward: gains.feedforward }
|
||||
}
|
||||
|
||||
/// 当前增益组（含前馈），用于诊断与热更新前的读取。
|
||||
#[must_use]
|
||||
pub fn gains(&self) -> VelocityGains {
|
||||
VelocityGains {
|
||||
kp: self.pid.kp(),
|
||||
ki: self.pid.ki(),
|
||||
kd: self.pid.kd(),
|
||||
feedforward: self.feedforward,
|
||||
}
|
||||
}
|
||||
|
||||
/// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
|
||||
pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
|
||||
let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
|
||||
(pid_out + self.feedforward * target_rpm).clamp(
|
||||
// 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。
|
||||
self.pid.output_min(),
|
||||
self.pid.output_max(),
|
||||
)
|
||||
}
|
||||
|
||||
/// 清空 PID 内部状态（切换模式 / 重新使能时调用）。
|
||||
pub fn reset(&mut self) {
|
||||
self.pid.reset();
|
||||
}
|
||||
|
||||
/// 前馈系数（raw / rpm）。
|
||||
#[must_use]
|
||||
pub fn feedforward(&self) -> f32 {
|
||||
self.feedforward
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn pure_feedforward_tracks_target() {
|
||||
let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0);
|
||||
assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3);
|
||||
// 前馈 -18000 被限幅到 -16384。
|
||||
assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedforward_plus_pid_respects_limit() {
|
||||
let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0);
|
||||
// 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。
|
||||
let out = vc.update(1000.0, 0.0, 0.001);
|
||||
assert!((out - 12000.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_error_yields_pure_feedforward() {
|
||||
// 误差为 0 时 PID 贡献为 0，输出只剩前馈。
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0);
|
||||
let out = vc.update(1000.0, 1000.0, 0.001);
|
||||
assert!((out - 9000.0).abs() < 1e-4, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_clears_pid_state() {
|
||||
let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0);
|
||||
for _ in 0..10 {
|
||||
vc.update(100.0, 0.0, 0.001);
|
||||
}
|
||||
vc.reset();
|
||||
// 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。
|
||||
let out = vc.update(100.0, 0.0, 0.001);
|
||||
assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_round_trips_through_gains() {
|
||||
let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
|
||||
let mut from_data = VelocityController::from_gains(12_000, gains);
|
||||
let mut by_hand =
|
||||
VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
|
||||
|
||||
assert_eq!(from_data.gains(), gains);
|
||||
// 两条构造路径必须逐位一致。
|
||||
for step in 0..16 {
|
||||
let target = 1000.0 + step as f32;
|
||||
let measured = 900.0 + step as f32 * 3.0;
|
||||
assert_eq!(
|
||||
from_data.update(target, measured, 0.001).to_bits(),
|
||||
by_hand.update(target, measured, 0.001).to_bits()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn from_gains_clamps_to_output_limit() {
|
||||
let mut vc = VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
|
||||
assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gains_finiteness_is_checkable() {
|
||||
assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
|
||||
assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
|
||||
assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
[package]
|
||||
name = "motor-model"
|
||||
description = "DC motor physical simulation model"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
motor-protocol = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,426 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DC motor physical models.
|
||||
|
||||
pub mod dc_motor {
|
||||
//! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。
|
||||
//!
|
||||
//! 电调坐标系（正电流 → 正转速），逐拍离散（`dt` 秒）：
|
||||
//!
|
||||
//! 1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才真正作用（模拟
|
||||
//! 链路 + 采样延迟；`0` 表示立即作用）。
|
||||
//! 2. **摩擦**：先由 `u` 与当前运动状态算出净转矩输入
|
||||
//! （静摩擦死区 → 库仑摩擦 → 黏滞阻尼，见 [`PlantMotor::step`]）。
|
||||
//! 3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
|
||||
//! 转速以时间常数 `tau` 一阶趋近（`alpha = 1 − exp(−dt/tau)`）。
|
||||
//! 4. **编码器积分与量化**：机械角按转速积分并对 8192 取模；反馈帧是
|
||||
//! `EscFeedback`，因此角度被截断为 `u16`、转速被截断为 `i16`——量化是
|
||||
//! 协议类型自带的，不需要额外开关。
|
||||
//!
|
||||
//! [`PlantParams::gm3508`] 复现最初的**无摩擦一阶模型**（`chassisd --mock`
|
||||
//! 与闭环测试依赖它，行为逐位不变）；[`PlantParams::gm3508_bench`] 是带摩擦
|
||||
//! 与延迟的**台架量级**预设，供自动整定使用。
|
||||
|
||||
use std::collections::VecDeque;
|
||||
|
||||
use motor_protocol::{ENCODER_TICKS, EscFeedback};
|
||||
|
||||
/// 转速绝对值低于该值（rpm）视为静止——静摩擦判别用。
|
||||
const STICK_RPM_EPS: f32 = 1.0;
|
||||
|
||||
/// 被控对象参数（电调坐标系）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct PlantParams {
|
||||
/// 空载增益（rpm / raw）：`GM3508` 实测约 0.565（≈ 9254 / 16384）。
|
||||
pub gain_rpm_per_raw: f32,
|
||||
/// 机电时间常数（秒）。
|
||||
pub tau_s: f32,
|
||||
/// 转速上限（rpm）。
|
||||
pub max_rpm: f32,
|
||||
/// 静摩擦阈值（raw）：静止且 |u| ≤ 该值时不产生任何运动（stick）。
|
||||
pub static_raw: f32,
|
||||
/// 库仑摩擦（raw）：与运动方向相反的恒定阻力。
|
||||
pub coulomb_raw: f32,
|
||||
/// 黏滞阻尼（raw / rpm）。
|
||||
pub viscous_raw_per_rpm: f32,
|
||||
/// 命令传输延迟（拍，`dt` 的整数倍）。
|
||||
pub delay_ticks: u32,
|
||||
/// 恒定外载（折算成 rpm，直接被目标转速减去）。
|
||||
pub load_rpm: f32,
|
||||
}
|
||||
|
||||
impl PlantParams {
|
||||
/// 无摩擦、无延迟的理想一阶模型（与历史 `PlantMotor::gm3508` 逐位一致）。
|
||||
#[must_use]
|
||||
pub const fn gm3508() -> Self {
|
||||
Self {
|
||||
gain_rpm_per_raw: 9254.0 / 16384.0,
|
||||
tau_s: 0.12,
|
||||
max_rpm: 9254.0,
|
||||
static_raw: 0.0,
|
||||
coulomb_raw: 0.0,
|
||||
viscous_raw_per_rpm: 0.0,
|
||||
delay_ticks: 0,
|
||||
load_rpm: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 台架量级预设：在 [`Self::gm3508`] 上叠加静摩擦 / 库仑摩擦 / 黏滞阻尼
|
||||
/// 与 2 拍命令延迟。
|
||||
///
|
||||
/// 摩擦量级取自 `docs/architecture.md` 与台架记录的空载实测（起动需
|
||||
/// 400~600 raw），**是量级估计而非辨识结果**；需要更贴合实车时可用
|
||||
/// [`Self::with_friction`] / [`Self::with_delay_ticks`] 覆盖。
|
||||
#[must_use]
|
||||
pub const fn gm3508_bench() -> Self {
|
||||
let mut params = Self::gm3508();
|
||||
params.static_raw = 450.0;
|
||||
params.coulomb_raw = 500.0;
|
||||
params.viscous_raw_per_rpm = 0.05;
|
||||
params.delay_ticks = 2;
|
||||
params
|
||||
}
|
||||
|
||||
/// 设置摩擦三件套（静摩擦阈值 / 库仑 / 黏滞）。
|
||||
#[must_use]
|
||||
pub const fn with_friction(
|
||||
mut self,
|
||||
static_raw: f32,
|
||||
coulomb_raw: f32,
|
||||
viscous_raw_per_rpm: f32,
|
||||
) -> Self {
|
||||
self.static_raw = static_raw;
|
||||
self.coulomb_raw = coulomb_raw;
|
||||
self.viscous_raw_per_rpm = viscous_raw_per_rpm;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置命令延迟（拍）。
|
||||
#[must_use]
|
||||
pub const fn with_delay_ticks(mut self, ticks: u32) -> Self {
|
||||
self.delay_ticks = ticks;
|
||||
self
|
||||
}
|
||||
|
||||
/// 设置恒定外载（rpm）。
|
||||
#[must_use]
|
||||
pub const fn with_load_rpm(mut self, rpm: f32) -> Self {
|
||||
self.load_rpm = rpm;
|
||||
self
|
||||
}
|
||||
}
|
||||
|
||||
/// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PlantMotor {
|
||||
params: PlantParams,
|
||||
speed_rpm: f32,
|
||||
angle_raw: f32,
|
||||
/// 命令延迟 FIFO（`delay_ticks == 0` 时恒为空）。
|
||||
delay_line: VecDeque<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, "复位后角度归零");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod gm3508 {
|
||||
//! GM3508 的被控对象参数（ARCHITECTURE.md §5.6 / §8.2）。
|
||||
//!
|
||||
//! 这里只放**有实测依据**的两组参数：
|
||||
//!
|
||||
//! - [`ideal_params`]：无摩擦、无延迟的理想一阶模型。历史闭环测试与
|
||||
//! `chassisd --mock` 依赖它，因此行为**逐位不变**；
|
||||
//! - [`bench_params`]：在理想模型上叠加台架实测的静 / 库仑摩擦
|
||||
//! （400~600 raw 量级）与 2 拍命令延迟，是自动整定的默认被控对象。
|
||||
//!
|
||||
//! 数值口径见 `docs/chassis.md` §7.4 与 `config/tune.toml`。
|
||||
|
||||
use super::dc_motor::PlantParams;
|
||||
|
||||
/// 理想模型：无摩擦、无延迟（历史默认行为）。
|
||||
#[must_use]
|
||||
pub fn ideal_params() -> PlantParams {
|
||||
PlantParams::gm3508()
|
||||
}
|
||||
|
||||
/// 台架量级模型：静 / 库仑摩擦 + 2 拍延迟（整定默认）。
|
||||
#[must_use]
|
||||
pub fn bench_params() -> PlantParams {
|
||||
PlantParams::gm3508_bench()
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn ideal_model_has_no_friction_or_delay() {
|
||||
let params = ideal_params();
|
||||
assert!(params.static_raw.abs() < 1e-6);
|
||||
assert!(params.coulomb_raw.abs() < 1e-6);
|
||||
assert_eq!(params.delay_ticks, 0);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn bench_model_has_the_measured_friction_and_delay() {
|
||||
let params = bench_params();
|
||||
assert!((400.0..=600.0).contains(¶ms.static_raw), "{}", params.static_raw);
|
||||
assert!(params.coulomb_raw > 0.0);
|
||||
assert_eq!(params.delay_ticks, 2);
|
||||
// 台架模型只在摩擦 / 延迟上更真实，增益与上限应保持一致。
|
||||
let ideal = ideal_params();
|
||||
assert!((params.gain_rpm_per_raw - ideal.gain_rpm_per_raw).abs() < 1e-9);
|
||||
assert!((params.max_rpm - ideal.max_rpm).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use dc_motor::{PlantMotor, PlantParams};
|
||||
pub use gm3508::{bench_params, ideal_params};
|
||||
@@ -0,0 +1,18 @@
|
||||
[package]
|
||||
name = "motor-protocol"
|
||||
description = "DJI C620/C610 and GM6020 wire protocols"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
proptest = { workspace = true }
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,911 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! DJI motor wire protocols.
|
||||
|
||||
pub mod common {
|
||||
//! DJI 电调（C620 / C610）CAN 协议编解码。
|
||||
//!
|
||||
//! 帧格式（大端）：
|
||||
//!
|
||||
//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）：
|
||||
//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令，
|
||||
//! 槽位按组内电调顺序。
|
||||
//! - 反馈（`0x200 + 电调ID`）：
|
||||
//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。
|
||||
//!
|
||||
//! 本模块只做纯编解码；主机侧速度/位置环由 `motor-control` 的算法在
|
||||
//! `motor / esc-bus` 的总线驱动中按节拍调用。
|
||||
|
||||
use crate::frame::{
|
||||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE,
|
||||
};
|
||||
use thiserror::Error;
|
||||
|
||||
/// 电调 ID 下限。
|
||||
pub const MIN_ESC_ID: u8 = 1;
|
||||
/// 电调 ID 上限。
|
||||
pub const MAX_ESC_ID: u8 = 8;
|
||||
|
||||
/// 电调型号决定电流标度。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum EscModel {
|
||||
/// C620（配 GM3508）：±16384 ↔ ±20 A。
|
||||
C620,
|
||||
/// C610（配 M2006）：±10000 ↔ ±10 A。
|
||||
///
|
||||
/// 接入前需实测核对。
|
||||
C610,
|
||||
}
|
||||
|
||||
impl EscModel {
|
||||
/// 满量程电流命令（raw）。
|
||||
#[must_use]
|
||||
pub const fn max_current_raw(self) -> i16 {
|
||||
match self {
|
||||
Self::C620 => 16384,
|
||||
Self::C610 => 10_000,
|
||||
}
|
||||
}
|
||||
|
||||
/// 满量程电流（安培）。
|
||||
#[must_use]
|
||||
pub const fn max_current_amp(self) -> f32 {
|
||||
match self {
|
||||
Self::C620 => 20.0,
|
||||
Self::C610 => 10.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 安培 → raw（限幅）。
|
||||
#[must_use]
|
||||
pub fn raw_from_amp(self, amp: f32) -> i16 {
|
||||
let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp();
|
||||
(raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw()))
|
||||
as i16)
|
||||
.clamp(-self.max_current_raw(), self.max_current_raw())
|
||||
}
|
||||
|
||||
/// raw → 安培。
|
||||
#[must_use]
|
||||
pub fn amp_from_raw(self, raw: i16) -> f32 {
|
||||
f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw())
|
||||
}
|
||||
}
|
||||
|
||||
/// 一帧电调反馈（原始、电调侧，未经方向修正）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct EscFeedback {
|
||||
/// 转子机械角原始值（0..8191，对应 0..360°）。
|
||||
pub angle_raw: u16,
|
||||
/// 转子转速（rpm，电调侧符号）。
|
||||
pub speed_rpm: i16,
|
||||
/// 转矩电流 raw（电调侧符号）。
|
||||
pub current_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
}
|
||||
|
||||
/// 帧解码错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum DecodeError {
|
||||
/// 帧不属于电调反馈范围（0x201..=0x208）。
|
||||
#[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")]
|
||||
NotFeedback {
|
||||
/// 收到的帧 ID。
|
||||
id: u32,
|
||||
},
|
||||
/// 数据长度不足 8 字节（协议载荷固定 8 字节）。
|
||||
#[error("frame carries only {dlc} bytes; the ESC feedback payload needs 8")]
|
||||
ShortFrame {
|
||||
/// 实际数据长度码。
|
||||
dlc: u8,
|
||||
},
|
||||
}
|
||||
|
||||
/// 电调 ID → 其电流命令所在的组控制帧 ID。
|
||||
///
|
||||
/// 返回 `None` 表示 ID 越界。
|
||||
#[must_use]
|
||||
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
|
||||
match esc_id {
|
||||
1..=4 => Some(COMMAND_FRAME_ID_PRIMARY),
|
||||
5..=8 => Some(COMMAND_FRAME_ID_SECONDARY),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 电调 ID → 其反馈帧 ID（`0x200 + 电调ID`）。
|
||||
///
|
||||
/// 返回 `None` 表示 ID 越界。GM6020 用**另一张表**，见
|
||||
/// [`crate::gm6020::feedback_frame_id`]。
|
||||
#[must_use]
|
||||
pub const fn feedback_frame_id(esc_id: u8) -> Option<u32> {
|
||||
if esc_id >= MIN_ESC_ID && esc_id <= MAX_ESC_ID {
|
||||
Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// 将 raw 电流限幅到电调满量程。
|
||||
#[must_use]
|
||||
pub const fn clamp_current(model: EscModel, raw: i16) -> i16 {
|
||||
if raw > model.max_current_raw() {
|
||||
model.max_current_raw()
|
||||
} else if raw < -model.max_current_raw() {
|
||||
-model.max_current_raw()
|
||||
} else {
|
||||
raw
|
||||
}
|
||||
}
|
||||
|
||||
/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。
|
||||
///
|
||||
/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。
|
||||
/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成
|
||||
/// （见 [`clamp_current`]）。
|
||||
#[must_use]
|
||||
pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
for (slot, raw) in currents.iter().enumerate() {
|
||||
data[slot * 2] = (raw >> 8) as u8;
|
||||
data[slot * 2 + 1] = (raw & 0xFF) as u8;
|
||||
}
|
||||
CanFrame::new(frame_id, data)
|
||||
}
|
||||
|
||||
/// 组装一帧电调反馈（仿真器 / 测试用）。
|
||||
#[must_use]
|
||||
pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
|
||||
data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
|
||||
data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
|
||||
data[6] = feedback.temperature_c as u8;
|
||||
CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
|
||||
}
|
||||
|
||||
/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]；
|
||||
/// 数据长度不足 8 字节时返回 [`DecodeError::ShortFrame`]。
|
||||
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
|
||||
if !frame.is_complete() {
|
||||
return Err(DecodeError::ShortFrame { dlc: frame.dlc });
|
||||
}
|
||||
if frame.id <= FEEDBACK_FRAME_ID_BASE
|
||||
|| frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID)
|
||||
{
|
||||
return Err(DecodeError::NotFeedback { id: frame.id });
|
||||
}
|
||||
let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8;
|
||||
Ok((
|
||||
esc_id,
|
||||
EscFeedback {
|
||||
angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]),
|
||||
speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]),
|
||||
current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]),
|
||||
temperature_c: frame.data[6] as i8,
|
||||
},
|
||||
))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn command_frame_id_routing() {
|
||||
assert_eq!(command_frame_id(1), Some(0x200));
|
||||
assert_eq!(command_frame_id(4), Some(0x200));
|
||||
assert_eq!(command_frame_id(5), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(8), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(0), None);
|
||||
assert_eq!(command_frame_id(9), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encode_current_frame_bytes() {
|
||||
let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]);
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]);
|
||||
|
||||
let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]);
|
||||
assert_eq!(frame.id, 0x1FF);
|
||||
assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_known_bytes() {
|
||||
let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||||
let (esc_id, fb) = decode_feedback(&frame).unwrap();
|
||||
assert_eq!(esc_id, 5);
|
||||
assert_eq!(fb.angle_raw, 8191);
|
||||
assert_eq!(fb.speed_rpm, -364);
|
||||
assert_eq!(fb.current_raw, 123);
|
||||
assert_eq!(fb.temperature_c, 45);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_rejects_non_feedback_ids() {
|
||||
for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] {
|
||||
let frame = CanFrame::new(id, [0; 8]);
|
||||
assert_eq!(
|
||||
decode_feedback(&frame),
|
||||
Err(DecodeError::NotFeedback { id }),
|
||||
"id = 0x{id:03X}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_roundtrip_all_ids() {
|
||||
let fb = EscFeedback {
|
||||
angle_raw: 8191,
|
||||
speed_rpm: -9254,
|
||||
current_raw: -16384,
|
||||
temperature_c: -40,
|
||||
};
|
||||
for esc_id in MIN_ESC_ID..=MAX_ESC_ID {
|
||||
let frame = encode_feedback_frame(esc_id, &fb);
|
||||
assert_eq!(frame.id, 0x200 + u32::from(esc_id));
|
||||
assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn amp_conversion() {
|
||||
let c620 = EscModel::C620;
|
||||
assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4);
|
||||
assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4);
|
||||
assert_eq!(c620.raw_from_amp(20.0), 16384);
|
||||
assert_eq!(c620.raw_from_amp(-5.0), -4096);
|
||||
// 超量程限幅。
|
||||
assert_eq!(c620.raw_from_amp(99.0), 16384);
|
||||
let c610 = EscModel::C610;
|
||||
assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4);
|
||||
assert_eq!(c610.raw_from_amp(10.0), 10000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn clamp_current_respects_model() {
|
||||
assert_eq!(clamp_current(EscModel::C620, 20000), 16384);
|
||||
assert_eq!(clamp_current(EscModel::C610, 15000), 10000);
|
||||
assert_eq!(clamp_current(EscModel::C620, 1000), 1000);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod gm6020 {
|
||||
//! GM6020 云台电机协议（ARCHITECTURE.md §5.2）。
|
||||
//!
|
||||
//! 与 C620/C610 的差别只有两点，因此两个协议族共享同一套帧布局：
|
||||
//!
|
||||
//! | 项目 | C620/C610（M3508/M2006） | GM6020 |
|
||||
//! | --- | --- | --- |
|
||||
//! | 控制量 | **电流**（±16384 / ±10000 ↔ 满量程电流） | **电压**（±30000 ↔ 满电压） |
|
||||
//! | 控制帧 ID | `0x200`（1–4）、`0x1FF`（5–8） | `0x1FF`（1–4）、`0x2FF`（5–8） |
|
||||
//! | 反馈帧 ID | `0x200 + ID`（0x201–0x208） | `0x204 + ID`（0x205–0x208，ID 1–4） |
|
||||
//! | 反馈载荷 | u16 角度 + i16 转速 + i16 电流 + i8 温度 | 同上 |
|
||||
//!
|
||||
//! > 命名说明：ARCHITECTURE.md §5.2 把命令字段写作 `current`，但 GM6020 的
|
||||
//! > 控制字是**电压**占空比命令（±30000）。这里用 `voltage_raw` 以免把
|
||||
//! > "电流环在电调内部"这件事记反——GM6020 内部同样只闭电流环。
|
||||
|
||||
use can::CanFrame;
|
||||
|
||||
use crate::common::{DecodeError, MAX_ESC_ID, MIN_ESC_ID};
|
||||
use crate::id::MotorId;
|
||||
|
||||
/// 控制帧 ID：GM6020 ID 1–4。
|
||||
pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x1FF;
|
||||
/// 控制帧 ID：GM6020 ID 5–8。
|
||||
pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x2FF;
|
||||
/// 反馈帧 ID 基址：`0x204 + ID`（ID 1–4 → 0x205–0x208）。
|
||||
pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x204;
|
||||
/// 电压命令满量程（raw）。
|
||||
pub const VOLTAGE_MAX_RAW: i16 = 30_000;
|
||||
/// 编码器单圈刻度（与 C620 相同）。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// GM6020 一帧反馈。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub struct Gm6020Feedback {
|
||||
/// 机械角（度，0..360）。
|
||||
pub angle_deg: f32,
|
||||
/// 单圈原始角度（0..8191）。
|
||||
pub angle_raw: u16,
|
||||
/// 转子转速（rpm）。
|
||||
pub speed_rpm: i16,
|
||||
/// 转矩电流（raw，±30000 ↔ 满量程）。
|
||||
pub current_raw: i16,
|
||||
/// 温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
}
|
||||
|
||||
/// GM6020 电压命令。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Gm6020Command {
|
||||
/// 电压命令（raw，`-30000..=30000`）。
|
||||
pub voltage_raw: i16,
|
||||
}
|
||||
|
||||
impl Gm6020Command {
|
||||
/// 构造并夹到合法范围。
|
||||
#[must_use]
|
||||
pub const fn new(voltage_raw: i16) -> Self {
|
||||
Self { voltage_raw: clamp_voltage(voltage_raw) }
|
||||
}
|
||||
|
||||
/// 编码为**广播控制帧**（同组四个槽位）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 帧 ID 非法时返回 [`DecodeError::NotFeedback`] 之外的 `BadFrameId`。
|
||||
///
|
||||
/// 说明：单电机编码只是"其余槽位填 0"的特例，因此本函数要求调用方
|
||||
/// 提供整组电压；单电机场景见 [`Gm6020Command::to_frame`]。
|
||||
#[must_use]
|
||||
pub fn encode_group(frame_id: u32, voltages: [i16; 4]) -> CanFrame {
|
||||
encode_voltage_frame(frame_id, voltages)
|
||||
}
|
||||
|
||||
/// 单电机编码：其余槽位补 0（会**覆盖**同组其它电机，仅用于单机联调）。
|
||||
///
|
||||
/// 注意用的是 **GM6020 自己的**控制帧映射（`0x1FF` / `0x2FF`），
|
||||
/// 而不是 [`MotorId::command_frame_id`] 的 C620 映射（`0x200` / `0x1FF`）——
|
||||
/// 两个协议族的帧 ID 表不同，混淆会导致云台收不到任何命令。
|
||||
#[must_use]
|
||||
pub fn to_frame<const ID: u8>(&self, id: MotorId<ID>) -> CanFrame {
|
||||
let esc_id = id.get();
|
||||
let mut voltages = [0i16; 4];
|
||||
let slot = command_slot(esc_id).unwrap_or(0);
|
||||
voltages[slot] = self.voltage_raw;
|
||||
let frame_id = command_frame_id(esc_id).unwrap_or(COMMAND_FRAME_ID_PRIMARY);
|
||||
encode_voltage_frame(frame_id, voltages)
|
||||
}
|
||||
}
|
||||
|
||||
/// 夹到电压满量程。
|
||||
#[must_use]
|
||||
pub const fn clamp_voltage(raw: i16) -> i16 {
|
||||
// `i16::clamp` 在 const fn 里尚不可用（Ord 未 const 稳定），手写三段。
|
||||
if raw > VOLTAGE_MAX_RAW {
|
||||
VOLTAGE_MAX_RAW
|
||||
} else if raw < -VOLTAGE_MAX_RAW {
|
||||
-VOLTAGE_MAX_RAW
|
||||
} else {
|
||||
raw
|
||||
}
|
||||
}
|
||||
|
||||
/// 由电调 ID 推出控制帧槽位（`None` 表示 ID 越界）。
|
||||
#[must_use]
|
||||
pub const fn command_slot(esc_id: u8) -> Option<usize> {
|
||||
if esc_id >= MIN_ESC_ID && esc_id <= 4 {
|
||||
Some((esc_id - 1) as usize)
|
||||
} else if esc_id >= 5 && esc_id <= MAX_ESC_ID {
|
||||
Some((esc_id - 5) as usize)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// 由电调 ID 推出控制帧 ID（`None` 表示 ID 越界）。
|
||||
#[must_use]
|
||||
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
|
||||
match command_slot(esc_id) {
|
||||
Some(_) if esc_id <= 4 => Some(COMMAND_FRAME_ID_PRIMARY),
|
||||
Some(_) => Some(COMMAND_FRAME_ID_SECONDARY),
|
||||
None => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 反馈帧 ID（`0x204 + ID`；ID 5–8 为 `0x208 + ID`，见模块文档）。
|
||||
#[must_use]
|
||||
pub const fn feedback_frame_id(esc_id: u8) -> Option<u32> {
|
||||
if esc_id >= MIN_ESC_ID && esc_id <= 4 {
|
||||
Some(FEEDBACK_FRAME_ID_BASE + esc_id as u32)
|
||||
} else if esc_id >= 5 && esc_id <= MAX_ESC_ID {
|
||||
Some(0x208 + esc_id as u32)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
/// 编码一组电压命令（4 × i16 大端）。
|
||||
#[must_use]
|
||||
pub fn encode_voltage_frame(frame_id: u32, voltages: [i16; 4]) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
for (slot, raw) in voltages.iter().enumerate() {
|
||||
let clamped = clamp_voltage(*raw);
|
||||
data[slot * 2] = (clamped >> 8) as u8;
|
||||
data[slot * 2 + 1] = (clamped & 0xFF) as u8;
|
||||
}
|
||||
CanFrame::new(frame_id, data)
|
||||
}
|
||||
|
||||
/// 组装一帧 GM6020 反馈（仿真器 / 测试用）。
|
||||
#[must_use]
|
||||
pub fn encode_feedback_frame(esc_id: u8, feedback: &Gm6020Feedback) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
|
||||
data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
|
||||
data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
|
||||
data[6] = feedback.temperature_c as u8;
|
||||
CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
|
||||
}
|
||||
|
||||
/// 解码一帧 GM6020 反馈，返回 `(电调ID, 反馈)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 帧 ID 不在 GM6020 反馈范围（0x205–0x208、0x20D–0x210）内时返回
|
||||
/// [`DecodeError::NotFeedback`]；长度不足时返回 [`DecodeError::ShortFrame`]。
|
||||
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, Gm6020Feedback), DecodeError> {
|
||||
if !frame.is_complete() {
|
||||
return Err(DecodeError::ShortFrame { dlc: frame.dlc });
|
||||
}
|
||||
let esc_id = if (0x205..=0x208).contains(&frame.id) {
|
||||
(frame.id - FEEDBACK_FRAME_ID_BASE) as u8
|
||||
} else if (0x20D..=0x210).contains(&frame.id) {
|
||||
(frame.id - 0x208) as u8
|
||||
} else {
|
||||
return Err(DecodeError::NotFeedback { id: frame.id });
|
||||
};
|
||||
let angle_raw = u16::from_be_bytes([frame.data[0], frame.data[1]]);
|
||||
let speed_rpm = i16::from_be_bytes([frame.data[2], frame.data[3]]);
|
||||
let current_raw = i16::from_be_bytes([frame.data[4], frame.data[5]]);
|
||||
let temperature_c = frame.data[6] as i8;
|
||||
Ok((
|
||||
esc_id,
|
||||
Gm6020Feedback {
|
||||
angle_raw: angle_raw % ENCODER_TICKS,
|
||||
angle_deg: f32::from(angle_raw) * 360.0 / f32::from(ENCODER_TICKS),
|
||||
speed_rpm,
|
||||
current_raw,
|
||||
temperature_c,
|
||||
},
|
||||
))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn voltage_is_clamped_to_full_scale() {
|
||||
assert_eq!(Gm6020Command::new(32_000).voltage_raw, VOLTAGE_MAX_RAW);
|
||||
assert_eq!(Gm6020Command::new(-32_000).voltage_raw, -VOLTAGE_MAX_RAW);
|
||||
assert_eq!(Gm6020Command::new(1234).voltage_raw, 1234);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn single_motor_encoding_lands_in_the_right_slot() {
|
||||
let frame = Gm6020Command::new(1000).to_frame(MotorId::<1>::NEW);
|
||||
assert_eq!(frame.id, 0x1FF);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 1000);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), 0);
|
||||
|
||||
let frame = Gm6020Command::new(-1000).to_frame(MotorId::<6>::NEW);
|
||||
assert_eq!(frame.id, 0x2FF, "ID 5–8 走副组帧");
|
||||
assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -1000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn group_encoding_matches_slot_order() {
|
||||
let frame = encode_voltage_frame(COMMAND_FRAME_ID_PRIMARY, [10, -20, 30, -40]);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[0], frame.data[1]]), 10);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[2], frame.data[3]]), -20);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[4], frame.data[5]]), 30);
|
||||
assert_eq!(i16::from_be_bytes([frame.data[6], frame.data[7]]), -40);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_round_trips_and_converts_angle() {
|
||||
let feedback = Gm6020Feedback {
|
||||
angle_deg: 0.0,
|
||||
angle_raw: 2048,
|
||||
speed_rpm: -321,
|
||||
current_raw: 4567,
|
||||
temperature_c: 42,
|
||||
};
|
||||
let frame = encode_feedback_frame(2, &feedback);
|
||||
assert_eq!(frame.id, 0x206);
|
||||
let (id, decoded) = decode_feedback(&frame).unwrap();
|
||||
assert_eq!(id, 2);
|
||||
assert_eq!(decoded.angle_raw, 2048);
|
||||
assert!((decoded.angle_deg - 90.0).abs() < 1e-3, "2048/8192 应为 90°");
|
||||
assert_eq!(decoded.speed_rpm, -321);
|
||||
assert_eq!(decoded.current_raw, 4567);
|
||||
assert_eq!(decoded.temperature_c, 42);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_id_range_is_enforced() {
|
||||
assert_eq!(feedback_frame_id(1), Some(0x205));
|
||||
assert_eq!(feedback_frame_id(4), Some(0x208));
|
||||
assert_eq!(feedback_frame_id(9), None);
|
||||
let c620 = CanFrame::new(0x201, [0; 8]);
|
||||
assert!(matches!(decode_feedback(&c620), Err(DecodeError::NotFeedback { .. })));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn short_frames_are_rejected() {
|
||||
let short = CanFrame::new(0x205, [0; 8]).with_dlc(4);
|
||||
assert!(matches!(decode_feedback(&short), Err(DecodeError::ShortFrame { .. })));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod m2006 {
|
||||
//! M2006 + C610：拨弹 / 小负载电机。本项目只用到协议常量，用于未来的
|
||||
//! 发射机构（发射机构尚未实现，见 ARCHITECTURE.md §5.7 的 `shooter` 任务）。
|
||||
|
||||
use crate::common::EscModel;
|
||||
|
||||
/// C610 电调型号（M2006 的配套电调）。
|
||||
///
|
||||
/// 电流标度 ±10000 ↔ ±10 A **接入前需实测核对**（ARCHITECTURE.md §2.2 注）。
|
||||
pub const C610: EscModel = EscModel::C610;
|
||||
|
||||
/// 减速比（转子 : 输出轴）＝ 36 : 1（M2006 常见配置，接硬件前核对）。
|
||||
pub const GEAR_RATIO: f32 = 36.0;
|
||||
|
||||
/// 编码器单圈刻度。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// 额定电流满量程（A，对应 raw 满量程 ±10000）。
|
||||
pub const MAX_CURRENT_AMP: f32 = 10.0;
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::common::clamp_current;
|
||||
|
||||
#[test]
|
||||
fn c610_saturation_uses_the_c610_scale() {
|
||||
assert_eq!(C610.max_current_raw(), 10_000);
|
||||
assert_eq!(clamp_current(C610, 20_000), 10_000);
|
||||
assert_eq!(clamp_current(C610, -20_000), -10_000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn max_current_amp_matches_the_raw_scale() {
|
||||
assert!((C610.max_current_amp() - MAX_CURRENT_AMP).abs() < 1e-6);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod m3508 {
|
||||
//! GM3508 + C620：底盘驱动轮的电流协议族（本项目的主力电机）。
|
||||
//!
|
||||
//! 全部编解码都在 [`crate::common`]（C620 与 C610 帧布局相同，只有
|
||||
//! 电流标度不同），本模块只固定 GM3508 的**型号常量与机械参数**，避免
|
||||
//! 每个使用点各自抄一遍数字。
|
||||
|
||||
use crate::common::EscModel;
|
||||
|
||||
/// C620 电调型号（GM3508 的配套电调）。
|
||||
pub const C620: EscModel = EscModel::C620;
|
||||
|
||||
/// 减速比（转子 : 输出轴）＝ 3591 / 187 ≈ 19.203。
|
||||
pub const GEAR_RATIO: f32 = 3591.0 / 187.0;
|
||||
|
||||
/// 编码器单圈刻度。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// 额定电流满量程（A，对应 raw 满量程 ±16384）。
|
||||
pub const MAX_CURRENT_AMP: f32 = 20.0;
|
||||
|
||||
/// 空载转速-电流增益（rpm / raw），来自台架实测（≈ 9254 / 16384）。
|
||||
pub const NO_LOAD_GAIN_RPM_PER_RAW: f32 = 9254.0 / 16384.0;
|
||||
|
||||
/// 空载转速上限（rpm，转子）。
|
||||
pub const NO_LOAD_MAX_RPM: f32 = 9254.0;
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::common::clamp_current;
|
||||
|
||||
#[test]
|
||||
fn gear_ratio_matches_the_manual() {
|
||||
assert!((GEAR_RATIO - 19.203).abs() < 5e-3, "{GEAR_RATIO}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn c620_saturation_uses_the_c620_scale() {
|
||||
assert_eq!(C620.max_current_raw(), 16384);
|
||||
assert_eq!(clamp_current(C620, 30_000), 16384);
|
||||
assert_eq!(clamp_current(C620, -30_000), -16384);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn no_load_gain_is_the_inverse_of_the_documented_feedforward() {
|
||||
// 台架文档里的前馈 ≈ 1.77 raw/rpm 正是 1 / 空载增益。
|
||||
let feedforward = 1.0 / NO_LOAD_GAIN_RPM_PER_RAW;
|
||||
assert!((feedforward - 1.77).abs() < 0.02, "{feedforward}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod frame {
|
||||
//! 协议帧 ID 常量与帧类型再导出。
|
||||
//!
|
||||
//! 帧类型（[`CanFrame`]）由 L1 的 [`can`] 提供；本模块只保留各协议族的
|
||||
//! **ID 约定**，因此"改 ID 常量"与"改帧结构"是两件互不影响的事。
|
||||
//!
|
||||
//! - C620/C610 电流协议族：本模块的常量 + [`crate::common`]；
|
||||
//! - GM6020 电压协议族：见 [`crate::gm6020`]（ID 常量在那边）；
|
||||
//! - DETA10 IMU：见 [`crate::delta10`]。
|
||||
|
||||
pub use can::CanFrame;
|
||||
|
||||
/// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。
|
||||
pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200;
|
||||
/// 控制帧 ID：电调 5–8 的电流命令。
|
||||
pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF;
|
||||
/// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。
|
||||
pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200;
|
||||
}
|
||||
|
||||
pub mod id {
|
||||
//! 编译期电机 ID 校验（ARCHITECTURE.md §5.2）。
|
||||
//!
|
||||
//! 电调 ID 写错是实车上最常见的低级错误之一：ID 9 不会报错，只会**静默**
|
||||
//! 收不到反馈。把 ID 放进 const 泛型后，"ID 必须在 1–8"变成**编译期**约束，
|
||||
//! 接错线之前先接错类型。
|
||||
//!
|
||||
//! ```ignore
|
||||
//! // 编译通过
|
||||
//! let id = MotorId::<3>::NEW;
|
||||
//! // 编译失败：Motor ID must be 1-8
|
||||
//! let bad = MotorId::<9>::NEW;
|
||||
//! ```
|
||||
|
||||
/// 合法的电调 ID 范围。
|
||||
pub const MOTOR_ID_RANGE: core::ops::RangeInclusive<u8> = 1..=8;
|
||||
|
||||
/// 编译期已校验的电调 ID。
|
||||
///
|
||||
/// 只能通过 [`MotorId::NEW`] 构造；该常量的求值会断言 ID 在 1–8 之间，
|
||||
/// 越界即编译失败。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Default)]
|
||||
pub struct MotorId<const ID: u8>;
|
||||
|
||||
impl<const ID: u8> MotorId<ID> {
|
||||
/// 编译期校验并构造。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `ID` 不在 `1..=8` 时在**编译期** panic（const 求值失败即编译错误）。
|
||||
pub const NEW: Self = {
|
||||
assert!(ID >= 1 && ID <= 8, "Motor ID must be 1-8");
|
||||
Self
|
||||
};
|
||||
|
||||
/// 取出 ID 数值。
|
||||
#[must_use]
|
||||
pub const fn get(&self) -> u8 {
|
||||
ID
|
||||
}
|
||||
|
||||
/// 所属控制帧 ID（**C620/C610 电流协议族**：主组 `0x200` / 副组 `0x1FF`）。
|
||||
///
|
||||
/// GM6020 的电压协议族用的是**另一张表**（`0x1FF` / `0x2FF`），见
|
||||
/// [`crate::gm6020::command_frame_id`]。两者不可混用。
|
||||
#[must_use]
|
||||
pub const fn command_frame_id(&self) -> u32 {
|
||||
if ID <= 4 {
|
||||
crate::frame::COMMAND_FRAME_ID_PRIMARY
|
||||
} else {
|
||||
crate::frame::COMMAND_FRAME_ID_SECONDARY
|
||||
}
|
||||
}
|
||||
|
||||
/// 组内槽位（0..4），即控制帧里第几个 i16。
|
||||
#[must_use]
|
||||
pub const fn slot(&self) -> usize {
|
||||
if ID <= 4 { (ID - 1) as usize } else { (ID - 5) as usize }
|
||||
}
|
||||
|
||||
/// 反馈帧 ID（`0x200 + ID`）。
|
||||
#[must_use]
|
||||
pub const fn feedback_frame_id(&self) -> u32 {
|
||||
crate::frame::FEEDBACK_FRAME_ID_BASE + ID as u32
|
||||
}
|
||||
}
|
||||
|
||||
/// 运行期已知 ID 的校验（用于读配置 / 解析命令行）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// ID 不在 `1..=8` 时返回原值，便于报错信息带上用户输入。
|
||||
pub const fn validate_motor_id(id: u8) -> Result<u8, u8> {
|
||||
if id >= 1 && id <= 8 { Ok(id) } else { Err(id) }
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn primary_and_secondary_groups_split_correctly() {
|
||||
assert_eq!(MotorId::<1>::NEW.command_frame_id(), 0x200);
|
||||
assert_eq!(MotorId::<4>::NEW.command_frame_id(), 0x200);
|
||||
assert_eq!(MotorId::<5>::NEW.command_frame_id(), 0x1FF);
|
||||
assert_eq!(MotorId::<8>::NEW.command_frame_id(), 0x1FF);
|
||||
|
||||
assert_eq!(MotorId::<1>::NEW.slot(), 0);
|
||||
assert_eq!(MotorId::<4>::NEW.slot(), 3);
|
||||
assert_eq!(MotorId::<5>::NEW.slot(), 0);
|
||||
assert_eq!(MotorId::<8>::NEW.slot(), 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_ids_follow_0x200_plus_id() {
|
||||
assert_eq!(MotorId::<1>::NEW.feedback_frame_id(), 0x201);
|
||||
assert_eq!(MotorId::<8>::NEW.feedback_frame_id(), 0x208);
|
||||
assert_eq!(MotorId::<3>::NEW.get(), 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn runtime_validation_returns_the_offending_id() {
|
||||
assert_eq!(validate_motor_id(1), Ok(1));
|
||||
assert_eq!(validate_motor_id(8), Ok(8));
|
||||
assert_eq!(validate_motor_id(0), Err(0));
|
||||
assert_eq!(validate_motor_id(9), Err(9));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod turn {
|
||||
//! 多圈累计：单圈编码器原始角 → 连续圈数。
|
||||
|
||||
/// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// 半圈阈值：帧间角变化超过它视为发生回绕。
|
||||
const HALF_TURN_TICKS: i32 = 4096;
|
||||
|
||||
/// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。
|
||||
///
|
||||
/// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约
|
||||
/// 9300 rpm ≈ 0.155 圈/ms，余量充足。
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct TurnTracker {
|
||||
revolutions: i32,
|
||||
prev_raw: Option<u16>,
|
||||
last_angle_rev: f32,
|
||||
}
|
||||
|
||||
impl TurnTracker {
|
||||
/// 创建跟踪器。
|
||||
#[must_use]
|
||||
pub const fn new() -> Self {
|
||||
Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 }
|
||||
}
|
||||
|
||||
/// 送入新的原始角，返回连续累计角（圈）。
|
||||
pub fn update(&mut self, raw: u16) -> f32 {
|
||||
if let Some(prev) = self.prev_raw {
|
||||
let delta = i32::from(raw) - i32::from(prev);
|
||||
if delta > HALF_TURN_TICKS {
|
||||
// 正向跨越 8191 → 0。
|
||||
self.revolutions -= 1;
|
||||
} else if delta < -HALF_TURN_TICKS {
|
||||
// 反向跨越 0 → 8191。
|
||||
self.revolutions += 1;
|
||||
}
|
||||
}
|
||||
self.prev_raw = Some(raw);
|
||||
self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0;
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 最近一次 [`TurnTracker::update`] 的返回值。
|
||||
#[must_use]
|
||||
pub const fn last_angle_rev(&self) -> f32 {
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 清零回到初始状态。
|
||||
pub fn reset(&mut self) {
|
||||
*self = Self::new();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn first_reading_is_raw_fraction() {
|
||||
let mut t = TurnTracker::new();
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn forward_wrap_counts_up() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
let after = t.update(100); // 8000 → 8191 → 0 → 100
|
||||
assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reverse_wrap_counts_down() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(100);
|
||||
let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向）
|
||||
assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn small_deltas_do_not_wrap() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(0);
|
||||
assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4);
|
||||
assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn multi_turn_forward_then_reverse_returns() {
|
||||
let mut t = TurnTracker::new();
|
||||
// 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。
|
||||
for _ in 0..3 {
|
||||
t.update(0);
|
||||
t.update(4000);
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
}
|
||||
let fwd = t.last_angle_rev();
|
||||
let expected_fwd = 3.0 + 100.0 / 8192.0;
|
||||
assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}");
|
||||
|
||||
// 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。
|
||||
for _ in 0..3 {
|
||||
t.update(8000);
|
||||
t.update(4000);
|
||||
t.update(0);
|
||||
}
|
||||
let back = t.last_angle_rev();
|
||||
assert!(back.abs() < 1e-4, "should return to 0, got {back}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn jitter_at_boundary_stays_put() {
|
||||
// 0 附近抖动不产生虚假圈数。
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8191);
|
||||
assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次
|
||||
assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4);
|
||||
assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回
|
||||
assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_forgets_history() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
t.reset();
|
||||
assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits());
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use common::{
|
||||
DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id,
|
||||
decode_feedback, encode_current_frame, encode_feedback_frame, feedback_frame_id,
|
||||
};
|
||||
pub use frame::*;
|
||||
pub use gm6020::{Gm6020Command, Gm6020Feedback};
|
||||
pub use id::*;
|
||||
pub use m2006::C610;
|
||||
pub use m3508::C620;
|
||||
pub use turn::*;
|
||||
@@ -0,0 +1,18 @@
|
||||
[package]
|
||||
name = "motor"
|
||||
description = "Protocol-independent motor device contract and registry"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -0,0 +1,964 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! Protocol-independent motor device contracts.
|
||||
|
||||
pub mod traits {
|
||||
//! 能力面 trait：小而正交，按硬件事实拆分。
|
||||
//!
|
||||
//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）：
|
||||
//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`]
|
||||
//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]）
|
||||
//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`]
|
||||
//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Command, MotorState};
|
||||
|
||||
/// 读反馈。所有可观察设备的最小面。
|
||||
pub trait MotorFeedback: Send + Sync {
|
||||
/// 控制器内的电机 ID（对 DJI 电调即电调 ID）。
|
||||
fn id(&self) -> u8;
|
||||
|
||||
/// 最新反馈快照。
|
||||
fn state(&self) -> MotorState;
|
||||
|
||||
/// 距最近一帧反馈的时长；`None` 表示从未收到。
|
||||
///
|
||||
/// 用于在线判定（反馈新鲜度），与 [`MotorState::has_feedback`]
|
||||
/// （历史上是否收到过）区分：上电门控之后运行中掉线要看本方法。
|
||||
fn feedback_age(&self) -> Option<Duration>;
|
||||
}
|
||||
|
||||
/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。
|
||||
pub trait EffortDrive: Send + Sync {
|
||||
/// 直接下发开环 raw（协议量程，物理方向）。
|
||||
fn set_effort_raw(&self, raw: i16);
|
||||
}
|
||||
|
||||
/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。
|
||||
pub trait VelocityDrive: Send + Sync {
|
||||
/// 设定目标转子转速。
|
||||
fn set_velocity(&self, rotor_rpm: f32);
|
||||
}
|
||||
|
||||
/// 位置目标（转子多圈，圈；物理方向）。
|
||||
pub trait PositionDrive: Send + Sync {
|
||||
/// 设定目标转子多圈位置。
|
||||
fn set_position(&self, rotor_rev: f32);
|
||||
}
|
||||
|
||||
/// 停用（零输出）。
|
||||
pub trait Disable: Send + Sync {
|
||||
/// 停用电机。
|
||||
fn disable(&self);
|
||||
}
|
||||
|
||||
/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。
|
||||
pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
|
||||
impl<T> Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
|
||||
/// One motor's data for one control-policy evaluation.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct PolicySample {
|
||||
/// ESC identifier.
|
||||
pub id: u8,
|
||||
/// Latest direction-corrected physical feedback.
|
||||
pub state: MotorState,
|
||||
/// Latest command lease value.
|
||||
pub command: Command,
|
||||
/// Whether fresh feedback is available.
|
||||
pub online: bool,
|
||||
}
|
||||
|
||||
/// Snapshot passed to an external effort policy once per bus tick.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct PolicyInput {
|
||||
/// Samples indexed by ESC id minus one.
|
||||
pub samples: [PolicySample; 8],
|
||||
/// Measured control period in seconds.
|
||||
pub dt: f32,
|
||||
}
|
||||
|
||||
/// Computes raw physical effort from motor commands and feedback.
|
||||
///
|
||||
/// `esc-bus` owns transport and safety but deliberately has no control-law
|
||||
/// dependency. Control crates implement this trait and the application composes
|
||||
/// the two sides.
|
||||
pub trait EffortPolicy: Send + 'static {
|
||||
/// Return physical effort raw values indexed by ESC id minus one.
|
||||
fn step(&mut self, input: &PolicyInput) -> [i16; 8];
|
||||
/// Clear controller state after an offline event or command-mode change.
|
||||
fn reset(&mut self, esc_id: u8);
|
||||
}
|
||||
}
|
||||
|
||||
pub mod state {
|
||||
//! 电机反馈快照（协议无关，方向修正后的物理量）。
|
||||
//!
|
||||
//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`]
|
||||
//! 或上层完成，不在此泄漏底盘语义。
|
||||
|
||||
/// 电机反馈快照。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Default)]
|
||||
pub struct MotorState {
|
||||
/// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。
|
||||
pub rotor_angle_raw: u16,
|
||||
/// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。
|
||||
pub rotor_revolutions: f32,
|
||||
/// 转子转速（rpm，物理方向）。
|
||||
pub rotor_speed_rpm: f32,
|
||||
/// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。
|
||||
pub effort_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
/// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。
|
||||
pub feedback_count: u64,
|
||||
}
|
||||
|
||||
impl MotorState {
|
||||
/// 是否已收到过反馈（上电后首帧门控）。
|
||||
#[must_use]
|
||||
pub fn has_feedback(&self) -> bool {
|
||||
self.feedback_count > 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod command {
|
||||
//! 驱动层命令寄存器（最后写入胜出）。
|
||||
|
||||
/// 发给驱动层的电机命令。
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub enum Command {
|
||||
/// 停用：目标清零，下发 0 输出。
|
||||
Disabled,
|
||||
/// 直接下发开环 raw（协议量程，物理方向），绕过闭环。
|
||||
/// ESC = 电流 raw；GM6020 = 电压 raw。
|
||||
EffortRaw(i16),
|
||||
/// 速度环跟踪目标转子转速（rpm，物理方向）。
|
||||
Velocity(f32),
|
||||
/// 位置环跟踪目标转子多圈位置（圈，物理方向）。
|
||||
Position(f32),
|
||||
}
|
||||
|
||||
/// 命令模式标签（不含设定值）——用于切换时复位控制器。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum CommandMode {
|
||||
/// 停用。
|
||||
Disabled,
|
||||
/// 开环 raw。
|
||||
EffortRaw,
|
||||
/// 速度环。
|
||||
Velocity,
|
||||
/// 位置环。
|
||||
Position,
|
||||
}
|
||||
|
||||
impl Command {
|
||||
/// 模式标签。
|
||||
#[must_use]
|
||||
pub const fn mode(self) -> CommandMode {
|
||||
match self {
|
||||
Self::Disabled => CommandMode::Disabled,
|
||||
Self::EffortRaw(_) => CommandMode::EffortRaw,
|
||||
Self::Velocity(_) => CommandMode::Velocity,
|
||||
Self::Position(_) => CommandMode::Position,
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod gearbox {
|
||||
//! 输出轴单位适配器：减速比换算。
|
||||
//!
|
||||
//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
|
||||
//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
|
||||
|
||||
/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
|
||||
pub struct OutputShaft<T> {
|
||||
inner: T,
|
||||
gear_ratio: f32,
|
||||
}
|
||||
|
||||
impl<T> OutputShaft<T> {
|
||||
/// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。
|
||||
///
|
||||
/// # Panics
|
||||
///
|
||||
/// `gear_ratio` 非有限或 ≤ 0 时 panic。
|
||||
#[must_use]
|
||||
pub fn new(inner: T, gear_ratio: f32) -> Self {
|
||||
assert!(
|
||||
gear_ratio.is_finite() && gear_ratio > 0.0,
|
||||
"gear_ratio must be > 0, got {gear_ratio}"
|
||||
);
|
||||
Self { inner, gear_ratio }
|
||||
}
|
||||
|
||||
/// 减速比。
|
||||
#[must_use]
|
||||
pub fn gear_ratio(&self) -> f32 {
|
||||
self.gear_ratio
|
||||
}
|
||||
|
||||
/// 底层电机引用。
|
||||
#[must_use]
|
||||
pub fn inner(&self) -> &T {
|
||||
&self.inner
|
||||
}
|
||||
|
||||
/// 转子量 → 输出轴量。
|
||||
#[must_use]
|
||||
pub fn to_output(&self, rotor: f32) -> f32 {
|
||||
rotor / self.gear_ratio
|
||||
}
|
||||
|
||||
/// 输出轴量 → 转子量。
|
||||
#[must_use]
|
||||
pub fn to_rotor(&self, output: f32) -> f32 {
|
||||
output * self.gear_ratio
|
||||
}
|
||||
|
||||
/// 由反馈快照推算输出轴转速（rpm）。
|
||||
#[must_use]
|
||||
pub fn output_speed_rpm(&self, state: &MotorState) -> f32 {
|
||||
state.rotor_speed_rpm / self.gear_ratio
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
|
||||
fn id(&self) -> u8 {
|
||||
self.inner.id()
|
||||
}
|
||||
|
||||
fn state(&self) -> MotorState {
|
||||
// 只透传转子量；不伪造输出轴字段。
|
||||
self.inner.state()
|
||||
}
|
||||
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
self.inner.feedback_age()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
/// `output_rpm` 为**输出轴**转速。
|
||||
fn set_velocity(&self, output_rpm: f32) {
|
||||
self.inner.set_velocity(self.to_rotor(output_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: EffortDrive> EffortDrive for OutputShaft<T> {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
self.inner.set_effort_raw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: PositionDrive> PositionDrive for OutputShaft<T> {
|
||||
/// `output_rev` 为**输出轴**圈数。
|
||||
fn set_position(&self, output_rev: f32) {
|
||||
self.inner.set_position(self.to_rotor(output_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: Disable> Disable for OutputShaft<T> {
|
||||
fn disable(&self) {
|
||||
self.inner.disable();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::Command;
|
||||
|
||||
struct Stub {
|
||||
cmd: std::sync::Mutex<Command>,
|
||||
}
|
||||
|
||||
impl Stub {
|
||||
fn new() -> Self {
|
||||
Self { cmd: std::sync::Mutex::new(Command::Disabled) }
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for Stub {
|
||||
fn id(&self) -> u8 {
|
||||
1
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
MotorState::default()
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
Some(Duration::ZERO)
|
||||
}
|
||||
}
|
||||
impl VelocityDrive for Stub {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
impl EffortDrive for Stub {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
*self.cmd.lock().unwrap() = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
impl PositionDrive for Stub {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
*self.cmd.lock().unwrap() = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
impl Disable for Stub {
|
||||
fn disable(&self) {
|
||||
*self.cmd.lock().unwrap() = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn velocity_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 19.203);
|
||||
shaft.set_velocity(100.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn position_scales_by_gear() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 36.0);
|
||||
shaft.set_position(2.0);
|
||||
match *shaft.inner().cmd.lock().unwrap() {
|
||||
Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"),
|
||||
ref c => panic!("unexpected {c:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn effort_passes_through() {
|
||||
let shaft = OutputShaft::new(Stub::new(), 10.0);
|
||||
shaft.set_effort_raw(-1234);
|
||||
assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234));
|
||||
}
|
||||
|
||||
#[test]
|
||||
#[should_panic = "gear_ratio"]
|
||||
fn rejects_nonpositive_gear() {
|
||||
let _ = OutputShaft::new(Stub::new(), 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod heading {
|
||||
//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。
|
||||
//!
|
||||
//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。
|
||||
//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。
|
||||
|
||||
use std::future::Future;
|
||||
|
||||
/// 绝对航向观测源。
|
||||
pub trait HeadingSource: Send {
|
||||
/// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send;
|
||||
}
|
||||
|
||||
/// 云台目标角的参考系。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum HeadingFrame {
|
||||
/// 电机转子多圈（上电相对）。
|
||||
RotorMultiTurn,
|
||||
/// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。
|
||||
AbsoluteField,
|
||||
}
|
||||
}
|
||||
|
||||
pub mod registry {
|
||||
//! 电机注册表：**按总线 + 反馈帧 ID** 找回设备（ARCHITECTURE.md §5.4）。
|
||||
//!
|
||||
//! # 为什么需要它
|
||||
//!
|
||||
//! 单总线底盘上，`EscBus` 自己就能把反馈帧分发给对应的 `EscMotor`。但整机是
|
||||
//! **双 CAN**（§2.1：底盘 `can0` + 功能总线 `can1`），云台 / 拨弹的帧与底盘的
|
||||
//! 帧可能落在**相同的 ID** 上（GM6020 与 C620 都可能是 0x2xx）。此时"这帧是谁的"
|
||||
//! 只能由 `(总线, 帧 ID)` 共同决定。
|
||||
//!
|
||||
//! [`MotorRegistry`] 就是这张映射表：注册时校验冲突，分发时 O(1) 命中。
|
||||
//! 当前 `chassisd` 的单总线底盘路径不经过它；它是云台 / 拨弹接入功能总线时的
|
||||
//! seam（先有接口与测试，避免将来把分发逻辑硬编码进每个任务）。
|
||||
|
||||
use can::CanFrame;
|
||||
use motor_protocol::feedback_frame_id;
|
||||
|
||||
/// 物理总线标识（ARCHITECTURE.md §2.1 的双 CAN 拓扑）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
|
||||
pub enum BusId {
|
||||
/// 底盘总线（C620/C610 驱动轮）。
|
||||
Chassis,
|
||||
/// 功能总线（云台 GM6020 + 拨弹 M2006/C610）。
|
||||
Function,
|
||||
}
|
||||
|
||||
impl BusId {
|
||||
/// 文本名。
|
||||
#[must_use]
|
||||
pub const fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::Chassis => "chassis",
|
||||
Self::Function => "function",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 注册表条目。
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub struct RegisteredMotor {
|
||||
/// 所属总线。
|
||||
pub bus: BusId,
|
||||
/// 反馈帧 ID。
|
||||
pub frame_id: u32,
|
||||
/// 轮名 / 设备名（配置里的 `name`）。
|
||||
pub name: String,
|
||||
}
|
||||
|
||||
/// 注册错误。
|
||||
///
|
||||
/// 手写 `Display` / `Error` 而不是 `derive(thiserror::Error)`：注册表属于
|
||||
/// **总是可用**的设备抽象（关掉 `bus` feature 也要能编译），不应为一个派生宏
|
||||
/// 引入可选依赖。
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub enum RegistryError {
|
||||
/// 同一 `(总线, 帧 ID)` 被注册两次——几乎总是接线或配置写重了。
|
||||
Duplicate {
|
||||
/// 总线。
|
||||
bus: &'static str,
|
||||
/// 冲突的帧 ID。
|
||||
frame_id: u32,
|
||||
/// 先注册的名字。
|
||||
existing: String,
|
||||
/// 后注册的名字。
|
||||
name: String,
|
||||
},
|
||||
/// 电调 ID 越界（1–8）。
|
||||
BadEscId(u8),
|
||||
}
|
||||
|
||||
impl std::fmt::Display for RegistryError {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
match self {
|
||||
Self::Duplicate { bus, frame_id, existing, name } => write!(
|
||||
f,
|
||||
"总线 {bus} 上的帧 ID 0x{frame_id:03X} 已被 '{existing}' 占用，无法再注册 '{name}'"
|
||||
),
|
||||
Self::BadEscId(id) => write!(f, "电调 ID {id} 越界（合法范围 1–8）"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl std::error::Error for RegistryError {}
|
||||
|
||||
/// 设备注册表。
|
||||
#[derive(Debug, Default, Clone)]
|
||||
pub struct MotorRegistry {
|
||||
entries: Vec<RegisteredMotor>,
|
||||
}
|
||||
|
||||
impl MotorRegistry {
|
||||
/// 空表。
|
||||
#[must_use]
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
/// 按电调 ID 注册（反馈帧 ID = `0x200 + 电调ID`，C620/C610 约定）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 电调 ID 越界或该帧 ID 已被占用时返回错误。
|
||||
pub fn register_esc(
|
||||
&mut self,
|
||||
bus: BusId,
|
||||
esc_id: u8,
|
||||
name: impl Into<String>,
|
||||
) -> Result<(), RegistryError> {
|
||||
let frame_id = feedback_frame_id(esc_id).ok_or(RegistryError::BadEscId(esc_id))?;
|
||||
self.register_frame(bus, frame_id, name)
|
||||
}
|
||||
|
||||
/// 按反馈帧 ID 注册（GM6020 或非 1–8 的 ID 用它）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 该 `(总线, 帧 ID)` 已被占用时返回错误。
|
||||
pub fn register_frame(
|
||||
&mut self,
|
||||
bus: BusId,
|
||||
frame_id: u32,
|
||||
name: impl Into<String>,
|
||||
) -> Result<(), RegistryError> {
|
||||
let name = name.into();
|
||||
if let Some(existing) = self.resolve(bus, frame_id) {
|
||||
return Err(RegistryError::Duplicate {
|
||||
bus: bus.name(),
|
||||
frame_id,
|
||||
existing: existing.name.clone(),
|
||||
name,
|
||||
});
|
||||
}
|
||||
self.entries.push(RegisteredMotor { bus, frame_id, name });
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 按 `(总线, 帧 ID)` 查找。
|
||||
#[must_use]
|
||||
pub fn resolve(&self, bus: BusId, frame_id: u32) -> Option<&RegisteredMotor> {
|
||||
self.entries.iter().find(|entry| entry.bus == bus && entry.frame_id == frame_id)
|
||||
}
|
||||
|
||||
/// 从一帧反馈找出它的归属设备（ARCHITECTURE.md §5.4 的 `update_from_frame`）。
|
||||
///
|
||||
/// 返回 `None` 表示这帧不属于任何已注册设备（例如 IMU 帧，或没配的电机）。
|
||||
#[must_use]
|
||||
pub fn dispatch(&self, bus: BusId, frame: &CanFrame) -> Option<&RegisteredMotor> {
|
||||
self.resolve(bus, frame.id)
|
||||
}
|
||||
|
||||
/// 已注册设备数。
|
||||
#[must_use]
|
||||
pub fn len(&self) -> usize {
|
||||
self.entries.len()
|
||||
}
|
||||
|
||||
/// 是否为空。
|
||||
#[must_use]
|
||||
pub fn is_empty(&self) -> bool {
|
||||
self.entries.is_empty()
|
||||
}
|
||||
|
||||
/// 全部条目（按注册顺序）。
|
||||
#[must_use]
|
||||
pub fn entries(&self) -> &[RegisteredMotor] {
|
||||
&self.entries
|
||||
}
|
||||
|
||||
/// 某条总线上的设备名。
|
||||
#[must_use]
|
||||
pub fn names_on(&self, bus: BusId) -> Vec<&str> {
|
||||
self.entries
|
||||
.iter()
|
||||
.filter(|entry| entry.bus == bus)
|
||||
.map(|entry| entry.name.as_str())
|
||||
.collect()
|
||||
}
|
||||
}
|
||||
|
||||
/// 底盘四轮的注册顺序（与 `mecanum` 的 FL/FR/RL/RR 一致）。
|
||||
pub const CHASSIS_WHEEL_NAMES: [&str; 4] =
|
||||
["front_left", "front_right", "rear_left", "rear_right"];
|
||||
|
||||
/// 把底盘四轮按顺序注册进表（电调 ID 从 1 开始连续）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 任一电调 ID 越界或冲突时返回错误。
|
||||
pub fn register_chassis_wheels(
|
||||
registry: &mut MotorRegistry,
|
||||
esc_ids: [u8; 4],
|
||||
) -> Result<(), RegistryError> {
|
||||
for (name, esc_id) in CHASSIS_WHEEL_NAMES.iter().zip(esc_ids) {
|
||||
registry.register_esc(BusId::Chassis, esc_id, *name)?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_protocol::{MAX_ESC_ID, MIN_ESC_ID};
|
||||
|
||||
#[test]
|
||||
fn registers_and_dispatches_chassis_wheels() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap();
|
||||
assert_eq!(registry.len(), 4);
|
||||
assert_eq!(registry.names_on(BusId::Chassis).len(), 4);
|
||||
assert!(registry.names_on(BusId::Function).is_empty());
|
||||
|
||||
let frame = CanFrame::new(0x203, [0; 8]);
|
||||
let found = registry.dispatch(BusId::Chassis, &frame).unwrap();
|
||||
assert_eq!(found.name, "rear_left", "0x203 = 电调 3 = 后左");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn same_frame_id_on_another_bus_is_not_a_conflict() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
registry.register_esc(BusId::Chassis, 1, "front_left").unwrap();
|
||||
// 功能总线上的 GM6020 ID 1 反馈同样是 0x205 —— 不同总线，互不冲突。
|
||||
registry.register_frame(BusId::Function, 0x205, "gimbal_yaw").unwrap();
|
||||
assert_eq!(registry.len(), 2);
|
||||
assert_eq!(registry.resolve(BusId::Chassis, 0x205), None);
|
||||
assert_eq!(registry.resolve(BusId::Function, 0x205).unwrap().name, "gimbal_yaw");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn duplicates_on_the_same_bus_are_rejected_with_both_names() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
registry.register_esc(BusId::Chassis, 2, "front_right").unwrap();
|
||||
let error = registry.register_esc(BusId::Chassis, 2, "typo_front_right").unwrap_err();
|
||||
let text = error.to_string();
|
||||
assert!(text.contains("front_right"), "{text}");
|
||||
assert!(text.contains("typo_front_right"), "{text}");
|
||||
assert_eq!(registry.len(), 1, "失败的注册不得写入");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn out_of_range_esc_ids_are_rejected() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
assert_eq!(
|
||||
registry.register_esc(BusId::Chassis, 0, "ghost"),
|
||||
Err(RegistryError::BadEscId(0))
|
||||
);
|
||||
assert_eq!(
|
||||
registry.register_esc(BusId::Chassis, 9, "ghost"),
|
||||
Err(RegistryError::BadEscId(9))
|
||||
);
|
||||
assert!(registry.is_empty());
|
||||
assert!(feedback_frame_id(MIN_ESC_ID).is_some());
|
||||
assert!(feedback_frame_id(MAX_ESC_ID).is_some());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn unknown_frames_dispatch_to_nothing() {
|
||||
let mut registry = MotorRegistry::new();
|
||||
register_chassis_wheels(&mut registry, [1, 2, 3, 4]).unwrap();
|
||||
// DETA10 的 IMU 帧不是任何电机。
|
||||
let imu = CanFrame::new(0x100, [0; 8]);
|
||||
assert!(registry.dispatch(BusId::Chassis, &imu).is_none());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod testing {
|
||||
//! 测试替身：内存电机与航向源。
|
||||
|
||||
use std::future::Future;
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use crate::{
|
||||
Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive,
|
||||
VelocityDrive,
|
||||
};
|
||||
|
||||
/// 锁辅助：中毒锁直接恢复内部数据。
|
||||
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
|
||||
/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。
|
||||
///
|
||||
/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。
|
||||
#[derive(Clone)]
|
||||
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))),
|
||||
last_feedback: Arc::new(Mutex::new(None)),
|
||||
}
|
||||
}
|
||||
|
||||
/// 注入反馈快照（记为「刚刚收到」）。
|
||||
pub fn inject_state(&self, state: MotorState) {
|
||||
self.inject_state_with_age(state, Duration::ZERO);
|
||||
}
|
||||
|
||||
/// 注入反馈快照并指定其“已过去”的时长（用于在线/超时测试）。
|
||||
pub fn inject_state_with_age(&self, state: MotorState, age: Duration) {
|
||||
lock(&self.published).0 = state;
|
||||
let at = Instant::now().checked_sub(age);
|
||||
*lock(&self.last_feedback) = Some(at.unwrap_or_else(Instant::now));
|
||||
}
|
||||
|
||||
/// 最近一次命令。
|
||||
#[must_use]
|
||||
pub fn last_command(&self) -> Command {
|
||||
lock(&self.published).1
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorFeedback for MockMotor {
|
||||
fn id(&self) -> u8 {
|
||||
self.id
|
||||
}
|
||||
fn state(&self) -> MotorState {
|
||||
lock(&self.published).0
|
||||
}
|
||||
fn feedback_age(&self) -> Option<Duration> {
|
||||
let at = *lock(&self.last_feedback);
|
||||
Some(at?.elapsed())
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for MockMotor {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
lock(&self.published).1 = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
|
||||
impl EffortDrive for MockMotor {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
lock(&self.published).1 = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionDrive for MockMotor {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
lock(&self.published).1 = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
|
||||
impl Disable for MockMotor {
|
||||
fn disable(&self) {
|
||||
lock(&self.published).1 = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
/// 恒定航向测试源。
|
||||
pub struct MockHeading {
|
||||
rad: f32,
|
||||
}
|
||||
|
||||
impl MockHeading {
|
||||
/// 创建固定航向源。
|
||||
#[must_use]
|
||||
pub const fn new(rad: f32) -> Self {
|
||||
Self { rad }
|
||||
}
|
||||
}
|
||||
|
||||
impl HeadingSource for MockHeading {
|
||||
fn heading_rad(&self) -> impl Future<Output = f32> + Send {
|
||||
std::future::ready(self.rad)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn mock_motor_records_commands() {
|
||||
let m = MockMotor::new(3);
|
||||
m.set_velocity(1000.0);
|
||||
assert_eq!(m.last_command(), Command::Velocity(1000.0));
|
||||
m.disable();
|
||||
assert_eq!(m.last_command(), Command::Disabled);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_injects_state() {
|
||||
let m = MockMotor::new(1);
|
||||
assert!(!m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_none());
|
||||
m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() });
|
||||
assert!(m.state().has_feedback());
|
||||
assert!(m.feedback_age().is_some_and(|age| age < Duration::from_millis(50)));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn mock_motor_feedback_age_can_be_stale() {
|
||||
let m = MockMotor::new(1);
|
||||
m.inject_state_with_age(
|
||||
MotorState { feedback_count: 1, ..MotorState::default() },
|
||||
Duration::from_millis(250),
|
||||
);
|
||||
assert!(m.feedback_age().is_some_and(|age| age >= Duration::from_millis(200)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod motor_common {
|
||||
//! 电机种类：把"型号"变成可在设备层与配置层之间传递的事实。
|
||||
//!
|
||||
//! 设备层不关心电调怎么接、齿轮多大，但必须知道**协议族**：电流环在电调内部，
|
||||
//! 而不同协议族的控制字含义不同（电流 vs 电压）。[`MotorKind`] 就是这张表。
|
||||
|
||||
use motor_protocol::EscModel;
|
||||
|
||||
/// 支持的电机型号。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
|
||||
pub enum MotorKind {
|
||||
/// GM3508 + C620（底盘驱动轮）。
|
||||
Gm3508,
|
||||
/// M2006 + C610（拨弹 / 小负载）。
|
||||
M2006,
|
||||
/// GM6020（云台直驱，**电压**控制）。
|
||||
Gm6020,
|
||||
}
|
||||
|
||||
impl MotorKind {
|
||||
/// 配套电调型号。
|
||||
#[must_use]
|
||||
pub const fn esc_model(self) -> EscModel {
|
||||
match self {
|
||||
// GM6020 的反馈标度与 C620 相同，但控制字是电压。
|
||||
Self::Gm3508 | Self::Gm6020 => EscModel::C620,
|
||||
Self::M2006 => EscModel::C610,
|
||||
}
|
||||
}
|
||||
|
||||
/// 控制量是否是**电压**（而非电流）。
|
||||
///
|
||||
/// 总线驱动据此选择组帧方式：C620/C610 走 `encode_current_frame`，
|
||||
/// GM6020 走 `motor_protocol::motor::gm6020::encode_voltage_frame`。
|
||||
#[must_use]
|
||||
pub const fn encodes_voltage(self) -> bool {
|
||||
matches!(self, Self::Gm6020)
|
||||
}
|
||||
|
||||
/// 该型号是否与其它型号**共用**电流控制帧（C620/C610 共用 `0x200`/`0x1FF`）。
|
||||
#[must_use]
|
||||
pub const fn shares_current_frames(self) -> bool {
|
||||
!self.encodes_voltage()
|
||||
}
|
||||
|
||||
/// 默认减速比（转子 : 输出轴）。接硬件前必须按实车核对。
|
||||
#[must_use]
|
||||
pub const fn default_gear_ratio(self) -> f32 {
|
||||
match self {
|
||||
Self::Gm3508 => 3591.0 / 187.0,
|
||||
// M2006 常见配置 36:1；与 ARCHITECTURE.md §8.2 的 motors.toml 一致。
|
||||
Self::M2006 => 36.0,
|
||||
// 云台直驱。
|
||||
Self::Gm6020 => 1.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 型号名（配置与日志用）。
|
||||
#[must_use]
|
||||
pub const fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::Gm3508 => "GM3508",
|
||||
Self::M2006 => "M2006",
|
||||
Self::Gm6020 => "GM6020",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn only_gm6020_encodes_voltage() {
|
||||
assert!(MotorKind::Gm6020.encodes_voltage());
|
||||
assert!(!MotorKind::Gm3508.encodes_voltage());
|
||||
assert!(!MotorKind::M2006.encodes_voltage());
|
||||
assert!(MotorKind::Gm3508.shares_current_frames());
|
||||
assert!(!MotorKind::Gm6020.shares_current_frames());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn esc_models_match_the_scale_documentation() {
|
||||
assert_eq!(MotorKind::Gm3508.esc_model(), EscModel::C620);
|
||||
assert_eq!(MotorKind::Gm6020.esc_model(), EscModel::C620);
|
||||
assert_eq!(MotorKind::M2006.esc_model(), EscModel::C610);
|
||||
assert_eq!(MotorKind::M2006.esc_model().max_current_raw(), 10_000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default_gear_ratios_are_sane() {
|
||||
assert!((MotorKind::Gm3508.default_gear_ratio() - 19.203).abs() < 5e-3);
|
||||
assert!((MotorKind::Gm6020.default_gear_ratio() - 1.0).abs() < 1e-6);
|
||||
assert!(MotorKind::M2006.default_gear_ratio() > 0.0);
|
||||
assert_eq!(MotorKind::Gm6020.name(), "GM6020");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub mod motor_gm6020 {
|
||||
//! GM6020 设备（云台直驱，**电压**控制）。
|
||||
//!
|
||||
//! # 当前状态（务必读）
|
||||
//!
|
||||
//! 协议层的电压帧编解码已经可用（[`motor_protocol::gm6020`]），但**总线驱动
|
||||
//! `EscBus` 目前只实现 C620/C610 的电流路径**：它把 `set_effort_raw` 直接组
|
||||
//! 成电流帧。把 GM6020 接上云台还需要总线驱动增加"电压帧"分支
|
||||
//! （[`MotorKind::encodes_voltage`] 就是给那条分支准备的判据），这属于云台
|
||||
//! 部分的工作（ARCHITECTURE.md §5.7 `gimbal` 任务）。
|
||||
//!
|
||||
//! 在此之前，本模块提供的是**型号事实**（协议族、减速比、ID 映射），
|
||||
//! 不含"已经能驱动 GM6020"的暗示。
|
||||
|
||||
use crate::MotorKind;
|
||||
|
||||
/// 该型号的 [`MotorKind`]。
|
||||
pub const KIND: MotorKind = MotorKind::Gm6020;
|
||||
|
||||
/// 云台直驱的减速比。
|
||||
pub const GEAR_RATIO: f32 = 1.0;
|
||||
|
||||
/// 该型号的控制量是电压而非电流。
|
||||
pub const ENCODES_VOLTAGE: bool = true;
|
||||
|
||||
/// 总线驱动是否已支持该型号的驱动路径。
|
||||
///
|
||||
/// 现在返回 `false`：电压帧分支随云台工作落地。用它做**运行期**判定，
|
||||
/// 而不是让配置静默接受一个驱动不了的型号。
|
||||
#[must_use]
|
||||
pub const fn drive_path_available() -> bool {
|
||||
false
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use motor_protocol::gm6020::{COMMAND_FRAME_ID_PRIMARY, feedback_frame_id};
|
||||
|
||||
#[test]
|
||||
fn kind_facts_match_the_protocol_module() {
|
||||
assert_eq!(KIND, MotorKind::Gm6020);
|
||||
assert!(KIND.encodes_voltage());
|
||||
assert!((KIND.default_gear_ratio() - GEAR_RATIO).abs() < 1e-6);
|
||||
// 两个常量同源：`ENCODES_VOLTAGE` 就是 `KIND.encodes_voltage()` 的镜像。
|
||||
assert_eq!(ENCODES_VOLTAGE, KIND.encodes_voltage());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn gm6020_uses_its_own_frame_ids() {
|
||||
assert_eq!(COMMAND_FRAME_ID_PRIMARY, 0x1FF);
|
||||
assert_eq!(feedback_frame_id(1), Some(0x205));
|
||||
// 与 C620 不同：C620 的 ID 1 控制帧是 0x200。
|
||||
assert_ne!(COMMAND_FRAME_ID_PRIMARY, motor_protocol::COMMAND_FRAME_ID_PRIMARY);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn drive_path_is_honestly_reported_as_unavailable() {
|
||||
assert!(!drive_path_available(), "云台电压路径落地后本断言才应改为 true");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub use command::*;
|
||||
pub use gearbox::*;
|
||||
pub use heading::*;
|
||||
pub use motor_common::MotorKind;
|
||||
pub use registry::*;
|
||||
pub use state::*;
|
||||
pub use testing::*;
|
||||
pub use traits::*;
|
||||
@@ -1,12 +1,16 @@
|
||||
[package]
|
||||
name = "motor-core"
|
||||
name = "pid"
|
||||
description = "PID controller"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "Protocol-agnostic motor abstraction: Motor trait, feedback state, and PID controllers"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
@@ -1,3 +1,5 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 通用 PID 控制器。
|
||||
|
||||
/// 并联式 PID 控制器（f32，显式时间步）。
|
||||
@@ -102,6 +104,12 @@ impl Pid {
|
||||
self.last_output = 0.0;
|
||||
}
|
||||
|
||||
/// 上一次 `update` 的输出（诊断 / 串级外环取速度目标用）。
|
||||
#[must_use]
|
||||
pub fn last_output(&self) -> f32 {
|
||||
self.last_output
|
||||
}
|
||||
|
||||
/// 当前积分累加项。
|
||||
#[must_use]
|
||||
pub fn integral(&self) -> f32 {
|
||||
@@ -1,6 +1,6 @@
|
||||
[package]
|
||||
name = "dji-esc-sim"
|
||||
description = "C620/C610 plant simulation: friction/delay model, scenario harness, and async plant rig"
|
||||
name = "sim-harness"
|
||||
description = "Closed-loop motor simulation harness and fake ESC rig"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
@@ -10,9 +10,12 @@ license.workspace = true
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
can = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
motor-model = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
@@ -20,3 +23,4 @@ rand = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
approx = { workspace = true }
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,20 +1,21 @@
|
||||
[package]
|
||||
name = "dji-esc"
|
||||
name = "slcan"
|
||||
description = "SLCAN serial transport bridge"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "DJI RoboMaster ESC (C620/C610) CAN protocol: codec, multi-turn tracking, and async bus driver"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
motor-core = { workspace = true }
|
||||
can-link = { workspace = true }
|
||||
can = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
slcan-proto = { workspace = true }
|
||||
serial = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
tokio = { workspace = true }
|
||||
@@ -1,8 +1,8 @@
|
||||
//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can_link::CanLink`]。
|
||||
//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can::CanLink`]。
|
||||
//!
|
||||
//! 基于 [`slcan`] crate（Lawicel ASCII 协议）：`S8` = 1 Mbps（C620 标准），`O` 打开通道。
|
||||
//! 阻塞串口 I/O 跑在专用线程上，与 SocketCAN 桥一样只暴露通道端点——对本 crate
|
||||
//! 的调用方而言，`dji-esc` 看到的是一个普通 [`CanLink`]，与传输介质无关。
|
||||
//! 的调用方（`motor / esc-bus`）而言，看到的是一个普通 [`CanLink`]，与传输介质无关。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
use std::io;
|
||||
@@ -11,9 +11,10 @@ use std::sync::atomic::{AtomicU64, Ordering};
|
||||
use std::thread;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
use can_link::{CanFrame as LinkFrame, CanLink};
|
||||
use can::{CanFrame as LinkFrame, CanLink};
|
||||
use serial::prelude::*;
|
||||
use slcan::{BitRate, CanSocket};
|
||||
use slcan_proto as slcan;
|
||||
use tokio::sync::mpsc;
|
||||
use tracing::{debug, error, info, warn};
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
[package]
|
||||
name = "socketcan"
|
||||
description = "SocketCAN transport bridge"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
esc-bus = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
socketcan-rs = { workspace = true }
|
||||
@@ -0,0 +1,122 @@
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
//! 真实总线桥接实现（Linux）。
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
mod imp {
|
||||
use std::io;
|
||||
use std::sync::Arc;
|
||||
|
||||
use can::{CanFrame as LinkFrame, CanLink};
|
||||
use socketcan_rs::{
|
||||
CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _,
|
||||
SocketOptions as _, StandardId, tokio::CanSocket,
|
||||
};
|
||||
use tokio::sync::mpsc;
|
||||
use tracing::{debug, error};
|
||||
|
||||
/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。
|
||||
const CHANNEL_BOUND: usize = 512;
|
||||
|
||||
/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。
|
||||
///
|
||||
/// 内部：
|
||||
/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208，
|
||||
/// 非反馈帧再由协议层解码拒绝，双保险）；
|
||||
/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 权限不足等 `io::Error`。
|
||||
pub fn open_link(interface: &str) -> io::Result<CanLink> {
|
||||
let socket = CanSocket::open(interface)?;
|
||||
socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?;
|
||||
let socket = Arc::new(socket);
|
||||
|
||||
let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
|
||||
let CanLink { outgoing_tx, incoming_rx } = bridge_side;
|
||||
|
||||
tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx));
|
||||
tokio::spawn(tx_bridge(socket, incoming_rx));
|
||||
Ok(bus_side)
|
||||
}
|
||||
|
||||
/// socket → 通道：真实总线反馈进入协议 bus。
|
||||
async fn rx_bridge(socket: Arc<CanSocket>, tx: mpsc::Sender<LinkFrame>) {
|
||||
loop {
|
||||
match socket.read_frame().await {
|
||||
Ok(frame) => {
|
||||
if let Some(frame) = to_link_frame(&frame) {
|
||||
if tx.send(frame).await.is_err() {
|
||||
debug!("bus side closed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(e) => {
|
||||
// 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程，
|
||||
// 由 systemd 拉起重连。
|
||||
error!(error = %e, "CAN read failed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 通道 → socket：协议 bus 命令写入真实总线。
|
||||
async fn tx_bridge(socket: Arc<CanSocket>, mut rx: mpsc::Receiver<LinkFrame>) {
|
||||
while let Some(frame) = rx.recv().await {
|
||||
match to_socketcan_frame(&frame) {
|
||||
Some(sc_frame) => {
|
||||
if let Err(e) = socket.write_frame(sc_frame).await {
|
||||
error!(error = %e, "CAN write failed, tx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
None => {
|
||||
debug!(id = frame.id, "drop frame with non-standard CAN id");
|
||||
}
|
||||
}
|
||||
}
|
||||
debug!("bus side closed, tx bridge exiting");
|
||||
}
|
||||
|
||||
/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。
|
||||
fn to_link_frame(frame: &ScFrame) -> Option<LinkFrame> {
|
||||
match frame {
|
||||
ScFrame::Data(df) if df.data().len() == 8 => {
|
||||
Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?))
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。
|
||||
fn to_socketcan_frame(frame: &LinkFrame) -> Option<ScFrame> {
|
||||
let id = StandardId::new(frame.id as u16)?;
|
||||
let df = CanDataFrame::new(id, &frame.data)?;
|
||||
Some(ScFrame::Data(df))
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
pub use imp::*;
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Unsupported;
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
impl std::fmt::Display for Unsupported {
|
||||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||||
f.write_str("SocketCAN is only available on Linux")
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
impl std::error::Error for Unsupported {}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub fn open_link(_interface: &str) -> Result<can::CanLink, Unsupported> {
|
||||
Err(Unsupported)
|
||||
}
|
||||
@@ -7,10 +7,11 @@
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use dji_esc::{CanFrame, CanLink};
|
||||
use motor_core::Motor;
|
||||
use socketcan::tokio::CanSocket;
|
||||
use socketcan::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId};
|
||||
use can::{CanFrame, CanLink};
|
||||
use motor::{MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use socketcan_rs::tokio::CanSocket;
|
||||
use socketcan_rs::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId};
|
||||
use tokio::time::timeout;
|
||||
|
||||
const IFACE: &str = "vcan0";
|
||||
@@ -22,7 +23,7 @@ fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame {
|
||||
#[tokio::test]
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn bridge_relays_command_to_bus() {
|
||||
let link = dji_esc_socketcan::open_link(IFACE).unwrap();
|
||||
let link = socketcan::open_link(IFACE).unwrap();
|
||||
let observer = CanSocket::open(IFACE).unwrap();
|
||||
|
||||
let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
|
||||
@@ -44,7 +45,7 @@ async fn bridge_relays_command_to_bus() {
|
||||
#[tokio::test]
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn bridge_relays_feedback_from_bus() {
|
||||
let CanLink { outgoing_tx: _, mut incoming_rx } = dji_esc_socketcan::open_link(IFACE).unwrap();
|
||||
let CanLink { outgoing_tx: _, mut incoming_rx } = socketcan::open_link(IFACE).unwrap();
|
||||
let simulator = CanSocket::open(IFACE).unwrap();
|
||||
|
||||
// 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。
|
||||
@@ -63,15 +64,14 @@ async fn bridge_relays_feedback_from_bus() {
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn esc_bus_end_to_end_over_vcan() {
|
||||
// EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。
|
||||
let link = dji_esc_socketcan::open_link(IFACE).unwrap();
|
||||
let link = socketcan::open_link(IFACE).unwrap();
|
||||
let mut esc = EscSim::new(IFACE, &[1]);
|
||||
|
||||
let bus = dji_esc::EscBus::spawn(
|
||||
link,
|
||||
vec![dji_esc::EscConfig::gm3508("m1", 1, 1.0)],
|
||||
dji_esc::BusOptions::default(),
|
||||
)
|
||||
.unwrap();
|
||||
let config = esc_bus::EscConfig::gm3508("m1", 1, 1.0);
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(1, config.current_limit, VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0));
|
||||
let bus =
|
||||
esc_bus::EscBus::spawn(link, vec![config], esc_bus::BusOptions::default(), policy).unwrap();
|
||||
bus.motor(1).unwrap().set_velocity(2000.0);
|
||||
|
||||
// 仿真 2s：模拟电调按命令回反馈，速度环应收敛。
|
||||
@@ -0,0 +1,27 @@
|
||||
[package]
|
||||
name = "spin"
|
||||
description = "Chassis and single-motor hardware bench tool"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
bench = { workspace = true }
|
||||
motor = { workspace = true }
|
||||
esc-bus = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
mecanum = { workspace = true }
|
||||
slcan = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
clap = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
tracing-subscriber = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,722 @@
|
||||
mod chassis {
|
||||
//! 实车台架工具：让**四个轮子同时**以固定**转子**转速旋转（默认 500 rpm）。
|
||||
//!
|
||||
//! ```sh
|
||||
//! # T153 板载 CANable2（slcan 固件）→ /dev/ttyACM0，四轮 500 rpm 转子，缓升 1s 保持 10s
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- \
|
||||
//! --device /dev/ttyACM0 --rpm 500
|
||||
//!
|
||||
//! # 只验证链路与反馈，不发任何非零电流（接线排查用）
|
||||
//! cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
//! ```
|
||||
//!
|
||||
//! # 抽象边界
|
||||
//!
|
||||
//! 本工具是**组合根**：`dji-esc-slcan`（链路桥接）→ `dji-esc`（C620 协议 +
|
||||
//! 速度环）→ `motor-core`（`VelocityDrive` / `MotorFeedback` / `Disable` 能力
|
||||
//! trait）。激励场景与安全包络来自同 crate 的 `chassis-bench` 库，与
|
||||
//! `chassis-tune` 的真机后端**共用同一套实现**。
|
||||
//!
|
||||
//! # 命令租约（重要）
|
||||
//!
|
||||
//! `EscBus` 的命令是**租约**：`command_timeout`（默认 50ms）内没有重新下发命令，
|
||||
//! 驱动会强制 0 电流。所以控制循环必须**逐拍重发**同一个设定值——本工具在 1kHz
|
||||
//! 节拍里重发（与 `chassisd` 的控制循环一致）。只在开头下发一次是**不行的**。
|
||||
//!
|
||||
//! # 安全
|
||||
//!
|
||||
//! 轮子必须架空或整车固定，急停可及。默认低限流、有限时长、ctrl-c 任意时刻
|
||||
//! 安全停机（先 0 电流再关总线）；未收到全部反馈前绝不下发速度。
|
||||
|
||||
use std::process::ExitCode;
|
||||
use std::time::Duration;
|
||||
|
||||
use bench::{SafetyEnvelope, SafetyMonitor};
|
||||
use clap::{Parser, ValueEnum};
|
||||
use esc_bus::{BusOptions, EscBus, EscBusHandle, EscConfig, EscMotor};
|
||||
use experiment::Scenario;
|
||||
use motor::{Disable, MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use slcan::{CanSpeed, SlcanOptions, open_link_with};
|
||||
use tokio::time::{Instant, MissedTickBehavior, interval, sleep};
|
||||
|
||||
/// 稳态判据：平均转子转速与目标的相对误差上限。
|
||||
const TOLERANCE: f32 = 0.05;
|
||||
/// 状态打印周期。
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(200);
|
||||
/// 反馈新鲜度阈值：`feedback_age` 超过它即视为该电调掉线。
|
||||
const FRESH: Duration = Duration::from_millis(100);
|
||||
/// 过速需要持续这么久才判定为失控并停机（避免单帧噪声误触发）。
|
||||
const OVERSPEED_GRACE: Duration = Duration::from_millis(60);
|
||||
/// 控制节拍（C620 期望 1kHz 命令）。
|
||||
const CONTROL_PERIOD: Duration = Duration::from_millis(1);
|
||||
/// 四轮默认名（顺序固定 FL、FR、RL、RR）。
|
||||
const WHEEL_NAMES: [&str; 4] = ["front_left", "front_right", "rear_left", "rear_right"];
|
||||
|
||||
#[derive(Debug, Parser)]
|
||||
#[command(name = "chassis-spin", version, about = "台架：四轮同时旋转（默认 500 rpm 转子）")]
|
||||
struct Args {
|
||||
/// slcan 串口设备（T153 板载 CANable2 为 /dev/ttyACM0）。
|
||||
#[arg(long, default_value = "/dev/ttyACM0")]
|
||||
device: String,
|
||||
|
||||
/// 串口波特率（CDC ACM 一般忽略；CH340 类适配器需与固件一致）。
|
||||
#[arg(long, default_value_t = 2_000_000)]
|
||||
serial_baud: u32,
|
||||
|
||||
/// 串口超时（毫秒）。读写共用：`serial` 的 `TTYPort::write` 也按它 poll(POLLOUT)。
|
||||
#[arg(long, default_value_t = 50)]
|
||||
serial_timeout_ms: u64,
|
||||
|
||||
/// CAN 波特率档位（C620 标准 = 1Mbps）。
|
||||
#[arg(long, value_enum, default_value_t = CanSpeedArg::M1)]
|
||||
can_speed: CanSpeedArg,
|
||||
|
||||
/// 电调 ID 列表，顺序 = FL、FR、RL、RR。
|
||||
#[arg(long, value_delimiter = ',', default_value = "1,2,3,4")]
|
||||
esc_ids: Vec<u8>,
|
||||
|
||||
/// 各轮安装方向 ±1（默认左右镜像 → 四轮物理同向）。
|
||||
#[arg(
|
||||
long,
|
||||
value_delimiter = ',',
|
||||
allow_hyphen_values = true,
|
||||
default_value = "1,-1,1,-1"
|
||||
)]
|
||||
directions: Vec<f32>,
|
||||
|
||||
/// 目标**转子**转速（rpm）；轮端 = 转子 ÷ 减速比。
|
||||
#[arg(long, default_value_t = 500.0)]
|
||||
rpm: f32,
|
||||
|
||||
/// 减速比（转子 : 输出轴，GM3508 ≈ 19.203）。
|
||||
#[arg(long, default_value_t = 19.203)]
|
||||
gear_ratio: f32,
|
||||
|
||||
/// 电流限幅（raw，C620 满量程 16384）。
|
||||
///
|
||||
/// 默认 1500 raw（≈1.8A）：要越过静摩擦（实测约 400~600 raw）且留出整定余量。
|
||||
/// 上限同时也是 PID 抗饱和的钳位点——**必须贴近实际所需电流**，
|
||||
/// 否则积分会在堵转期间积累，一旦起动就冲过头。
|
||||
#[arg(long, default_value_t = 1500)]
|
||||
current_limit: i16,
|
||||
|
||||
/// 缓升时间：0 → 目标转速（毫秒）。
|
||||
///
|
||||
/// 默认 3000：空载 GM3508 的静摩擦需要约 400~600 raw 才能起动（stick-slip），
|
||||
/// 缓升让积分在起动前平滑建立，避免"先憋住、一动就飞"。
|
||||
#[arg(long, default_value_t = 3000)]
|
||||
ramp_ms: u64,
|
||||
|
||||
/// 目标转速保持时长（秒）。
|
||||
#[arg(long, default_value_t = 10.0)]
|
||||
hold_secs: f32,
|
||||
|
||||
/// 等待全部电调反馈的超时（毫秒）。
|
||||
#[arg(long, default_value_t = 3000)]
|
||||
startup_timeout_ms: u64,
|
||||
|
||||
/// 只验证链路与反馈，不发出任何非零电流。
|
||||
#[arg(long)]
|
||||
dry_run: bool,
|
||||
|
||||
/// 允许缺失部分电调：仅驱动有反馈的电调继续测试（默认要求四个电调全部在线）。
|
||||
#[arg(long)]
|
||||
allow_partial: bool,
|
||||
|
||||
/// 速度环 kp（raw/rpm）。
|
||||
///
|
||||
/// 空载台架默认 2.0：被控对象近似积分器（≈12 rpm/s per raw），
|
||||
/// 高 kp（如 8~18）会因反馈延迟震荡；低 kp 配合大 ki 才稳。
|
||||
#[arg(long, default_value_t = 2.0)]
|
||||
kp: f32,
|
||||
/// 速度环 ki（raw/(rpm·s)）。
|
||||
///
|
||||
/// 空载台架默认 10：静摩擦需要积分项缓慢"顶"到起动电流，ki 太小会长期
|
||||
/// 停在 300~400 rpm 达不到目标；抗饱和由 `current_limit` 钳位兜底，
|
||||
/// 所以大 ki 不会跑飞。
|
||||
#[arg(long, default_value_t = 10.0)]
|
||||
ki: f32,
|
||||
/// 速度环 kd。
|
||||
#[arg(long, default_value_t = 0.0)]
|
||||
kd: f32,
|
||||
/// 速度前馈（raw/rpm）。
|
||||
///
|
||||
/// **空载台架默认 0**：轮子架空时维持转速只需要克服摩擦的极小电流，
|
||||
/// 任何接近 1/K（GM3508 空载 K ≈ 0.57 rpm/raw → 1/K ≈ 1.77）的前馈都会把
|
||||
/// 空载轮子直接推到最高转速。只有带载工况才需要正前馈。
|
||||
#[arg(long, default_value_t = 0.0)]
|
||||
feedforward: f32,
|
||||
|
||||
/// 过速保护上限（rpm）：任一参与测试的电调超过它即立即停机。
|
||||
/// 默认 = 目标 × 2 + 200。
|
||||
#[arg(long)]
|
||||
max_rpm: Option<f32>,
|
||||
}
|
||||
|
||||
/// CAN 波特率档位（对应 slcan 的 `Sn` 命令）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)]
|
||||
enum CanSpeedArg {
|
||||
/// `S8`：1 Mbps。
|
||||
#[value(name = "1m")]
|
||||
M1,
|
||||
/// `S6`：500 kbps。
|
||||
#[value(name = "500k")]
|
||||
K500,
|
||||
}
|
||||
|
||||
impl From<CanSpeedArg> for CanSpeed {
|
||||
fn from(value: CanSpeedArg) -> Self {
|
||||
match value {
|
||||
CanSpeedArg::M1 => Self::M1,
|
||||
CanSpeedArg::K500 => Self::K500,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Args {
|
||||
/// 过速保护上限：显式 `--max-rpm` 优先，否则目标 × 2 + 200。
|
||||
fn overspeed_limit(&self) -> f32 {
|
||||
self.max_rpm.unwrap_or(self.rpm * 2.0 + 200.0)
|
||||
}
|
||||
|
||||
/// 参数自检：越界参数在碰硬件之前就拒绝。
|
||||
fn validate(&self) -> Result<(), String> {
|
||||
if self.esc_ids.is_empty() {
|
||||
return Err("--esc-ids 不能为空".into());
|
||||
}
|
||||
if self.esc_ids.len() != self.directions.len() {
|
||||
return Err(format!(
|
||||
"--esc-ids（{} 个）与 --directions（{} 个）数量必须一致",
|
||||
self.esc_ids.len(),
|
||||
self.directions.len()
|
||||
));
|
||||
}
|
||||
for &id in &self.esc_ids {
|
||||
if !(1..=8).contains(&id) {
|
||||
return Err(format!("电调 ID {id} 超出 1..=8"));
|
||||
}
|
||||
}
|
||||
for &direction in &self.directions {
|
||||
let bits = direction.to_bits();
|
||||
if bits != 1.0f32.to_bits() && bits != (-1.0f32).to_bits() {
|
||||
return Err(format!("方向必须是 +1 或 -1，得到 {direction}"));
|
||||
}
|
||||
}
|
||||
if !(self.rpm.is_finite() && self.rpm >= 0.0) {
|
||||
return Err(format!("--rpm 必须 ≥ 0，得到 {}", self.rpm));
|
||||
}
|
||||
if let Some(max) = self.max_rpm
|
||||
&& !(max.is_finite() && max > self.rpm)
|
||||
{
|
||||
return Err(format!("--max-rpm 必须大于目标转速 {}，得到 {max}", self.rpm));
|
||||
}
|
||||
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
|
||||
return Err(format!("--gear-ratio 必须 > 0，得到 {}", self.gear_ratio));
|
||||
}
|
||||
if !(1..=16384).contains(&self.current_limit) {
|
||||
return Err(format!(
|
||||
"--current-limit 必须在 1..=16384，得到 {}",
|
||||
self.current_limit
|
||||
));
|
||||
}
|
||||
if self.hold_secs < 0.0 {
|
||||
return Err(format!("--hold-secs 不能为负，得到 {}", self.hold_secs));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// 四个电调配置（方向、减速比、电流限幅、PID 全部显式落地）。
|
||||
fn esc_configs(&self) -> Vec<EscConfig> {
|
||||
self.esc_ids
|
||||
.iter()
|
||||
.enumerate()
|
||||
.map(|(index, &id)| {
|
||||
let name = WHEEL_NAMES
|
||||
.get(index)
|
||||
.map_or_else(|| format!("wheel{id}"), |name| (*name).to_owned());
|
||||
let mut cfg = EscConfig::gm3508(name, id, self.directions[index]);
|
||||
cfg.gear_ratio = self.gear_ratio;
|
||||
cfg.current_limit = self.current_limit;
|
||||
cfg
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
fn control_policy(&self) -> MotorPolicy {
|
||||
let mut policy = MotorPolicy::new();
|
||||
let gains = VelocityGains::new(self.kp, self.ki, self.kd, self.feedforward);
|
||||
for id in &self.esc_ids {
|
||||
policy.configure(*id, self.current_limit, gains);
|
||||
}
|
||||
policy
|
||||
}
|
||||
}
|
||||
|
||||
fn bus_options() -> BusOptions {
|
||||
BusOptions {
|
||||
period: CONTROL_PERIOD,
|
||||
feedback_timeout: Duration::from_millis(100),
|
||||
// 命令租约 50ms；控制循环每 1ms 续租。
|
||||
command_timeout: Duration::from_millis(50),
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn main_with_args(parameters: Vec<String>) -> ExitCode {
|
||||
init_tracing();
|
||||
let args =
|
||||
Args::parse_from(std::iter::once(String::from("spin-chassis")).chain(parameters));
|
||||
if let Err(problem) = args.validate() {
|
||||
eprintln!("✗ 参数错误：{problem}");
|
||||
return ExitCode::FAILURE;
|
||||
}
|
||||
print_banner(&args);
|
||||
|
||||
match run(&args).await {
|
||||
Ok(true) => ExitCode::SUCCESS,
|
||||
Ok(false) => ExitCode::FAILURE,
|
||||
Err(e) => {
|
||||
eprintln!("✗ 运行失败：{e}");
|
||||
ExitCode::FAILURE
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 初始化日志：链路桥的读写失败/静默告警必须可见，
|
||||
/// 否则现场只剩一句"没有反馈"，无法区分适配器故障与整车接线问题。
|
||||
fn init_tracing() {
|
||||
let filter = tracing_subscriber::EnvFilter::try_from_default_env().unwrap_or_else(|_| {
|
||||
tracing_subscriber::EnvFilter::new("warn,slcan=debug,socketcan=debug")
|
||||
});
|
||||
let _ = tracing_subscriber::fmt().with_env_filter(filter).try_init();
|
||||
}
|
||||
|
||||
fn print_banner(args: &Args) {
|
||||
println!("=== 台架：四轮同时旋转（转子目标 {:.1} rpm）===", args.rpm);
|
||||
println!(
|
||||
"设备 {} | CAN {:?} | 电调 {:?} | 方向 {:?} | 减速比 {:.3}",
|
||||
args.device, args.can_speed, args.esc_ids, args.directions, args.gear_ratio
|
||||
);
|
||||
println!(
|
||||
"转子 {:.1} rpm ÷ {:.3} = 轮端 {:.2} rpm | 电流限幅 {} raw | 缓升 {}ms | 保持 {:.1}s{}",
|
||||
args.rpm,
|
||||
args.gear_ratio,
|
||||
args.rpm / args.gear_ratio,
|
||||
args.current_limit,
|
||||
args.ramp_ms,
|
||||
args.hold_secs,
|
||||
if args.dry_run {
|
||||
" | dry-run"
|
||||
} else if args.allow_partial {
|
||||
" | allow-partial"
|
||||
} else {
|
||||
""
|
||||
}
|
||||
);
|
||||
if args.dry_run {
|
||||
println!("ℹ dry-run：只读反馈，不发出非零电流。\n");
|
||||
} else {
|
||||
println!(
|
||||
"速度环 kp={} ki={} kd={} ff={} | 过速保护 {:.0} rpm",
|
||||
args.kp,
|
||||
args.ki,
|
||||
args.kd,
|
||||
args.feedforward,
|
||||
args.overspeed_limit()
|
||||
);
|
||||
println!("⚠ 轮子必须架空或整车固定，急停可及；ctrl-c 随时安全停机。\n");
|
||||
}
|
||||
}
|
||||
|
||||
/// 打开链路 → 装配协议 bus → 执行 → **任何路径都先停电流再关总线**。
|
||||
async fn run(args: &Args) -> Result<bool, Box<dyn std::error::Error>> {
|
||||
let options = SlcanOptions {
|
||||
serial_baud: args.serial_baud,
|
||||
speed: args.can_speed.into(),
|
||||
// 读写共用该超时（serial crate 的 TTYPort::write 也先 poll(POLLOUT)）：
|
||||
// 设太小会把"这一拍写不出去"误判成写失败。总线繁忙时读立即返回，
|
||||
// 放大它只影响总线完全静默时的下行延迟。
|
||||
read_timeout: Duration::from_millis(args.serial_timeout_ms),
|
||||
};
|
||||
let link = open_link_with(&args.device, options).map_err(|e| {
|
||||
format!("打开 slcan 设备 {} 失败：{e}（检查设备路径 / 权限 / 是否被占用）", args.device)
|
||||
})?;
|
||||
println!("✓ slcan 已打开：{}（CAN {:?}）", args.device, args.can_speed);
|
||||
|
||||
let bus = EscBus::spawn(link, args.esc_configs(), bus_options(), args.control_policy())?;
|
||||
let outcome = drive(args, &bus).await;
|
||||
|
||||
// 安全收尾：先 0 电流并让它至少发出一拍，再关总线。
|
||||
for motor in bus.motors() {
|
||||
motor.disable();
|
||||
}
|
||||
sleep(Duration::from_millis(50)).await;
|
||||
bus.shutdown().await;
|
||||
println!("已停机（0 电流）并关闭总线");
|
||||
outcome
|
||||
}
|
||||
|
||||
async fn drive(
|
||||
args: &Args,
|
||||
bus: &EscBusHandle<MotorPolicy>,
|
||||
) -> Result<bool, Box<dyn std::error::Error>> {
|
||||
let motors = bus.motors();
|
||||
wait_for_feedback(motors, Duration::from_millis(args.startup_timeout_ms)).await;
|
||||
|
||||
let targets: Vec<EscMotor> = motors.iter().filter(|m| is_fresh(m)).cloned().collect();
|
||||
let missing: Vec<u8> =
|
||||
motors.iter().filter(|m| !is_fresh(m)).map(MotorFeedback::id).collect();
|
||||
|
||||
if !missing.is_empty() {
|
||||
eprintln!(
|
||||
"✗ 以下电调没有新鲜反馈：{missing:?}（收到 {:?}）",
|
||||
motors.iter().map(MotorFeedback::feedback_age).collect::<Vec<_>>()
|
||||
);
|
||||
eprintln!(
|
||||
" 排查：CAN 波特率（C620 = 1Mbps = S8）| CANH/CANL 是否接反 | 120Ω 终端电阻 | 电调供电 | 电调 CAN ID 拨码 | 设备 {}",
|
||||
args.device
|
||||
);
|
||||
if !args.allow_partial {
|
||||
eprintln!(" 若确认台架只接了部分电调，可加 --allow-partial 降级测试。");
|
||||
return Ok(false);
|
||||
}
|
||||
eprintln!("⚠ --allow-partial：仅驱动有反馈的电调继续测试。\n");
|
||||
}
|
||||
if targets.is_empty() {
|
||||
eprintln!("✗ 没有任何电调在反馈，无法测试。");
|
||||
return Ok(false);
|
||||
}
|
||||
println!(
|
||||
"✓ 参与测试的电调：{:?}（新鲜度 < {FRESH:?}）\n",
|
||||
targets.iter().map(MotorFeedback::id).collect::<Vec<_>>()
|
||||
);
|
||||
|
||||
if args.dry_run {
|
||||
return Ok(dry_run(&targets).await);
|
||||
}
|
||||
Ok(spin(args, &targets).await)
|
||||
}
|
||||
|
||||
/// 该电调是否有**新鲜**反馈（而非历史上有过反馈）。
|
||||
fn is_fresh(motor: &EscMotor) -> bool {
|
||||
motor.feedback_age().is_some_and(|age| age <= FRESH)
|
||||
}
|
||||
|
||||
/// 上电门控：等到每个电调都有一帧**新鲜**反馈；超时即返回 `false`。
|
||||
async fn wait_for_feedback(motors: &[EscMotor], timeout: Duration) -> bool {
|
||||
let deadline = Instant::now() + timeout;
|
||||
loop {
|
||||
if motors.iter().all(is_fresh) {
|
||||
return true;
|
||||
}
|
||||
if Instant::now() >= deadline {
|
||||
return false;
|
||||
}
|
||||
sleep(Duration::from_millis(20)).await;
|
||||
}
|
||||
}
|
||||
|
||||
/// dry-run：只观察反馈，不下发任何非零电流（总线仍在发 0 电流帧）。
|
||||
async fn dry_run(targets: &[EscMotor]) -> bool {
|
||||
let mut ticker = interval(REPORT_PERIOD);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = Instant::now() + Duration::from_secs(3);
|
||||
while Instant::now() < deadline {
|
||||
ticker.tick().await;
|
||||
for motor in targets {
|
||||
let st = motor.state();
|
||||
println!(
|
||||
" {:<11} esc{} | 转子 {:>7.1} rpm | 温度 {:>3}℃ | 反馈 {:>6} 帧 | 新鲜度 {:?}",
|
||||
motor.name(),
|
||||
motor.id(),
|
||||
st.rotor_speed_rpm,
|
||||
st.temperature_c,
|
||||
st.feedback_count,
|
||||
motor.feedback_age().map(|age| age.as_millis())
|
||||
);
|
||||
}
|
||||
println!();
|
||||
}
|
||||
println!("✓ 链路与反馈验证完成（未发出非零电流）");
|
||||
true
|
||||
}
|
||||
|
||||
/// 本次台架的安全包络：只开过速保护（`chassis-spin` 的历史行为）。
|
||||
fn envelope(args: &Args) -> SafetyEnvelope {
|
||||
SafetyEnvelope::overspeed_only(args.overspeed_limit(), OVERSPEED_GRACE)
|
||||
}
|
||||
|
||||
/// 缓升 → 保持 → 采样；返回稳态是否达标。
|
||||
async fn spin(args: &Args, motors: &[EscMotor]) -> bool {
|
||||
let ramp = Duration::from_millis(args.ramp_ms);
|
||||
let hold = Duration::from_secs_f32(args.hold_secs);
|
||||
let overspeed_limit = args.overspeed_limit();
|
||||
// 设定值曲线与安全判定都来自 chassis-bench 库：与 chassis-tune 的真机
|
||||
// 后端共用同一套实现，避免"台架测的行为"和"整定优化的行为"分叉。
|
||||
let scenario = Scenario::ramp_and_hold(args.rpm, ramp.as_secs_f32(), hold.as_secs_f32());
|
||||
let mut ticker = interval(CONTROL_PERIOD);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
let start = Instant::now();
|
||||
let mut monitor = SafetyMonitor::new(envelope(args), start);
|
||||
let mut next_report = REPORT_PERIOD;
|
||||
let mut samples: Vec<Vec<f32>> = vec![Vec::new(); motors.len()];
|
||||
let mut overspeed_warned = false;
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = tokio::signal::ctrl_c() => {
|
||||
println!("\n收到 ctrl-c，中断测试");
|
||||
return false;
|
||||
}
|
||||
_ = ticker.tick() => {}
|
||||
}
|
||||
let elapsed = start.elapsed();
|
||||
if elapsed >= ramp + hold {
|
||||
break;
|
||||
}
|
||||
// 逐拍重发（= 命令租约续期），斜坡段目标线性上升。
|
||||
let setpoint = scenario.setpoint_at(elapsed.as_secs_f32());
|
||||
for motor in motors {
|
||||
motor.set_velocity(setpoint);
|
||||
}
|
||||
// 过速保护：控制器失稳时立刻停机，别让轮子飞到最高转速。
|
||||
let peak =
|
||||
motors.iter().map(|m| m.state().rotor_speed_rpm.abs()).fold(0.0f32, f32::max);
|
||||
if peak > overspeed_limit && !overspeed_warned {
|
||||
overspeed_warned = true;
|
||||
eprintln!("⚠ 过速 {peak:.0} rpm > 上限 {overspeed_limit:.0} rpm，观察中……");
|
||||
}
|
||||
if let Some(abort) = monitor.check(motors, Instant::now()) {
|
||||
eprintln!(
|
||||
"✗ 过速保护触发：{peak:.0} rpm 超过上限 {overspeed_limit:.0} rpm，立即停机（{abort}）。\n 速度环整定不当（kp/ki/前馈过大）——空载台架前馈应为 0。"
|
||||
);
|
||||
return false;
|
||||
}
|
||||
if elapsed >= ramp {
|
||||
for (slot, motor) in samples.iter_mut().zip(motors) {
|
||||
slot.push(motor.state().rotor_speed_rpm);
|
||||
}
|
||||
}
|
||||
if elapsed >= next_report {
|
||||
report_line(motors, setpoint, elapsed);
|
||||
next_report += REPORT_PERIOD;
|
||||
}
|
||||
}
|
||||
summarize(args, motors, &samples)
|
||||
}
|
||||
|
||||
fn report_line(motors: &[EscMotor], setpoint: f32, elapsed: Duration) {
|
||||
let rotor: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>7.1}", m.state().rotor_speed_rpm)).collect();
|
||||
let current: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>6}", m.state().effort_raw)).collect();
|
||||
let temp: Vec<String> =
|
||||
motors.iter().map(|m| format!("{:>3}", m.state().temperature_c)).collect();
|
||||
println!(
|
||||
"[t={:>5.1}s] 目标 {:>7.1} | 转子 {} | 电流 {} | 温度 {}",
|
||||
elapsed.as_secs_f32(),
|
||||
setpoint,
|
||||
rotor.join(" "),
|
||||
current.join(" "),
|
||||
temp.join(" ")
|
||||
);
|
||||
}
|
||||
|
||||
fn summarize(args: &Args, motors: &[EscMotor], samples: &[Vec<f32>]) -> bool {
|
||||
if samples.iter().all(Vec::is_empty) {
|
||||
println!("保持时长为 0，未采集到稳态样本");
|
||||
return true;
|
||||
}
|
||||
println!(
|
||||
"\n=== 稳态结论（目标转子 {:.1} rpm ≈ 轮端 {:.2} rpm）===",
|
||||
args.rpm,
|
||||
args.rpm / args.gear_ratio
|
||||
);
|
||||
let mut all_ok = true;
|
||||
for (index, motor) in motors.iter().enumerate() {
|
||||
let mean = mean(&samples[index]);
|
||||
let error = if args.rpm.abs() > f32::EPSILON {
|
||||
(mean - args.rpm).abs() / args.rpm.abs()
|
||||
} else {
|
||||
0.0
|
||||
};
|
||||
let ok = error <= TOLERANCE;
|
||||
all_ok &= ok;
|
||||
println!(
|
||||
" {:<11} esc{} | 稳态 {:>7.1} rpm | 轮端 {:>6.2} rpm | 误差 {:>5.1}% | 电流 {:>6} raw | 温度 {:>3}℃ | {}",
|
||||
motor.name(),
|
||||
motor.id(),
|
||||
mean,
|
||||
mean / args.gear_ratio,
|
||||
error * 100.0,
|
||||
motor.state().effort_raw,
|
||||
motor.state().temperature_c,
|
||||
if ok { "✓" } else { "✗" }
|
||||
);
|
||||
}
|
||||
if all_ok {
|
||||
println!("\n✓ 通过：{} 个轮子均达到目标转速 ±{:.0}%", motors.len(), TOLERANCE * 100.0);
|
||||
} else {
|
||||
println!(
|
||||
"\n✗ 未通过：存在轮速偏差 > {:.0}%，检查 PID / 机械阻力 / 电流限幅 / 方向配置",
|
||||
TOLERANCE * 100.0
|
||||
);
|
||||
}
|
||||
all_ok
|
||||
}
|
||||
|
||||
fn mean(samples: &[f32]) -> f32 {
|
||||
if samples.is_empty() {
|
||||
return 0.0;
|
||||
}
|
||||
samples.iter().sum::<f32>() / samples.len() as f32
|
||||
}
|
||||
}
|
||||
|
||||
mod motor_one {
|
||||
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||||
//!
|
||||
//! ```sh
|
||||
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||||
//! cargo run --release -p bench --bin socketcan-spin -- can0 1 1000 10
|
||||
//! ```
|
||||
//!
|
||||
//! 关键点在 `spin_one`：它只依赖 `motor::Motor` 抽象，完全不知道
|
||||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||||
//! 原样可用。抽象（`can / imu`）、协议实现（`motor-protocol`）、设备层（`motor / esc-bus`）与总线后端（`driver crates`）在此汇合。
|
||||
//!
|
||||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use std::time::Duration;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use esc_bus::{BusOptions, EscBus, EscConfig};
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor::Motor;
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
#[cfg(target_os = "linux")]
|
||||
use socketcan::open_link;
|
||||
#[cfg(target_os = "linux")]
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
pub async fn main_with_args(argv: Vec<String>) {
|
||||
let mut args = argv.into_iter();
|
||||
let interface = args.next().unwrap_or_else(|| "can0".into());
|
||||
let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
|
||||
let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
|
||||
let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
|
||||
|
||||
println!("=== 单电机抽象验证 ===");
|
||||
println!(
|
||||
"接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s"
|
||||
);
|
||||
|
||||
let link = open_link(&interface).unwrap_or_else(|e| {
|
||||
eprintln!(
|
||||
"打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）"
|
||||
);
|
||||
std::process::exit(1);
|
||||
});
|
||||
let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
|
||||
println!(
|
||||
"电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}",
|
||||
config.model, config.gear_ratio, config.current_limit
|
||||
);
|
||||
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(esc_id, config.current_limit, VelocityGains::new(10.0, 0.5, 0.0, 1.5));
|
||||
let bus = EscBus::spawn(link, vec![config], BusOptions::default(), policy)
|
||||
.expect("EscBus 配置非法");
|
||||
let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
|
||||
|
||||
spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
|
||||
|
||||
bus.shutdown().await;
|
||||
println!("已退出");
|
||||
}
|
||||
|
||||
/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
|
||||
/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
|
||||
///
|
||||
/// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发
|
||||
/// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值
|
||||
/// （真实底盘的控制循环每 1ms 重发一次）。
|
||||
#[cfg(target_os = "linux")]
|
||||
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||||
const LEASE_PERIOD: Duration = Duration::from_millis(10);
|
||||
const REPORT_PERIOD: Duration = Duration::from_millis(100);
|
||||
|
||||
motor.set_velocity(target_rpm);
|
||||
println!("已下发目标转速，开始观测（100ms 周期）……\n");
|
||||
|
||||
let mut lease = interval(LEASE_PERIOD);
|
||||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let mut report = interval(REPORT_PERIOD);
|
||||
report.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = tokio::time::Instant::now() + duration;
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = tokio::signal::ctrl_c() => {
|
||||
println!("\n收到 ctrl-c");
|
||||
break;
|
||||
}
|
||||
_ = lease.tick() => motor.set_velocity(target_rpm), // 续租
|
||||
_ = report.tick() => {
|
||||
let st = motor.state();
|
||||
if st.has_feedback() {
|
||||
println!(
|
||||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||||
st.rotor_speed_rpm,
|
||||
st.effort_raw,
|
||||
st.temperature_c,
|
||||
st.rotor_revolutions,
|
||||
st.feedback_count
|
||||
);
|
||||
} else {
|
||||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||||
}
|
||||
}
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
motor.disable();
|
||||
// 让 0 电流帧至少发出一拍再关总线。
|
||||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||||
println!("已停机（0 电流）");
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
#[allow(clippy::unused_async)]
|
||||
pub async fn main_with_args(_argv: Vec<String>) {
|
||||
eprintln!("spin motor 仅支持 Linux（需要 SocketCAN）");
|
||||
}
|
||||
}
|
||||
|
||||
#[tokio::main]
|
||||
async fn main() -> std::process::ExitCode {
|
||||
let mut args = std::env::args().skip(1);
|
||||
match args.next().as_deref() {
|
||||
Some("chassis") => chassis::main_with_args(args.collect()).await,
|
||||
Some("motor") => {
|
||||
motor_one::main_with_args(args.collect()).await;
|
||||
std::process::ExitCode::SUCCESS
|
||||
}
|
||||
Some("--help" | "-h") => {
|
||||
println!("usage: spin <chassis|motor> [options]");
|
||||
std::process::ExitCode::SUCCESS
|
||||
}
|
||||
_ => {
|
||||
eprintln!("usage: spin <chassis|motor> [options]");
|
||||
std::process::ExitCode::from(2)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
[package]
|
||||
name = "tuning"
|
||||
description = "PID tuning algorithms, search, cost, and presets"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
pid = { workspace = true }
|
||||
motor-control = { workspace = true }
|
||||
experiment = { workspace = true }
|
||||
sim-harness = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
toml = { workspace = true }
|
||||
rand = { workspace = true }
|
||||
rayon = { workspace = true }
|
||||
argmin = { workspace = true }
|
||||
argmin-math = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
indicatif = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
motor-protocol = { workspace = true }
|
||||
motor-model = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
approx = { workspace = true }
|
||||
proptest = { workspace = true }
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,19 +1,19 @@
|
||||
[Unit]
|
||||
Description=Sentry chassis daemon (mecanum, 4x GM3508/C620)
|
||||
After=network.target
|
||||
# 若使用 USB 转 CAN，等待接口就绪：
|
||||
# After=systemd-networkd.service
|
||||
# RequiresMountsFor=/sys/class/net/can0
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
ExecStart=/usr/local/bin/chassisd --interface can0 --config /etc/chassisd/chassis.toml
|
||||
Restart=always
|
||||
RestartSec=1
|
||||
# 实时性：提升调度优先级（按需调整）
|
||||
# CPUSchedulingPolicy=fifo
|
||||
# CPUSchedulingPriority=20
|
||||
# LimitRTPRIO=95
|
||||
|
||||
# 实时调度由 systemd 设置；应用不调用 libc FFI。
|
||||
CPUSchedulingPolicy=fifo
|
||||
CPUSchedulingPriority=80
|
||||
LimitRTPRIO=95
|
||||
# 仅提高锁定额度；std 程序不调用 mlockall。
|
||||
LimitMEMLOCK=infinity
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
|
||||
+4
-160
@@ -1,162 +1,6 @@
|
||||
# 架构设计
|
||||
# 架构文档入口
|
||||
|
||||
## 分层
|
||||
|
||||
```
|
||||
┌────────────────┐ ┌──────────────────────┐ ┌─────────────────────────┐
|
||||
│ chassisd │ │ chassis-tune │ │ chassis-bench │
|
||||
│ 守护进程 │ │ 自动整定（搜索/报告） │ │ 台架（激励/包络/采样） │
|
||||
│ 1kHz 循环 / 安全│ │ sim ↓ bench ↓ │ │ chassis-spin 等 │
|
||||
└───┬────────┬───┘ └────┬──────────┬──────┘ └────┬───────────────────┘
|
||||
│ │ │ │ │
|
||||
│ │ │ └───────┬───────┘
|
||||
▼ ▼ ▼ ▼
|
||||
┌───────────┐ ┌──────────────────┐ ┌───────────────────────┐
|
||||
│ mecanum │ │ dji-esc-sim │ │ dji-esc-slcan │
|
||||
│ 麦轮运动学 │ │ 被控对象 / 场景 │ │ dji-esc-socketcan │
|
||||
└───────────┘ │ 离线 harness │ │ 真实总线 ↔ CanLink │
|
||||
│ 异步假电调 │ └──────────┬────────────┘
|
||||
└────────┬─────────┘ │
|
||||
│ CanLink（tokio mpsc 通道对）
|
||||
▼ ▼
|
||||
┌────────────────────┐ ┌──────────────────┐
|
||||
│ motor-core │◄─│ dji-esc │
|
||||
│ 能力 trait / Pid │ │ C620/C610 协议 │
|
||||
│ VelocityGains │ │ EscBus 异步驱动 │
|
||||
└────────────────────┘ └──────────────────┘
|
||||
|
||||
┌────────────────────┐
|
||||
│ chassis-config │ chassis.toml schema（chassisd + chassis-tune 共用）
|
||||
└────────────────────┘
|
||||
```
|
||||
|
||||
核心解耦点：`CanLink` 是一对 tokio 通道（发送 / 接收 `CanFrame`）。`dji-esc` 的协议状态机
|
||||
只面向通道工作，因此：
|
||||
|
||||
- 闭环仿真由 `dji-esc-sim` 提供两条路径：**异步假电调**（挂在同一对通道上，`chassisd --mock`
|
||||
与集成测试用）与**同步 harness**（纯浮点积分、无任务无通道，自动整定用，可并行上千次）；
|
||||
- `dji-esc-socketcan`（板载原生 SocketCAN）与 `dji-esc-slcan`（CANable 等 USB 适配器，
|
||||
串口 Lawicel 协议）只把真实总线桥接到通道，**不含任何应用逻辑**；应用住在 `chassis-bench`
|
||||
（台架）与 `chassis-tune`（整定）；
|
||||
- 未来更换总线介质（串口转 CAN、MCU 转发）只需新增一个桥接 crate。
|
||||
|
||||
## 被控对象模型（dji-esc-sim）
|
||||
|
||||
逐拍离散（`dt` 秒），电调坐标系（正电流 → 正转速）：
|
||||
|
||||
1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才作用（链路 + 采样延迟）；
|
||||
2. **摩擦**：运动时 `net = u − sign(rpm)·coulomb − viscous·rpm`；静止且 `|u| > static` 时
|
||||
`net = u − sign(u)·coulomb`；静止且 `|u| ≤ static` 时 `net = 0`（粘住）；
|
||||
3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
|
||||
以时间常数 `tau` 一阶趋近；
|
||||
4. **量化**：机械角按转速积分并对 8192 取模，反馈帧是 `EscFeedback`，
|
||||
因此角度被截断为 `u16`、转速为 `i16`——量化由协议类型自带。
|
||||
|
||||
`PlantParams::gm3508()` 是**无摩擦无延迟**的理想模型（`chassisd --mock` 与历史闭环测试依赖它，
|
||||
行为逐位不变）；`PlantParams::gm3508_bench()` 叠加静 / 库仑摩擦（400~600 raw 量级，
|
||||
取自台架实测）与 2 拍延迟，是整定的默认被控对象。
|
||||
|
||||
单点模型整定出来的增益往往对增益 / 时间常数 / 延迟高度敏感，因此 `chassis-tune` 默认在
|
||||
`PlantEnsemble`（域随机化：增益 ±20%、时间常数 ±30%、延迟 0~3 拍、摩擦 ±50%）上做鲁棒优化，
|
||||
而不是针对一条理想曲线做最优化。
|
||||
|
||||
## 自动整定（chassis-tune）
|
||||
|
||||
两阶段搜索：
|
||||
|
||||
1. **粗扫**：Latin Hypercube 分层采样 + `rayon` 并行评估，用有限次数铺满四维空间；
|
||||
2. **精修**：以成本最低的 top-k 为起点，在**坐标空间**（对数刻度，增益恒正）做多起点
|
||||
`argmin` 局部搜索（Nelder-Mead 默认，可选 PSO）。
|
||||
|
||||
成本 = 归一化加权（调节时间 / 超调 / IAE / 电流）；**硬约束不进权重**：过速、饱和占比过高、
|
||||
不收敛直接给罚值并记录判定——否则搜索会主动挑一个会飞车的解。
|
||||
|
||||
可复现性：每个候选的随机性只来自 `(种子, 候选下标)` 派生的独立 RNG，粗扫结果排序后再取 top-k，
|
||||
因此同 seed 两次运行**逐位一致**（CI 的 `just tune-smoke` 依赖这一点）。
|
||||
|
||||
超参数（参数空间 / 场景 / 权重 / 验收阈值 / 搜索预算 / 被控对象 / 真机安全包络）不在命令行
|
||||
默认值里散着，而是版本化在 `config/tune.toml` 的具名预设中（见
|
||||
[`preset.rs`](../crates/chassis-tune/src/preset.rs)）。优先级为
|
||||
**内置默认 < 预设文件 < 显式 CLI**，逐字段覆盖；`current_limit` / `direction` / `gear_ratio`
|
||||
不是预设字段，写进预设文件会直接报错（安全参数不允许被预设绕过）。
|
||||
|
||||
真机后端（`--backend bench`）与仿真后端实现同一个 `Evaluator` trait，区别只是"评估一组增益"
|
||||
从浮点积分换成真机试验：打开链路 → 上电门控（反馈新鲜）→ 逐次热更新增益
|
||||
（`EscMotor::set_velocity_gains`，不重启总线）→ 逐拍重发设定值 + 安全包络 → 任何退出路径
|
||||
先 0 电流 → 试验间静默。搜索强制单线程，因为只有一条总线。
|
||||
|
||||
## 命令租约与链路深度（实车踩过的坑）
|
||||
|
||||
1. **命令是租约**：`EscBus` 在 `command_timeout`（默认 50ms）内没收到新命令就强制 0 输出
|
||||
（防上层任务死亡后旧指令持续输出）。所以 `set_velocity` 不是"设一次就完事"，上层必须
|
||||
逐拍重发；只在开头下发一次的台架脚本会在 50ms 后自动失电。
|
||||
2. **命令队列必须浅**：驱动按 1kHz 生产命令，而 slcan 一次 write 在 CDC ACM 上约 2ms，
|
||||
下行只有几百帧/秒。若 `CanLink` 命令方向缓冲很深（早期是 512），队列会被过时命令灌满，
|
||||
ESC 执行的是**一秒多以前**算出的电流——速度环等于开了 1.4s 延迟，轮子必然跑飞。
|
||||
现在 slcan 桥的 `CHANNEL_BOUND = 8`：`send().await` 立即回压，每一帧都按最新反馈现算。
|
||||
反馈方向是采样流，来不及就丢（`try_send`），绝不阻塞泵线程。
|
||||
3. **桥的两个方向不能互相饿死**：发送要有上界（`TX_BURST`）并在一帧写不动时立刻让位，
|
||||
接收要批量排空（`RX_BURST`）。`serial` crate 的 `TTYPort` 读写共用同一个超时，
|
||||
写超时属正常现象，不能当致命错误（否则一次写超时就会把接收方向一起带停）。
|
||||
|
||||
## DJI 电调 CAN 协议要点（C620 手册）
|
||||
|
||||
| 项目 | 值 |
|
||||
| --- | --- |
|
||||
| 控制帧 ID | `0x200`（电调 1–4）、`0x1FF`（电调 5–8） |
|
||||
| 控制帧格式 | 8 字节 = 4 × i16 大端电流命令，按电调 ID 顺序 |
|
||||
| C620 电流标度 | ±16384 ↔ ±20 A |
|
||||
| C610 电流标度 | ±10000 ↔ ±10 A（M2006 组合，接硬件前需核对） |
|
||||
| 反馈帧 ID | `0x200 + 电调ID`（`0x201`–`0x208`） |
|
||||
| 反馈帧格式 | u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃，均大端 |
|
||||
| 反馈频率 | 每电调 1kHz |
|
||||
|
||||
C620 内部**只闭电流环**：转速 / 位置环由主控侧 PID 实现（`motor-core::Pid`，
|
||||
挂在 `EscBus` 的 1kHz 发送节拍上）。GM3508 减速比 3591 / 187 ≈ 19.203。
|
||||
|
||||
## 单位与方向约定
|
||||
|
||||
- 机器人坐标系：ISO 约定——x 前、y 左、ω 逆时针为正（见 `mecanum`）。
|
||||
- `Motor` 接口呈现的是**物理量**（安装方向修正后）：
|
||||
`EscConfig::direction = -1` 时，驱动层在写命令与读反馈时统一翻转符号，
|
||||
上层永远以"轮子正转 = 底盘前进方向"的正向工作。
|
||||
- `MotorState::rotor_speed_rpm` 为电机转子转速；轮端转速 = 转子转速 / 减速比。
|
||||
- 电流一律以协议原始值（raw）为主，安培换算通过 `EscModel` 常量提供。
|
||||
|
||||
## 安全设计
|
||||
|
||||
1. **上电门控**：底盘循环在收到任一电机首帧反馈前不下发速度命令。
|
||||
2. **反馈离线**：任一电机反馈超时（默认 100ms）→ 该电机命令强制 0 电流并告警。
|
||||
3. **命令超时**：上层 twist 命令超时（默认 200ms）→ 目标速度归零。
|
||||
4. 所有超时可经 `chassis.toml` 配置。
|
||||
|
||||
## 测试策略
|
||||
|
||||
| 层级 | 内容 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试、slcan 帧形状过滤 | 全平台 |
|
||||
| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、**命令租约到期与续租**、**增益热更新**、守护进程全链路 | 全平台 |
|
||||
| 模型一致性 | 同步 harness 与 `EscBus` 异步闭环在同一组增益下的稳态 / effort 对齐（防止"对着错模型调参"） | 全平台 |
|
||||
| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） |
|
||||
| 超参数预设 | schema 往返、逐字段覆盖与优先级、未知键 / 越界 / 缺失报错、`config/tune.toml` 全部预设可解算、命名与后端一致、安全参数不可注入 | 全平台 |
|
||||
| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） |
|
||||
| 实车台架 | `chassis-spin`（`chassis-bench`）四轮定速旋转：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 目标板 + slcan/SocketCAN（人工） |
|
||||
| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check | CI |
|
||||
|
||||
## 部署
|
||||
|
||||
`chassisd` 为单二进制守护进程：`--interface can0 --config /etc/chassisd/chassis.toml`。
|
||||
systemd 样例见 `deploy/chassisd.service`。板端 CAN 两条路径：
|
||||
|
||||
- **SocketCAN**：USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`；
|
||||
- **slcan**：USB 适配器（CANable2 等）在 CDC ACM 下枚举为 `/dev/ttyACM0`，由
|
||||
[`dji-esc-slcan`](../crates/dji-esc-slcan) 在用户态处理（不需 `ip link`，也绕开了
|
||||
gs_usb 驱动）。台架工具 `chassis-spin`（[`chassis-bench`](../crates/chassis-bench)）与
|
||||
自动整定的真机后端走这条路径。整定工具本身是宿主机 / 板端通用的纯 Rust 程序，
|
||||
`current_limit` 等安全参数由 CLI 显式给出，不参与搜索。
|
||||
|
||||
## 相关文档
|
||||
|
||||
- [底盘开发文档](chassis.md)——接线 / 标定 / 上车流程 / 自动整定与超参数预设 / 故障排查
|
||||
- [整体架构规划](../ARCHITECTURE.md)——全系统分层与阶段计划
|
||||
架构规范的唯一来源是仓库根目录的 [`ARCHITECTURE.md`](../ARCHITECTURE.md)。
|
||||
|
||||
本文件不再复制分层图或依赖规则，以免两份架构文档漂移。接线、标定、台架、
|
||||
自动整定和故障排查见 [`chassis.md`](chassis.md)。
|
||||
|
||||
+49
-94
@@ -32,12 +32,11 @@
|
||||
|
||||
| 文档 | 回答的问题 | 读者 |
|
||||
| --- | --- | --- |
|
||||
| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 整套 RoboMaster CAN 控制系统的**全系统分层规划**（含云台、发射机构、delta10 IMU、T153 实时性、9 电机） | 架构决策者 |
|
||||
| [`docs/architecture.md`](architecture.md) | 本仓库**已实现**部分的解耦点：`CanLink` seam、被控对象模型、自动整定两阶段搜索、测试策略 | 改代码的人 |
|
||||
| **本文** | **底盘这一块**怎么开发、怎么标定、怎么整定、怎么上车、出事怎么办 | 底盘开发 / 调试 / 上车的人 |
|
||||
| [`ARCHITECTURE.md`](../ARCHITECTURE.md) | 职责型 workspace、允许依赖、std/unsafe 策略、实现边界 | 架构决策者 |
|
||||
| [`docs/architecture.md`](architecture.md) | 架构文档入口，不复制规范 | 改代码的人 |
|
||||
| **本文** | **底盘这一块**怎么开发、标定、整定、上车与排障 | 底盘开发 / 调试 / 上车的人 |
|
||||
|
||||
`ARCHITECTURE.md` 里的 `rm-*` crate 划分是**规划**；本仓库实际落地为下面这套 crate 命名
|
||||
（功能对应关系见第 5 节）。本文只覆盖底盘切片，云台 / 发射 / IMU 不在范围内。
|
||||
本文只覆盖四轮底盘切片；云台、发射机构与 DETA10 真实载荷布局不在当前范围内。
|
||||
|
||||
---
|
||||
|
||||
@@ -46,7 +45,7 @@
|
||||
### 2.1 无硬件（macOS / Linux / CI 均可）
|
||||
|
||||
```sh
|
||||
cargo test --workspace # 单测 + 闭环仿真 + 整定，全部无需硬件
|
||||
cargo test --workspace --all-targets # 单测 + 闭环仿真 + 整定，全部无需硬件
|
||||
cargo run -p chassisd -- --mock # 进程内电机模型的全链路演示
|
||||
cargo run -p chassis-tune -- presets # 看有哪些整定超参数预设
|
||||
just tune-smoke # 确定性整定冒烟（CI 同款）
|
||||
@@ -57,10 +56,10 @@ just lint # fmt + clippy（与 CI 同标准）
|
||||
|
||||
```sh
|
||||
# 1) 只验链路与反馈，绝不发非零电流（接线排查第一步）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --dry-run
|
||||
cargo run --release -p spin -- chassis --device /dev/ttyACM0 --dry-run
|
||||
|
||||
# 2) 四轮定性旋转（轮子必须架空或整车固定，急停可及）
|
||||
cargo run --release -p chassis-bench --bin chassis-spin -- --device /dev/ttyACM0 --rpm 500
|
||||
cargo run --release -p spin -- chassis --device /dev/ttyACM0 --rpm 500
|
||||
|
||||
# 3) 先离线整定，再上台架复验（用版本化的超参数预设）
|
||||
cargo run --release -p chassis-tune -- sweep --preset config/tune.toml --preset-name sim_bench_model
|
||||
@@ -89,8 +88,8 @@ T153 (Linux) Gm3508 × 4 + C620
|
||||
|
||||
| 路径 | 打开方式 | 适用 |
|
||||
| --- | --- | --- |
|
||||
| **slcan**（`dji-esc-slcan`） | 用户态直接读写 `/dev/ttyACM0`（Lawicel ASCII 协议） | 免 `ip link`、绕开 `gs_usb` 驱动；台架与初始联调首选 |
|
||||
| **SocketCAN**（`dji-esc-socketcan`） | `ip link set can0 up type can bitrate 1000000 restart-ms 100` | 板载原生 CAN；生产部署 |
|
||||
| **slcan**（`slcan`） | 用户态直接读写 `/dev/ttyACM0`（Lawicel ASCII 协议） | 免 `ip link`、绕开 `gs_usb` 驱动；台架与初始联调首选 |
|
||||
| **SocketCAN**（`socketcan`） | `ip link set can0 up type can bitrate 1000000 restart-ms 100` | 板载原生 CAN；生产部署 |
|
||||
|
||||
### 3.2 设备与协议常量（C620 手册）
|
||||
|
||||
@@ -105,11 +104,11 @@ T153 (Linux) Gm3508 × 4 + C620
|
||||
| 反馈频率 | 每电调 1 kHz |
|
||||
| 减速比 | GM3508 = 3591 / 187 ≈ **19.203**（转子 : 输出轴） |
|
||||
|
||||
C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-core::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。
|
||||
C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`motor-control::Pid`，挂在 `EscBus` 的 1 kHz 节拍上）。
|
||||
|
||||
### 3.3 电调 ID 与安装方向
|
||||
|
||||
默认约定（`config/chassis.toml.example`、`chassis-spin` 默认值一致）：
|
||||
默认约定（`config/chassis.toml.example`、`spin chassis` 默认值一致）：
|
||||
|
||||
| 轮位 | 轮序下标 | 电调 ID | 安装方向 |
|
||||
| --- | --- | --- | --- |
|
||||
@@ -125,7 +124,7 @@ C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`moto
|
||||
|
||||
| 场景 | 建议限幅（raw） | 说明 |
|
||||
| --- | --- | --- |
|
||||
| 空载台架（`chassis-spin` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 |
|
||||
| 空载台架（`spin chassis` 默认） | **1500**（≈1.8 A） | 需越过静摩擦（实测 400~600 raw）并留整定余量 |
|
||||
| 整车实车 | 6000 ~ 12000 | 视供电与机械而定；同时是 PID 抗饱和的钳位点 |
|
||||
|
||||
> **限流不是"越大越安全"**：它同时是 PID 输出的钳位点。设得远大于实际所需，
|
||||
@@ -135,7 +134,7 @@ C620 **内部只闭电流环**：转速环 / 位置环由主控侧实现（`moto
|
||||
|
||||
- 电调供电与主控**共地**；CAN_H / CAN_L 不接反，两端 120Ω 终端电阻齐全。
|
||||
- 轮子**架空**或整车固定；急停开关在手可及处。
|
||||
- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`chassis-spin` 会打印每个电调的
|
||||
- 首次上电**先 `--dry-run`**，确认四轮反馈都在刷（`spin chassis` 会打印每个电调的
|
||||
转速 / 温度 / 反馈年龄）。
|
||||
|
||||
---
|
||||
@@ -161,7 +160,7 @@ v_RR = vx - vy + a·ω
|
||||
```
|
||||
|
||||
正解是逆解的左伪逆（JᵀJ 对角，可解析求平均）。实现见
|
||||
[`crates/mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` /
|
||||
[`mecanum`](../crates/mecanum)：`Mecanum::wheel_speeds` / `twist` /
|
||||
`wheel_rpms` / `twist_from_wheel_rpms`。
|
||||
|
||||
### 4.3 单位与层级（最容易出错的地方）
|
||||
@@ -175,7 +174,7 @@ v_RR = vx - vy + a·ω
|
||||
| 控制量 effort | 驱动 / PID | 协议 **raw**（ESC = 电流，C620 ±16384） |
|
||||
| `kp/ki/kd/feedforward` | 速度环 | raw/rpm、raw/(rpm·s)、raw/(rpm/s)、raw/rpm |
|
||||
|
||||
- **减速比换算只发生在一个地方**：`motor-core::OutputShaft` 包装（`转子 = 输出轴 × gear_ratio`）。
|
||||
- **减速比换算只发生在一个地方**：`motor / esc-bus::OutputShaft` 包装（`转子 = 输出轴 × gear_ratio`）。
|
||||
`EscBus` / `EscMotor` 只认转子，绝不碰减速比；`EscConfig::gear_ratio` 仅作配置元数据。
|
||||
- PID 的接线顺序：`ChassisController` 算出**轮端** rpm → `OutputShaft` 换算成转子 rpm →
|
||||
`EscMotor` 的速度环以转子 rpm 闭环。
|
||||
@@ -184,77 +183,34 @@ v_RR = vx - vy + a·ω
|
||||
|
||||
## 5. 软件架构
|
||||
|
||||
### 5.1 Crate 分层与职责
|
||||
### 5.1 职责边界
|
||||
|
||||
**依赖只能向下**（上层可依赖下层，反之不行）：
|
||||
完整 crate 清单和允许依赖见 [`ARCHITECTURE.md`](../ARCHITECTURE.md)。底盘路径上的关键职责是：
|
||||
|
||||
```
|
||||
第 4 层 应用 / 工具 chassisd chassis-tune chassis-bench
|
||||
（含两个 bin）
|
||||
第 3 层 领域 chassis-config mecanum dji-esc-sim
|
||||
（schema / 运动学 / 被控对象 + 离线 harness）
|
||||
第 2 层 协议 / 设备 dji-esc（C620 编解码 + EscBus 驱动 + 命令租约）
|
||||
第 1 层 抽象 can-link（CanFrame + 通道对） motor-core（能力 trait + PID）
|
||||
第 0 层 传输 dji-esc-slcan（串口） / dji-esc-socketcan（内核 CAN）
|
||||
```
|
||||
| 职责 | crate |
|
||||
| --- | --- |
|
||||
| CAN 帧、总线接口、链路与 Mock | `can` |
|
||||
| 电调协议 | `motor-protocol` |
|
||||
| 设备契约、命令/状态/减速比/注册表 | `motor` |
|
||||
| 帧泵、租约、反馈新鲜度、零电流 | `esc-bus` |
|
||||
| PID、速度/位置环、组合策略 | `pid`、`motor-control` |
|
||||
| 麦轮运动学 | `mecanum` |
|
||||
| 配置 schema | `chassis-config` |
|
||||
| 台架安全与采样 | `bench` |
|
||||
| 应用入口 | `chassisd`、`spin`、`chassis-tune` |
|
||||
|
||||
`dji-esc-slcan` / `dji-esc-socketcan` 只实现 `can-link` 的通道语义，**不含任何应用逻辑**；
|
||||
它们被第 4 层的工具与守护进程选用，而不是被协议层依赖。
|
||||
crate 内源码保持扁平，不建立 `src/<module>/` 子目录。
|
||||
|
||||
| Crate | 职责 | 平台 |
|
||||
| --- | --- | --- |
|
||||
| [`motor-core`](../crates/motor-core) | 能力 trait（`VelocityDrive` 等）、`MotorState`、`Pid`、`VelocityController`、`OutputShaft` | 纯逻辑，全平台 |
|
||||
| [`can-link`](../crates/can-link) | `CanFrame` + `CanLink` 通道对（**无协议知识**） | 纯逻辑，全平台 |
|
||||
| [`dji-esc`](../crates/dji-esc) | C620/C610 编解码、多圈累计、`EscBus` 异步驱动、命令租约 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-sim`](../crates/dji-esc-sim) | 被控对象模型、场景、**离线 harness**、异步假电调 | 纯逻辑，全平台 |
|
||||
| [`dji-esc-slcan`](../crates/dji-esc-slcan) | slcan（Lawicel）串口 ↔ `CanLink` 桥接 | 全平台 |
|
||||
| [`dji-esc-socketcan`](../crates/dji-esc-socketcan) | SocketCAN ↔ `CanLink` 桥接 | Linux |
|
||||
| [`mecanum`](../crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 |
|
||||
| [`chassis-config`](../crates/chassis-config) | `chassis.toml` schema：解析 / 校验 / 写回 | 全平台 |
|
||||
| [`chassis-bench`](../crates/chassis-bench) | 台架：安全包络、采样、`chassis-spin` / `socketcan-spin` | 全平台（SocketCAN 工具需 Linux） |
|
||||
| [`chassis-tune`](../crates/chassis-tune) | 速度环 PID 自动整定 + **超参数预设** | 全平台 |
|
||||
| [`chassisd`](../crates/chassisd) | 底盘守护进程：1 kHz 循环、安全逻辑、指令源 seam | 全平台（实车需 Linux） |
|
||||
### 5.2 依赖与组合
|
||||
|
||||
### 5.2 依赖方向（硬约束）
|
||||
`esc-bus` 不依赖 PID 或 `motor-control`。`motor::EffortPolicy` 是两者的组合点：
|
||||
应用构造 `motor-control::MotorPolicy`，并把它传给 `EscBus::spawn`。
|
||||
|
||||
```
|
||||
chassisd ─┬─► chassis-config ─► motor-core, mecanum
|
||||
├─► dji-esc ──► can-link, motor-core
|
||||
├─► dji-esc-sim ─► dji-esc（协议类型）, can-link
|
||||
└─► dji-esc-socketcan ─► can-link
|
||||
总线仍然在 1 kHz 节拍上运行策略，因此控制时序不变；变化的是控制律不再被封在总线驱动里。
|
||||
反馈离线、命令租约到期和模式切换会把对应策略状态复位并强制零输出。
|
||||
|
||||
chassis-tune ─┬─► dji-esc-sim（离线 harness）
|
||||
├─► chassis-bench（安全包络 + 采样）
|
||||
├─► chassis-config（共用 schema）
|
||||
└─► dji-esc ─► can-link, motor-core
|
||||
|
||||
chassis-bench ─┬─► dji-esc ─► can-link, motor-core
|
||||
├─► dji-esc-sim（场景 / 轨迹类型）
|
||||
├─► dji-esc-slcan / dji-esc-socketcan
|
||||
└─► chassis-config
|
||||
```
|
||||
|
||||
**传输后端 crate 只做桥接，不含应用逻辑**：`chassis-spin` 曾住在 `dji-esc-slcan` 的
|
||||
example 里，现在统一收归 `chassis-bench`；整定逻辑住在 `chassis-tune`。要换总线介质
|
||||
（串口转 CAN、MCU 转发），只需**新增一个桥接 crate**，协议 / 控制 / 整定全部不动。
|
||||
|
||||
### 5.3 一个 1 ms 拍里发生了什么
|
||||
|
||||
```
|
||||
tick (1ms)
|
||||
├─ 读取 watch<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）。
|
||||
|
||||
---
|
||||
`bench` 只产生试验轨迹和安全中止原因；指标、成本、搜索和报告适配由
|
||||
`tuning` 与 `chassis-tune` 完成。
|
||||
|
||||
## 6. 关键机制（实车踩过的坑）
|
||||
|
||||
@@ -292,7 +248,7 @@ tick (1ms)
|
||||
|
||||
## 7. 控制设计
|
||||
|
||||
### 7.1 PID（`motor-core::Pid`）
|
||||
### 7.1 PID（`motor-control::Pid`）
|
||||
|
||||
并联式 PID，显式时间步 `dt`（秒），特性：
|
||||
|
||||
@@ -302,7 +258,7 @@ tick (1ms)
|
||||
4. 可选**微分一阶低通**（`with_derivative_tau`），抑制编码器量化噪声；
|
||||
5. `dt <= 0` 或非有限输入 → 返回上次输出且不更新内部状态（防御跳帧 / 首帧）。
|
||||
|
||||
### 7.2 速度环（`motor-core::VelocityController`）
|
||||
### 7.2 速度环（`motor-control::VelocityController`）
|
||||
|
||||
```
|
||||
effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rpm, ±limit )
|
||||
@@ -324,7 +280,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp
|
||||
| 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 |
|
||||
| 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 |
|
||||
|
||||
### 7.4 被控对象模型（`dji-esc-sim`）
|
||||
### 7.4 被控对象模型（`sim-harness::motor_model`）
|
||||
|
||||
逐拍离散（`dt` 秒，电调坐标系：正电流 → 正转速）：
|
||||
|
||||
@@ -394,9 +350,9 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程
|
||||
|
||||
| # | 步骤 | 命令 | 通过判据 |
|
||||
| --- | --- | --- | --- |
|
||||
| 1 | **链路与反馈** | `chassis-spin --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID |
|
||||
| 1 | **链路与反馈** | `spin chassis --dry-run` | 四个电调都在刷反馈（转速 ≈ 0、温度正常、反馈年龄 < 100 ms）。缺一个就查线 / 电调 ID |
|
||||
| 2 | **电调 ID 对应** | 同 1，逐个拔插 | 拔掉哪个电调，哪个 ID 的反馈消失 → ID 映射正确 |
|
||||
| 3 | **方向** | `chassis-spin --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` |
|
||||
| 3 | **方向** | `spin chassis --rpm 200`（低速） | 四个轮子都朝"底盘前进"方向转；反了就改 `direction` |
|
||||
| 4 | **减速比 / 轮径** | 手转一圈 + 读反馈角度 | 反馈角度 × 减速比 ≈ 轮子转过角度；不符则改 `gear_ratio` |
|
||||
| 5 | **几何参数** | 卷尺量 `half_length` / `half_width` / `wheel_radius` | 填入 `chassis.toml`（影响运动学精度，不影响稳定性） |
|
||||
| 6 | **限流** | 从 1500 raw 起，逐步加到"能可靠起动且不冲" | 越小越好；实车再按需上调 |
|
||||
@@ -485,7 +441,7 @@ chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预
|
||||
| `field_updown` | bench | **up-down** 1200 rpm | 实战验收：升 / 降 / 回零双向，阈值收紧到 12% / 3% |
|
||||
|
||||
**预设 schema**（`version = 1`，未知键报错，见
|
||||
[`crates/chassis-tune/src/preset.rs`](../crates/chassis-tune/src/preset.rs)）：
|
||||
[`crates/tuning/src/lib.rs`](../crates/tuning/src/lib.rs)）：
|
||||
|
||||
```toml
|
||||
version = 1
|
||||
@@ -556,7 +512,7 @@ feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜
|
||||
|
||||
## 11. 台架工具
|
||||
|
||||
### 11.1 `chassis-spin`（`chassis-bench`，slcan，全平台）
|
||||
### 11.1 `spin chassis`（`spin`，slcan，全平台）
|
||||
|
||||
四轮同时以固定**转子**转速旋转。关键参数：
|
||||
|
||||
@@ -586,13 +542,13 @@ feedforward = { min = 0.0, max = 0.0, scale = "linear" } # 单点区间 = 搜
|
||||
- 上电门控：四个电调都上报新鲜反馈前绝不下发速度；
|
||||
- 空载的合适整定与带载**完全不同**，空载**不要**加前馈。
|
||||
|
||||
### 11.2 `socketcan-spin`（`chassis-bench`，仅 Linux）
|
||||
### 11.2 `spin motor`（`spin`，仅 Linux）
|
||||
|
||||
单电机链路验证，走 SocketCAN。用于板载原生 CAN 的接线排查：
|
||||
|
||||
```sh
|
||||
ip link set can0 up type can bitrate 1000000 restart-ms 100
|
||||
cargo run --release -p chassis-bench --bin socketcan-spin -- --interface can0 --dry-run
|
||||
cargo run --release -p spin -- motor -- --interface can0 --dry-run
|
||||
```
|
||||
|
||||
---
|
||||
@@ -632,7 +588,7 @@ cargo run --release -p chassis-bench --bin socketcan-spin -- --interface can0 --
|
||||
| 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） |
|
||||
| **预设** | schema 往返、逐字段覆盖、优先级（CLI > 预设 > 内置）、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 |
|
||||
| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） |
|
||||
| 实车台架 | `chassis-spin` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） |
|
||||
| 实车台架 | `spin chassis` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） |
|
||||
| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check；armv7 静态构建 `chassisd` | CI |
|
||||
|
||||
### 13.2 每个阶段完成后的门禁（`just lint` + 以下）
|
||||
@@ -660,7 +616,7 @@ just build-armv7 # cargo build --release -p chassisd --target armv7-unknown-
|
||||
just deploy # 编译 + 推送 + mock 试运行（默认 root@192.168.2.2）
|
||||
```
|
||||
|
||||
目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。二进制静态链接，不依赖板端 libc。
|
||||
目标板实测：创龙 T153 / Ubuntu 22.04（armv7）。发布二进制使用 musl 静态链接。
|
||||
|
||||
### 14.2 板端拉起 CAN
|
||||
|
||||
@@ -692,9 +648,8 @@ RestartSec=1
|
||||
> （`ip -d link show can0` 可见），`chassisd` 侧表现为四电机反馈离线、强制零电流——
|
||||
> **这正是安全设计的预期行为**，不是故障。
|
||||
>
|
||||
> 调度策略：`ARCHITECTURE.md` 规划了 `SCHED_FIFO` + `mlockall`（对应 `rm-app::realtime`），
|
||||
> 本仓库目前**未实现**实时优先级提升，当前只依赖 1 kHz tokio 定时器 +
|
||||
> `MissedTickBehavior::Skip`。板端抖动实测与是否需要加固见第 15 节的抖动条目。
|
||||
> 调度策略：应用不调用 libc FFI。
|
||||
> 应用不调用 libc FFI。`SCHED_FIFO` 与资源限制由 systemd 服务单元设置；std 程序不调用 `mlockall`。
|
||||
|
||||
---
|
||||
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user