refactor: enforce layered crates and Rust audit gates

This commit is contained in:
Horldsence
2026-09-24 13:00:03 +08:00
parent af151525d1
commit 8a582fc102
67 changed files with 3918 additions and 581 deletions
+4
View File
@@ -0,0 +1,4 @@
#!/bin/sh
set -eu
git diff --cached --check
cargo +nightly -Zscript /Volumes/Peng/AppCode/Rust/sentry-chassis/scripts/audit-rules.rs --staged
+4 -5
View File
@@ -16,14 +16,13 @@ jobs:
- uses: dtolnay/rust-toolchain@stable
with:
components: rustfmt, clippy
- uses: dtolnay/rust-toolchain@nightly
- uses: Swatinem/rust-cache@v2
- run: cargo +nightly -Zscript scripts/audit-rules.rs --self-test
- run: cargo +nightly -Zscript scripts/audit-rules.rs
- 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
- name: deterministic tuning smoke
@@ -42,7 +41,7 @@ jobs:
sudo modprobe vcan
sudo ip link add dev vcan0 type vcan
sudo ip link set up vcan0
- run: cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1
- run: cargo test -p integration-tests --test socketcan_vcan -- --ignored --test-threads 1
cross-check:
runs-on: ubuntu-latest
+3
View File
@@ -1 +1,4 @@
/target
__pycache__/
*.pyc
+17 -4
View File
@@ -13,6 +13,7 @@
- **依赖必须无环**:允许依赖同层或更低抽象,但禁止反向控制依赖和隐藏的完整链路打包。
- **std,禁止 unsafe**:workspace 使用 `unsafe_code = "forbid"`;允许使用提供安全 API 的第三方依赖。
- **实时调度交给 systemd**:进程不调用 libc FFI,不实现 `SCHED_FIFO` 或 `mlockall`。
- **二级语义分层**:crate 名不变,manifest 按 `crates/<layer>/<domain>` 组织;依赖只能由高层指向低层。
- **crate 内保持扁平**:只允许 Cargo 惯例目录 `src`、`tests`;`src` 内不建立模块子目录。
- **安全边界独立于控制律**:总线负责反馈新鲜度、命令租约和零电流;控制器通过策略接口组合。
@@ -26,11 +27,13 @@
协议
motor-protocol C620/C610、GM6020 编解码、多圈与 ID
delta10-protocol DETA10 帧布局与解码
dr16-protocol DJI DR16 D-BUS 帧布局与解码
驱动
socketcan Linux SocketCAN ↔ CanLink
slcan Lawicel 串口 CAN ↔ CanLink
delta10 DETA10 IMU 驱动
dr16 DR16 D-BUS 串口接收驱动
设备与运行时
motor Motor 能力 trait、状态、命令、减速比、注册表、EffortPolicy
@@ -53,20 +56,29 @@
chassisd 底盘守护进程二进制
spin chassis / motor 台架二进制
chassis-tune 自动整定二进制与报告/绘图适配
验证
integration-tests 跨层闭环、语义一致性与 vcan 集成测试
```
共 21 个 crate。crate 名不使用 `rm-` 等无领域含义的统一前缀。
共 24 个 crate(含顶层 `verification/integration` 集成测试包)。crate 名不使用 `rm-` 等无领域含义的统一前缀。
## 3. 允许依赖
机器可读规则见 [`config/dependency-policy.toml`](config/dependency-policy.toml) 和
[`docs/dependencies.md`](docs/dependencies.md)。`just audit` 同时检查层 rank、正常/dev 依赖、
外部库职责和 trait 边界;正常与 dev 依赖都必须单向。
主要边如下:
```text
motor-protocol -> can
delta10-protocol -> can, imu
dr16-protocol -> (无内部依赖)
socketcan -> can
slcan -> can
delta10 -> can, imu, delta10-protocol
dr16 -> dr16-protocol
motor -> can, motor-protocol
esc-bus -> can, motor, motor-protocol
@@ -82,8 +94,8 @@ tuning -> pid, motor-control, motor-model, sim-harness, experiment
chassis-config -> esc-bus, motor-control, motor-protocol, mecanum
bench -> can, esc-bus, experiment, motor, motor-control, socketcan, slcan
chassisd -> chassis-config, esc-bus, mecanum, motor-control, sim-harness, socketcan
spin -> bench, esc-bus, motor-control, socketcan, slcan
chassisd -> chassis-config, dr16, esc-bus, mecanum, motor-control, sim-harness, socketcan
spin -> bench, dr16, esc-bus, motor-control, socketcan, slcan
chassis-tune -> bench, chassis-config, experiment, sim-harness, tuning
```
@@ -136,7 +148,8 @@ tests/*.rs # 可选集成测试
- 四轮 ESC 总线、命令租约、反馈离线保护、外部控制策略。
- PID、速度/位置/串级/前馈、麦轮运动学。
- 电机模型、总线损伤、同步/异步闭环、参数搜索、预设与报告。
- `chassisd --mock`、`spin chassis|motor`、`chassis-tune`。
- `chassisd --mock`、`spin chassis|motor|rc`、`chassis-tune`。
- DR16/D-BUS 遥控输入、S1 使能、断流失能和 500 ms 速度斜坡。
未实现:
Generated
+39 -8
View File
@@ -205,6 +205,7 @@ dependencies = [
"approx",
"esc-bus",
"mecanum",
"motor",
"motor-control",
"motor-protocol",
"serde",
@@ -256,6 +257,8 @@ dependencies = [
"can",
"chassis-config",
"clap",
"dr16",
"dr16-protocol",
"esc-bus",
"mecanum",
"motor",
@@ -540,6 +543,24 @@ dependencies = [
"libloading",
]
[[package]]
name = "dr16"
version = "0.1.0"
dependencies = [
"dr16-protocol",
"serial",
"tokio",
"tracing",
]
[[package]]
name = "dr16-protocol"
version = "0.1.0"
dependencies = [
"proptest",
"thiserror",
]
[[package]]
name = "dwrote"
version = "0.11.5"
@@ -595,12 +616,8 @@ version = "0.1.0"
dependencies = [
"approx",
"can",
"experiment",
"motor",
"motor-control",
"motor-model",
"motor-protocol",
"sim-harness",
"thiserror",
"tokio",
"tracing",
@@ -873,6 +890,22 @@ dependencies = [
"web-time",
]
[[package]]
name = "integration-tests"
version = "0.1.0"
dependencies = [
"can",
"esc-bus",
"experiment",
"motor",
"motor-control",
"motor-model",
"motor-protocol",
"sim-harness",
"socketcan 0.1.0",
"tokio",
]
[[package]]
name = "ioctl-rs"
version = "0.1.6"
@@ -1662,10 +1695,6 @@ name = "socketcan"
version = "0.1.0"
dependencies = [
"can",
"esc-bus",
"motor",
"motor-control",
"motor-protocol",
"socketcan 4.0.0",
"thiserror",
"tokio",
@@ -1699,6 +1728,8 @@ version = "0.1.0"
dependencies = [
"bench",
"clap",
"dr16",
"dr16-protocol",
"esc-bus",
"experiment",
"mecanum",
+48 -42
View File
@@ -1,27 +1,30 @@
[workspace]
resolver = "3"
members = [
"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/bench",
"crates/chassisd",
"crates/spin",
"crates/chassis-tune",
"crates/contract/can",
"crates/contract/imu",
"crates/protocol/motor",
"crates/protocol/imu",
"crates/transport/can",
"crates/transport/serial",
"crates/device/imu",
"crates/device/motor",
"crates/device/esc",
"crates/control/algorithm",
"crates/control/motor",
"crates/control/chassis",
"crates/protocol/remote",
"crates/device/remote",
"crates/model/motor",
"crates/experiment/trace",
"crates/simulation/motor",
"crates/tooling/tuning",
"crates/config/chassis",
"crates/application/bench",
"crates/application/daemon",
"crates/application/tools",
"crates/application/tuning",
"crates/verification/integration",
]
[workspace.package]
@@ -71,27 +74,30 @@ anyhow = "1"
plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] }
# ---- 职责型内部 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" }
bench = { path = "crates/bench" }
chassisd = { path = "crates/chassisd" }
spin = { path = "crates/spin" }
chassis-tune = { path = "crates/chassis-tune" }
can = { path = "crates/contract/can" }
imu = { path = "crates/contract/imu" }
motor-protocol = { path = "crates/protocol/motor" }
delta10-protocol = { path = "crates/protocol/imu" }
socketcan = { path = "crates/transport/can" }
slcan = { path = "crates/transport/serial" }
delta10 = { path = "crates/device/imu" }
motor = { path = "crates/device/motor" }
esc-bus = { path = "crates/device/esc" }
pid = { path = "crates/control/algorithm" }
motor-control = { path = "crates/control/motor" }
mecanum = { path = "crates/control/chassis" }
dr16-protocol = { path = "crates/protocol/remote" }
dr16 = { path = "crates/device/remote" }
motor-model = { path = "crates/model/motor" }
experiment = { path = "crates/experiment/trace" }
sim-harness = { path = "crates/simulation/motor" }
tuning = { path = "crates/tooling/tuning" }
chassis-config = { path = "crates/config/chassis" }
bench = { path = "crates/application/bench" }
chassisd = { path = "crates/application/daemon" }
spin = { path = "crates/application/tools" }
chassis-tune = { path = "crates/application/tuning" }
integration-tests = { path = "crates/verification/integration" }
[workspace.lints.rust]
unsafe_code = "forbid"
+12 -1
View File
@@ -2,7 +2,17 @@
RoboMaster 哨兵机器人麦轮底盘:Linux ARM 主控、4 × GM3508/C620、SocketCAN 或 SLCAN、tokio 异步运行时。
架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 21 个职责单一 crate 组成,不使用 `rm-` 前缀,crate 内源码保持扁平。
架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 24 个职责单一 crate / 测试包组成,不使用 `rm-` 前缀,crate 内源码保持扁平。
安装本地规则 hook:
```sh
just install-hooks
just audit
```
依赖层级、外部库白名单和 trait 边界见 [`docs/dependencies.md`](docs/dependencies.md)。
规则审查器是基于 Nightly `cargo -Zscript` 的单文件 Rust 脚本。
## 结构
@@ -12,6 +22,7 @@ RoboMaster 哨兵机器人麦轮底盘:Linux ARM 主控、4 × GM3508/C620、S
- `motor` / `esc-bus`:设备契约与安全总线运行时。
- `pid` / `motor-control` / `mecanum`:纯控制算法。
- `motor-model` / `experiment` / `sim-harness` / `tuning`:仿真与自动整定。
- `dr16-protocol` / `dr16`:DJI DR16/D-BUS 协议和串口接收。
- `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`:配置、台架和应用入口。
`esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为
+15
View File
@@ -4,6 +4,21 @@
[can]
interface = "can0"
# DJI DT7 + DR16 遥控接收机。没有遥控时删除整个 [remote] 段,守护进程会保持静止。
[remote]
kind = "dr16"
device = "/dev/ttyS2"
# 合法 D-BUS 帧断流超时(毫秒);超时立即失能停车。
timeout_ms = 100
# 摇杆满量程死区(0..0.5),死区后使用平方响应曲线。
deadband = 0.05
# 从零到满量程的逐轴速度斜坡(毫秒);S1 失能、断流和超时立即归零。
ramp_ms = 500
# 实车方向相反时置 true,不需要改代码。
invert_vx = false
invert_vy = false
invert_omega = false
[chassis]
# 控制循环周期(毫秒)。C620 期望 1kHz 命令。
control_period_ms = 1
+278
View File
@@ -0,0 +1,278 @@
# Machine-readable dependency and layer policy.
policy_version = 1
[layers]
"contract/can" = 0
"contract/imu" = 0
"protocol/imu" = 1
"protocol/motor" = 1
"protocol/remote" = 1
"transport/can" = 2
"transport/serial" = 2
"device/imu" = 3
"device/motor" = 3
"device/remote" = 3
"control/algorithm" = 4
"control/chassis" = 4
"device/esc" = 4
"experiment/trace" = 5
"control/motor" = 6
"model/motor" = 7
"simulation/motor" = 8
"tooling/tuning" = 9
"config/chassis" = 10
"application/bench" = 11
"application/daemon" = 12
"application/tools" = 13
"application/tuning" = 13
"verification/integration" = 14
[forbidden.normal]
esc_bus = ["motor-control", "pid"]
bench = ["tuning"]
sim_harness = ["tuning"]
[trait_boundaries]
protocol_motor = ["MotorProtocol", "EffortScale", "FeedbackFields"]
device_motor = ["MotorSpec", "MotorFeedback", "EffortDrive", "VelocityDrive", "PositionDrive", "Disable"]
model_motor = ["MotorPlant", "DcModelSpec", "VoltageModelSpec"]
esc_bus_forbidden_symbols = ["MotorKind", "EscModel"]
[packages."bench"]
path = "crates/application/bench"
layer = "application/bench"
rank = 11
normal = ["can", "esc-bus", "experiment", "motor", "motor-control", "slcan", "socketcan"]
dev = []
external = ["anyhow", "thiserror", "tokio", "tracing"]
dev_external = ["tokio"]
[packages."can"]
path = "crates/contract/can"
layer = "contract/can"
rank = 0
normal = []
dev = []
external = ["tokio"]
dev_external = ["tokio"]
[packages."chassis-config"]
path = "crates/config/chassis"
layer = "config/chassis"
rank = 10
normal = ["esc-bus", "mecanum", "motor", "motor-control", "motor-protocol"]
dev = []
external = ["serde", "thiserror", "toml"]
dev_external = ["approx"]
[packages."chassis-tune"]
path = "crates/application/tuning"
layer = "application/tuning"
rank = 13
normal = ["bench", "can", "chassis-config", "esc-bus", "experiment", "motor", "motor-control", "motor-model", "motor-protocol", "sim-harness", "slcan", "socketcan", "tuning"]
dev = []
external = ["anyhow", "argmin", "argmin-math", "clap", "csv", "indicatif", "plotters", "rand", "rayon", "serde", "serde_json", "thiserror", "tokio", "toml", "tracing", "tracing-subscriber"]
dev_external = ["approx"]
[packages."chassisd"]
path = "crates/application/daemon"
layer = "application/daemon"
rank = 12
normal = ["can", "chassis-config", "dr16", "dr16-protocol", "esc-bus", "mecanum", "motor", "motor-control", "sim-harness", "socketcan"]
dev = []
external = ["clap", "tokio", "tracing", "tracing-subscriber"]
dev_external = ["approx", "tokio"]
[packages."delta10"]
path = "crates/device/imu"
layer = "device/imu"
rank = 3
normal = ["can", "delta10-protocol", "imu"]
dev = []
external = ["thiserror", "tokio"]
dev_external = []
[packages."delta10-protocol"]
path = "crates/protocol/imu"
layer = "protocol/imu"
rank = 1
normal = ["can", "imu"]
dev = []
external = ["thiserror"]
dev_external = ["approx"]
[packages."dr16"]
path = "crates/device/remote"
layer = "device/remote"
rank = 3
normal = ["dr16-protocol"]
dev = []
external = ["serial", "tokio", "tracing"]
dev_external = ["tokio"]
[packages."dr16-protocol"]
path = "crates/protocol/remote"
layer = "protocol/remote"
rank = 1
normal = []
dev = []
external = ["thiserror"]
dev_external = ["proptest"]
[packages."esc-bus"]
path = "crates/device/esc"
layer = "device/esc"
rank = 4
normal = ["can", "motor", "motor-protocol"]
dev = []
external = ["thiserror", "tokio", "tracing"]
dev_external = ["approx", "tokio"]
[packages."experiment"]
path = "crates/experiment/trace"
layer = "experiment/trace"
rank = 5
normal = []
dev = []
external = []
dev_external = ["approx"]
[packages."imu"]
path = "crates/contract/imu"
layer = "contract/imu"
rank = 0
normal = []
dev = []
external = []
dev_external = []
[packages."integration-tests"]
path = "crates/verification/integration"
layer = "verification/integration"
rank = 14
normal = ["can", "esc-bus", "experiment", "motor", "motor-control", "motor-model", "motor-protocol", "sim-harness", "socketcan"]
dev = []
external = ["tokio"]
dev_external = ["tokio"]
[packages."mecanum"]
path = "crates/control/chassis"
layer = "control/chassis"
rank = 4
normal = []
dev = []
external = []
dev_external = ["approx", "proptest"]
[packages."motor"]
path = "crates/device/motor"
layer = "device/motor"
rank = 3
normal = ["can", "motor-protocol"]
dev = []
external = ["thiserror"]
dev_external = ["approx"]
[packages."motor-control"]
path = "crates/control/motor"
layer = "control/motor"
rank = 6
normal = ["motor", "pid"]
dev = []
external = []
dev_external = ["approx"]
[packages."motor-model"]
path = "crates/model/motor"
layer = "model/motor"
rank = 7
normal = ["motor-protocol"]
dev = []
external = []
dev_external = ["approx"]
[packages."motor-protocol"]
path = "crates/protocol/motor"
layer = "protocol/motor"
rank = 1
normal = ["can"]
dev = []
external = ["thiserror"]
dev_external = ["approx", "proptest"]
[packages."pid"]
path = "crates/control/algorithm"
layer = "control/algorithm"
rank = 4
normal = []
dev = []
external = []
dev_external = ["approx"]
[packages."sim-harness"]
path = "crates/simulation/motor"
layer = "simulation/motor"
rank = 8
normal = ["can", "experiment", "motor", "motor-control", "motor-model", "motor-protocol"]
dev = []
external = ["rand", "thiserror", "tokio", "tracing"]
dev_external = ["approx", "tokio"]
[packages."slcan"]
path = "crates/transport/serial"
layer = "transport/serial"
rank = 2
normal = ["can"]
dev = []
external = ["serial", "slcan", "thiserror", "tokio", "tracing"]
dev_external = ["tokio"]
[packages."socketcan"]
path = "crates/transport/can"
layer = "transport/can"
rank = 2
normal = ["can"]
dev = []
external = ["socketcan", "thiserror", "tokio", "tracing"]
dev_external = ["tokio"]
[packages."spin"]
path = "crates/application/tools"
layer = "application/tools"
rank = 13
normal = ["bench", "dr16", "dr16-protocol", "esc-bus", "experiment", "mecanum", "motor", "motor-control", "slcan", "socketcan"]
dev = []
external = ["clap", "tokio", "tracing", "tracing-subscriber"]
dev_external = ["tokio"]
[packages."tuning"]
path = "crates/tooling/tuning"
layer = "tooling/tuning"
rank = 9
normal = ["experiment", "motor-control", "motor-model", "motor-protocol", "pid", "sim-harness"]
dev = []
external = ["argmin", "argmin-math", "indicatif", "rand", "rayon", "serde", "thiserror", "toml", "tracing"]
dev_external = ["approx", "proptest"]
[external_roles]
tokio = ["contract", "transport", "device", "simulation", "tooling", "application", "verification"]
socketcan = ["transport", "device", "application", "verification"]
slcan = ["transport", "device", "application", "verification"]
serial = ["transport", "device", "application", "verification"]
thiserror = ["protocol", "transport", "device", "control", "model", "experiment", "simulation", "tooling", "config", "application", "verification"]
tracing = ["transport", "device", "simulation", "tooling", "application", "verification"]
tracing-subscriber = ["application", "verification"]
serde = ["config", "tooling", "application", "verification"]
toml = ["config", "tooling", "application", "verification"]
clap = ["application", "verification"]
argmin = ["tooling", "application", "verification"]
argmin-math = ["tooling", "application", "verification"]
rand = ["simulation", "tooling", "application", "verification"]
rayon = ["tooling", "application", "verification"]
csv = ["tooling", "application", "verification"]
serde_json = ["tooling", "application", "verification"]
indicatif = ["tooling", "application", "verification"]
plotters = ["tooling", "application", "verification"]
anyhow = ["tooling", "application", "verification"]
approx = ["protocol", "transport", "device", "control", "model", "experiment", "simulation", "tooling", "config", "application", "verification"]
proptest = ["protocol", "transport", "device", "control", "model", "experiment", "simulation", "tooling", "config", "application", "verification"]
+6 -6
View File
@@ -19,7 +19,7 @@
# 不属于超参数,本文件**不接受**这些键(写了会直接报错)。它们只来自
# chassis.toml 或显式 CLI——否则预设文件就成了绕过限流的口子。
#
# schema:见 crates/chassis-tune/src/preset.rs(version = 1,未知键报错)。
# schema:见 crates/application/tuning/src/main.rs(version = 1,未知键报错)。
version = 1
default_preset = "sim_bench_model"
@@ -68,7 +68,7 @@ max_evals = 4000
threads = 0
[preset.sim_bench_model.sim]
plant = "gm3508-bench"
plant = "bench"
# 域随机化:单点模型调出来的增益对增益/时间常数/延迟高度敏感,
# 因此默认在 8 个成员的集成上做鲁棒优化,而不是对着一条理想曲线最优化。
ensemble = 8
@@ -128,7 +128,7 @@ max_evals = 1000
threads = 0
[preset.sim_ideal.sim]
plant = "gm3508"
plant = "ideal"
# 理想模型本身就是确定的,单成员即可。
ensemble = 1
robust_weight = 0.0
@@ -193,7 +193,7 @@ threads = 1
[preset.bench_noload.sim]
# 真机后端不用这些,但保留"最接近台架的模型",方便先离线预演再上车。
plant = "gm3508-bench"
plant = "bench"
ensemble = 8
robust_weight = 0.5
gain_pct = 0.2
@@ -254,7 +254,7 @@ max_evals = 80
threads = 1
[preset.bench_loaded.sim]
plant = "gm3508-bench"
plant = "bench"
ensemble = 8
robust_weight = 0.5
gain_pct = 0.2
@@ -314,7 +314,7 @@ max_evals = 40
threads = 1
[preset.field_updown.sim]
plant = "gm3508-bench"
plant = "bench"
ensemble = 8
robust_weight = 0.5
gain_pct = 0.2
@@ -24,3 +24,6 @@ anyhow = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "application/bench"
@@ -15,6 +15,8 @@ motor = { workspace = true }
esc-bus = { workspace = true }
motor-control = { workspace = true }
mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
chassis-config = { workspace = true }
sim-harness = { workspace = true }
socketcan = { workspace = true }
@@ -26,3 +28,6 @@ tracing-subscriber = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/daemon"
@@ -166,62 +166,63 @@ pub mod messages {
pub use messages::*;
pub mod source {
//! 上层速度指令源(twist source)。
//! 上层底盘指令源。
//!
//! 守护进程通过 [`watch`] 通道把指令交给控制循环;指令源在独立任务中
//! 持续产出 twist。遥控器(DR16)、自主导航等后续接入时各自实现
//! [`TwistSource`] 即可。
//! 守护进程通过 [`watch`] 通道把指令交给控制循环;指令源持续产出
//! [`RemoteCommand`]。遥控器、自主导航等来源共享同一接口,失能语义不会在
//! 各来源里重复实现。
use std::future::Future;
use mecanum::Twist;
use tokio::sync::{mpsc, watch};
/// 速度指令源。
pub trait TwistSource: Send {
use crate::RemoteCommand;
/// 底盘指令源。
pub trait CommandSource: Send {
/// 取下一条指令;`None` 表示源关闭。
fn next_twist(&mut self) -> impl Future<Output = Option<Twist>> + Send;
fn next_command(&mut self) -> impl Future<Output = Option<RemoteCommand>> + Send;
}
/// 恒零占位源:永不产出(watch 保持初值 `Twist::ZERO`,指令超时逻辑兜底)。
/// 恒失能占位源:永不产出,控制器超时逻辑兜底停车。
pub struct ZeroSource;
impl TwistSource for ZeroSource {
async fn next_twist(&mut self) -> Option<Twist> {
impl CommandSource for ZeroSource {
async fn next_command(&mut self) -> Option<RemoteCommand> {
std::future::pending().await
}
}
/// 通道指令源(测试 / 简单进程内注入用)。
pub struct ChannelSource {
rx: mpsc::Receiver<Twist>,
rx: mpsc::Receiver<RemoteCommand>,
}
impl ChannelSource {
/// 由 mpsc 接收端构造。
#[must_use]
pub const fn new(rx: mpsc::Receiver<Twist>) -> Self {
pub const fn new(rx: mpsc::Receiver<RemoteCommand>) -> Self {
Self { rx }
}
/// 创建一对(发送端, 源)。
#[must_use]
pub fn pair(bound: usize) -> (mpsc::Sender<Twist>, Self) {
pub fn pair(bound: usize) -> (mpsc::Sender<RemoteCommand>, Self) {
let (tx, rx) = mpsc::channel(bound);
(tx, Self { rx })
}
}
impl TwistSource for ChannelSource {
async fn next_twist(&mut self) -> Option<Twist> {
impl CommandSource for ChannelSource {
async fn next_command(&mut self) -> Option<RemoteCommand> {
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() {
pub async fn pump_source(mut source: impl CommandSource, tx: watch::Sender<RemoteCommand>) {
while let Some(command) = source.next_command().await {
if tx.send(command).is_err() {
break;
}
}
@@ -229,28 +230,249 @@ pub mod source {
#[cfg(test)]
mod tests {
use mecanum::Twist;
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 (watch_tx, mut watch_rx) = watch::channel(RemoteCommand::DISABLED);
let pump = tokio::spawn(pump_source(source, watch_tx));
tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap();
let command = RemoteCommand::new(Twist { vx: 1.0, vy: 0.0, omega: 0.0 });
tx.send(command).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 });
assert_eq!(*watch_rx.borrow_and_update(), command);
// 源关闭 → 泵任务退出。
drop(tx);
pump.await.unwrap();
assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值
assert_eq!(watch_rx.borrow().twist, command.twist);
}
}
}
pub use source::*;
pub mod remote {
//! DR16 遥控输入到安全底盘指令的映射。
//!
//! 左摇杆上下控制前后、左右控制旋转;右摇杆左右控制横移。摇杆先过死区,
//! 再做平方响应曲线。S1 上档是唯一的运动使能开关。
use std::future::pending;
use std::time::{Duration, Instant};
use chassis_config::RemoteConfig;
use dr16::{SampleStream, StampedSample};
use dr16_protocol::{Dr16Sample, SwitchPosition};
use mecanum::Twist;
use crate::{CommandSource, RemoteCommand};
/// DR16 摇杆满量程的协议值。
const FULL_SCALE: f32 = 660.0;
/// 遥控输入映射器。
#[derive(Debug, Clone, Copy)]
pub struct RemoteMapper {
deadband: f32,
max_vx: f32,
max_vy: f32,
max_omega: f32,
invert_vx: bool,
invert_vy: bool,
invert_omega: bool,
}
impl RemoteMapper {
/// 创建映射器;配置语义由 `chassis-config::Config::validate` 保证。
#[must_use]
pub const fn new(
config: &RemoteConfig,
max_forward: f32,
max_strafe: f32,
max_spin: f32,
) -> Self {
Self {
deadband: config.deadband,
max_vx: max_forward,
max_vy: max_strafe,
max_omega: max_spin,
invert_vx: config.invert_vx,
invert_vy: config.invert_vy,
invert_omega: config.invert_omega,
}
}
/// 将一个合法 DR16 样本映射为底盘意图。
#[must_use]
pub fn map(&self, sample: &Dr16Sample) -> RemoteCommand {
let vx = self.axis(sample.channel_t(), self.max_vx, self.invert_vx);
// ISO 坐标 y 向左为正,右摇杆向右应产生负 y。
let vy = -self.axis(sample.channel_a(), self.max_vy, self.invert_vy);
// ISO 坐标 omega 逆时针为正,左摇杆向右应产生顺时针负 omega。
let omega = -self.axis(sample.channel_r(), self.max_omega, self.invert_omega);
RemoteCommand {
twist: Twist { vx, vy, omega },
enabled: sample.s1 == SwitchPosition::Up,
}
}
fn axis(&self, raw: i16, maximum: f32, invert: bool) -> f32 {
let sign = if invert { -1.0 } else { 1.0 };
sign * self.shaped(raw) * maximum
}
fn shaped(&self, raw: i16) -> f32 {
let normalized = (f32::from(raw) / FULL_SCALE).clamp(-1.0, 1.0);
let magnitude = normalized.abs();
if magnitude <= self.deadband {
return 0.0;
}
let scaled = (magnitude - self.deadband) / (1.0 - self.deadband);
normalized.signum() * scaled * scaled
}
}
/// DR16 样本源:超时和串口关闭都会立即发布失能指令。
pub struct Dr16CommandSource {
samples: SampleStream,
mapper: RemoteMapper,
timeout: Duration,
last_received_at: Instant,
timed_out: bool,
closed: bool,
}
impl Dr16CommandSource {
/// 创建 DR16 指令源。
#[must_use]
pub fn new(samples: SampleStream, mapper: RemoteMapper, timeout: Duration) -> Self {
Self {
samples,
mapper,
timeout,
last_received_at: Instant::now(),
timed_out: false,
closed: false,
}
}
fn command(&mut self, stamped: StampedSample) -> RemoteCommand {
self.last_received_at = stamped.received_at;
self.timed_out = false;
self.mapper.map(&stamped.sample)
}
}
impl CommandSource for Dr16CommandSource {
async fn next_command(&mut self) -> Option<RemoteCommand> {
if self.closed {
return pending().await;
}
if self.timed_out {
return if let Some(stamped) = self.samples.recv().await {
Some(self.command(stamped))
} else {
self.closed = true;
Some(RemoteCommand::DISABLED)
};
}
let elapsed = Instant::now().saturating_duration_since(self.last_received_at);
let remaining = self.timeout.saturating_sub(elapsed);
match tokio::time::timeout(remaining, self.samples.recv()).await {
Ok(Some(stamped)) => Some(self.command(stamped)),
Ok(None) => {
self.closed = true;
Some(RemoteCommand::DISABLED)
}
Err(_) => {
self.timed_out = true;
Some(RemoteCommand::DISABLED)
}
}
}
}
#[cfg(test)]
mod tests {
use dr16::sample_channel;
use dr16_protocol::ExtendedInput;
use super::*;
fn sample(channels: [i16; 4], s1: SwitchPosition) -> Dr16Sample {
Dr16Sample { channels, s1, s2: SwitchPosition::Mid, extended: ExtendedInput::default() }
}
fn mapper() -> RemoteMapper {
RemoteMapper::new(&RemoteConfig::default(), 3.0, 2.0, 4.0)
}
#[test]
fn maps_left_stick_to_forward_and_rotation_and_right_stick_to_strafe() {
let command = mapper().map(&sample([660, 0, 660, 660], SwitchPosition::Up));
assert!(command.enabled);
assert!((command.twist.vx - 3.0).abs() < 1e-6);
assert!((command.twist.vy + 2.0).abs() < 1e-6);
assert!((command.twist.omega + 4.0).abs() < 1e-6);
}
#[test]
fn applies_deadband_square_curve_and_inversions() {
let config = RemoteConfig {
invert_vx: true,
invert_vy: true,
invert_omega: true,
..RemoteConfig::default()
};
let mapper = RemoteMapper::new(&config, 1.0, 1.0, 1.0);
assert!(mapper.map(&sample([0, 0, 33, 0], SwitchPosition::Up)).twist.vx.abs() < 1e-6);
let command = mapper.map(&sample([660, 0, 660, 660], SwitchPosition::Up));
assert!((command.twist.vx + 1.0).abs() < 1e-6);
assert!((command.twist.vy - 1.0).abs() < 1e-6);
assert!((command.twist.omega - 1.0).abs() < 1e-6);
}
#[test]
fn s1_must_be_up_and_bad_switch_is_disabled() {
for position in [SwitchPosition::Down, SwitchPosition::Mid] {
assert!(!mapper().map(&sample([660, 660, 660, 660], position)).enabled);
}
}
#[tokio::test(start_paused = true)]
async fn timeout_publishes_disabled_once_and_recovers() {
let (sender, samples) = sample_channel();
let mut source = Dr16CommandSource::new(samples, mapper(), Duration::from_millis(100));
let _ = sender.send(sample([660, 0, 0, 0], SwitchPosition::Up));
assert!(source.next_command().await.unwrap().enabled);
let disabled = source.next_command().await.unwrap();
assert!(!disabled.enabled);
assert_eq!(disabled.effective_twist(), Twist::ZERO);
let _ = sender.send(sample([0, 0, 0, 0], SwitchPosition::Up));
assert!(source.next_command().await.unwrap().enabled);
}
#[tokio::test(start_paused = true)]
async fn closed_stream_disables_immediately() {
let (sender, samples) = sample_channel();
drop(sender);
let mut source = Dr16CommandSource::new(samples, mapper(), Duration::from_millis(100));
let command = source.next_command().await.unwrap();
assert!(!command.enabled);
assert_eq!(command.effective_twist(), Twist::ZERO);
}
}
}
pub mod telemetry {
//! 诊断输出:控制周期抖动统计(ARCHITECTURE.md §7.2)。
//!
@@ -569,11 +791,52 @@ pub mod controller {
use tokio::time::{Instant, MissedTickBehavior, interval};
use tracing::{info, warn};
use crate::RemoteCommand;
/// 逐轴速度斜坡。
#[derive(Debug, Clone, Copy)]
struct TwistRamp {
current: Twist,
step_vx: f32,
step_vy: f32,
step_omega: f32,
}
impl TwistRamp {
fn new(limits: (f32, f32, f32), period: Duration, ramp: Duration) -> Self {
let ratio = (period.as_secs_f32() / ramp.as_secs_f32()).clamp(0.0, 1.0);
Self {
current: Twist::ZERO,
step_vx: limits.0 * ratio,
step_vy: limits.1 * ratio,
step_omega: limits.2 * ratio,
}
}
fn advance(&mut self, target: Twist) -> Twist {
self.current = Twist {
vx: approach(self.current.vx, target.vx, self.step_vx),
vy: approach(self.current.vy, target.vy, self.step_vy),
omega: approach(self.current.omega, target.omega, self.step_omega),
};
self.current
}
fn reset(&mut self) {
self.current = Twist::ZERO;
}
}
fn approach(current: f32, target: f32, maximum_step: f32) -> f32 {
let delta = target - current;
if delta.abs() <= maximum_step { target } else { current + delta.signum() * maximum_step }
}
/// 底盘控制器。
///
/// 职责(1kHz 节拍):
/// 1. 读取最新 twist 指令,**指令超时则归零**;
/// 2. 速度限幅(保持方向按比例收缩);
/// 1. 读取最新 [`RemoteCommand`],超时或失能时立即归零;
/// 2. 速度限幅(保持方向按比例收缩),再逐轴斜坡平滑;
/// 3. **上电门控**:四台电机都收到过反馈才下发速度,否则全部停用;
/// 4. 麦轮逆解 → 各轮目标转速(rpm,轮端)→ 经 [`OutputShaft`] 换算到转子。
///
@@ -585,6 +848,7 @@ pub mod controller {
/// (max_vx, max_vy, max_omega)。
limits: (f32, f32, f32),
command_timeout: Duration,
ramp_time: Duration,
}
impl<W> ChassisController<W>
@@ -598,19 +862,25 @@ pub mod controller {
wheels: [W; 4],
limits: (f32, f32, f32),
command_timeout: Duration,
ramp_time: Duration,
) -> Self {
Self { kinematics, wheels, limits, command_timeout }
Self { kinematics, wheels, limits, command_timeout, ramp_time }
}
/// 启动控制循环任务。
pub fn spawn(self, commands: watch::Receiver<Twist>, period: Duration) -> JoinHandle<()> {
pub fn spawn(
self,
commands: watch::Receiver<RemoteCommand>,
period: Duration,
) -> JoinHandle<()> {
tokio::spawn(self.run(commands, period))
}
async fn run(self, mut commands: watch::Receiver<Twist>, period: Duration) {
async fn run(self, mut commands: watch::Receiver<RemoteCommand>, period: Duration) {
let mut ticker = interval(period);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
let mut latest = Twist::ZERO;
let mut ramp = TwistRamp::new(self.limits, period, self.ramp_time);
let mut latest = RemoteCommand::DISABLED;
let mut last_command_at: Option<Instant> = None;
let mut ticks: u64 = 0;
@@ -627,13 +897,21 @@ pub mod controller {
// 尚未收到任何指令:保持静止。
None => true,
};
let twist = if timed_out {
let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback());
let stop = timed_out || !latest.enabled || !all_seen_feedback;
let twist = if stop {
ramp.reset();
Twist::ZERO
} else {
latest.clamped(self.limits.0, self.limits.1, self.limits.2)
let target = latest.effective_twist().clamped(
self.limits.0,
self.limits.1,
self.limits.2,
);
ramp.advance(target)
};
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) {
@@ -696,6 +974,46 @@ pub mod controller {
m
}
#[test]
fn ramp_reaches_full_scale_in_requested_time_and_resets() {
let mut ramp = TwistRamp::new(
(2.0, 2.0, 2.0),
Duration::from_millis(1),
Duration::from_millis(500),
);
let target = Twist { vx: 2.0, ..Twist::ZERO };
for _ in 0..250 {
ramp.advance(target);
}
assert!((ramp.current.vx - 1.0).abs() < 1e-5);
for _ in 0..250 {
ramp.advance(target);
}
assert!((ramp.current.vx - target.vx).abs() < 1e-4);
ramp.reset();
assert_eq!(ramp.current, Twist::ZERO);
}
#[test]
fn ramp_reverses_through_zero_without_overshoot() {
let mut ramp = TwistRamp::new(
(1.0, 1.0, 1.0),
Duration::from_millis(1),
Duration::from_millis(10),
);
for _ in 0..10 {
ramp.advance(Twist { vy: 1.0, ..Twist::ZERO });
}
let mut crossed_zero = false;
for _ in 0..20 {
let output = ramp.advance(Twist { vy: -1.0, ..Twist::ZERO });
assert!(output.vy <= 1.0 + 1e-6 && output.vy >= -1.0 - 1e-6);
crossed_zero |= output.vy.abs() < 1e-6;
}
assert!(crossed_zero);
assert!((ramp.current.vy + 1.0).abs() < 1e-5);
}
/// 证明 `ChassisController` 只依赖能力 trait:换 `MockMotor` 原样工作。
#[tokio::test(start_paused = true)]
async fn controller_drives_wheels_through_capability_traits() {
@@ -706,12 +1024,13 @@ pub mod controller {
wrap_wheels(mocks.clone(), [GEAR; 4]),
(3.0, 3.0, 6.0),
Duration::from_millis(200),
Duration::from_millis(1),
);
let (tx, rx) = watch::channel(Twist::ZERO);
let (tx, rx) = watch::channel(RemoteCommand::DISABLED);
let handle = ctrl.spawn(rx, Duration::from_millis(1));
let twist = Twist { vx: 1.0, ..Twist::ZERO };
tx.send_replace(twist);
tx.send_replace(RemoteCommand::new(twist));
tokio::time::sleep(Duration::from_millis(5)).await;
let expected = kinematics.wheel_rpms(&twist);
@@ -721,7 +1040,7 @@ pub mod controller {
let want = expected[i] * GEAR;
assert!(
(rotor_rpm - want).abs() < 1e-2,
"wheel {i}: rotor_rpm {rotor_rpm} != {want}"
"wheel {i}: {rotor_rpm} != {want}"
);
}
ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
@@ -736,9 +1055,7 @@ pub mod controller {
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 missing = MockMotor::new(2);
let wheels = wrap_wheels(
[ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)],
[GEAR; 4],
@@ -748,11 +1065,12 @@ pub mod controller {
wheels,
(3.0, 3.0, 6.0),
Duration::from_millis(200),
Duration::from_millis(500),
);
let (tx, rx) = watch::channel(Twist::ZERO);
let (tx, rx) = watch::channel(RemoteCommand::DISABLED);
let handle = ctrl.spawn(rx, Duration::from_millis(1));
tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
tx.send_replace(RemoteCommand::new(Twist { vx: 1.0, ..Twist::ZERO }));
tokio::time::sleep(Duration::from_millis(5)).await;
assert_eq!(ready.last_command(), Command::Disabled);
@@ -760,6 +1078,29 @@ pub mod controller {
handle.abort();
}
#[tokio::test(start_paused = true)]
async fn disabled_command_stops_immediately_without_ramping() {
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),
Duration::from_millis(500),
);
let (tx, rx) = watch::channel(RemoteCommand::DISABLED);
let handle = ctrl.spawn(rx, Duration::from_millis(1));
tx.send_replace(RemoteCommand::new(Twist { vx: 3.0, ..Twist::ZERO }));
tokio::time::sleep(Duration::from_millis(200)).await;
tx.send_replace(RemoteCommand { enabled: false, ..RemoteCommand::STOP });
tokio::time::sleep(Duration::from_millis(2)).await;
for mock in mocks {
assert_eq!(mock.last_command(), Command::Velocity(0.0));
}
handle.abort();
}
}
}
@@ -771,11 +1112,11 @@ use std::process::ExitCode;
use std::time::Duration;
use clap::Parser;
use mecanum::Twist;
use tokio::sync::watch;
use tracing::{error, info};
use tracing_subscriber::EnvFilter;
use crate::remote::{Dr16CommandSource, RemoteMapper};
use crate::{ChassisController, wrap_wheels};
use can::CanLink;
use chassis_config::Config;
@@ -802,6 +1143,10 @@ struct Args {
/// 只装配配置、链路、总线和控制任务,短跑后安全退出。
#[arg(long)]
dry_run: bool,
/// 覆盖配置文件中的 DR16 串口设备(如 `/dev/ttyS2`)。
#[arg(long)]
rc_device: Option<String>,
}
fn main() -> ExitCode {
@@ -811,13 +1156,18 @@ fn main() -> ExitCode {
let args = Args::parse();
let config = match load_config(&args) {
let mut config = match load_config(&args) {
Ok(config) => config,
Err(e) => {
error!("{e}");
return ExitCode::FAILURE;
}
};
if let Some(device) = args.rc_device.clone() {
let mut remote = config.remote.take().unwrap_or_default();
remote.device = device;
config.remote = Some(remote);
}
let interface = args.interface.clone().unwrap_or_else(|| config.can.interface.clone());
let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() {
@@ -856,6 +1206,25 @@ async fn run(
dry_run: bool,
) -> Result<(), Box<dyn std::error::Error>> {
let bus_options = config.bus_options();
let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega);
let remote_source = if mock {
info!("mock 模式不打开 DR16 串口,遥控输入保持失能");
None
} else {
match config.remote.clone() {
Some(remote_config) => {
let samples = dr16::open(&remote_config.device)?;
let mapper = RemoteMapper::new(&remote_config, limits.0, limits.1, limits.2);
let source = Dr16CommandSource::new(
samples,
mapper,
Duration::from_millis(remote_config.timeout_ms),
);
Some((source, remote_config.ramp_ms))
}
None => None,
}
};
// 1) CAN 链路:仿真或真实 SocketCAN。
let link = if mock {
@@ -884,10 +1253,8 @@ async fn run(
"chassisd 启动"
);
// 3) 控制循环(当前无指令源:watch 保持 ZERO,安全静止;
// 遥控 / 自主导航后续经 chassis_config::source 接入)。
let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega);
// 3) 控制循环与遥控指令源。
let (command_tx, command_rx) = watch::channel(crate::RemoteCommand::DISABLED);
let motors: [EscMotor; 4] =
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let gears: [f32; 4] = config
@@ -902,11 +1269,16 @@ async fn run(
wrap_wheels(motors, gears),
limits,
Duration::from_millis(config.chassis.command_timeout_ms),
Duration::from_millis(remote_source.as_ref().map_or(500, |(_, ramp_ms)| *ramp_ms)),
)
.spawn(twist_rx, period);
let _twist_tx = twist_tx; // 保留发送端供后续指令源使用
.spawn(command_rx, period);
let remote_task = remote_source
.map(|(source, _)| tokio::spawn(crate::source::pump_source(source, command_tx.clone())));
if dry_run {
if let Some(task) = remote_task {
task.abort();
}
tokio::time::sleep(Duration::from_millis(20)).await;
for motor in bus.motors() {
motor.disable();
@@ -927,6 +1299,9 @@ async fn run(
}
// 5) 停车:显式停用电机并等若干节拍把 0 电流帧刷入总线。
if let Some(task) = remote_task {
task.abort();
}
for motor in bus.motors() {
motor.disable();
}
@@ -953,6 +1328,7 @@ fn open_real_link(interface: &str) -> Result<CanLink, Box<dyn std::error::Error>
mod daemon_integration {
use super::*;
use esc_bus::EscBusHandle;
use mecanum::Twist;
use motor::MotorFeedback;
use sim_harness::spawn_plant;
use tokio::time::sleep;
@@ -964,7 +1340,7 @@ mod daemon_integration {
fn stack(
config: &Config,
command_timeout_ms: u64,
) -> (EscBusHandle<MotorPolicy>, watch::Sender<Twist>) {
) -> (EscBusHandle<MotorPolicy>, watch::Sender<RemoteCommand>) {
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);
@@ -976,7 +1352,7 @@ mod daemon_integration {
let bus =
EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options(), policy).unwrap();
let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
let (twist_tx, twist_rx) = watch::channel(RemoteCommand::DISABLED);
let motors: [EscMotor; 4] =
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let gears: [f32; 4] = config
@@ -991,6 +1367,7 @@ mod daemon_integration {
wrap_wheels(motors, gears),
(config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
Duration::from_millis(command_timeout_ms),
Duration::from_millis(2),
);
controller.spawn(twist_rx, config.bus_options().period);
(bus, twist_tx)
@@ -1025,7 +1402,7 @@ mod daemon_integration {
let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
twist_tx.send_replace(RemoteCommand::new(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)
@@ -1035,7 +1412,7 @@ mod daemon_integration {
assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
// 停车:指令归零后各轮减速到停。
twist_tx.send_replace(Twist::ZERO);
twist_tx.send_replace(RemoteCommand::STOP);
let stopped =
wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
let rpm = wheel_rpms(&bus);
@@ -1048,7 +1425,7 @@ mod daemon_integration {
let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
twist_tx.send_replace(RemoteCommand::new(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, || {
@@ -1076,7 +1453,7 @@ mod daemon_integration {
let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
// 只发一次指令,随后沉默 → 超过 command_timeout(200ms)后应停车。
twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
twist_tx.send_replace(RemoteCommand::new(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;
@@ -1096,11 +1473,11 @@ mod daemon_integration {
config.chassis.max_vx = 1.0; // 限速 1 m/s
let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist {
twist_tx.send_replace(RemoteCommand::new(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;
@@ -16,6 +16,8 @@ esc-bus = { workspace = true }
experiment = { workspace = true }
motor-control = { workspace = true }
mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
slcan = { workspace = true }
socketcan = { workspace = true }
tokio = { workspace = true }
@@ -25,3 +27,6 @@ tracing-subscriber = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "application/tools"
@@ -574,6 +574,93 @@ mod chassis {
}
}
mod rc_monitor {
//! 只读 DR16 遥控监视器:不打开 CAN,不驱动任何电机。
use std::process::ExitCode;
use std::time::{Duration, Instant};
use clap::Parser;
use dr16::open;
use dr16_protocol::SwitchPosition;
#[derive(Debug, Parser)]
#[command(name = "spin-rc", version, about = "只读监视 DR16/D-BUS 遥控输入")]
struct Args {
/// DR16 D-BUS UART 设备。
#[arg(long, default_value = "/dev/ttyS2")]
device: String,
/// 显示打印周期(毫秒)。
#[arg(long, default_value_t = 100)]
report_ms: u64,
/// 映射显示用死区。
#[arg(long, default_value_t = 0.05)]
deadband: f32,
}
fn shaped(raw: i16, deadband: f32) -> f32 {
let normalized = (f32::from(raw) / 660.0).clamp(-1.0, 1.0);
let magnitude = normalized.abs();
if magnitude <= deadband {
return 0.0;
}
normalized.signum() * ((magnitude - deadband) / (1.0 - deadband)).powi(2)
}
pub async fn main_with_args(parameters: Vec<String>) -> ExitCode {
let args = Args::parse_from(std::iter::once(String::from("spin-rc")).chain(parameters));
let stream = match open(&args.device) {
Ok(stream) => stream,
Err(error) => {
eprintln!("✗ 打开 DR16 串口 {} 失败:{error}", args.device);
return ExitCode::FAILURE;
}
};
println!("=== DR16/D-BUS 只读监视({})===", args.device);
println!("不接触 CAN / 电机;ctrl-c 退出。满量程映射:vx=3.0 vy=3.0 omega=6.0(仅显示)\n");
let mut report = tokio::time::interval(Duration::from_millis(args.report_ms.max(1)));
report.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
loop {
tokio::select! {
_ = tokio::signal::ctrl_c() => {
println!("\n已退出");
return ExitCode::SUCCESS;
}
_ = report.tick() => {
let Some(stamped) = stream.latest() else {
println!("等待 DR16 帧……");
continue;
};
let sample = stamped.sample;
let age = Instant::now().saturating_duration_since(stamped.received_at);
let vx = shaped(sample.channel_t(), args.deadband) * 3.0;
let vy = -shaped(sample.channel_a(), args.deadband) * 3.0;
let omega = -shaped(sample.channel_r(), args.deadband) * 6.0;
println!(
"age {:>4}ms | A {:>4} E {:>4} T {:>4} R {:>4} | S1 {:?}({}) S2 {:?}({}) | vx {:+.2} vy {:+.2} omega {:+.2}",
age.as_millis(),
sample.channel_a(),
sample.channel_e(),
sample.channel_t(),
sample.channel_r(),
sample.s1,
sample.s1.wire_value(),
sample.s2,
sample.s2.wire_value(),
vx,
vy,
omega,
);
if sample.s1 != SwitchPosition::Up {
println!(" S1 非上档:底盘使能关闭");
}
}
}
}
}
}
mod motor_one {
//! 单电机验证工具:让一个 C620 / GM3508 转起来,验证电机抽象全链路。
//!
@@ -710,12 +797,13 @@ async fn main() -> std::process::ExitCode {
motor_one::main_with_args(args.collect()).await;
std::process::ExitCode::SUCCESS
}
Some("rc") => rc_monitor::main_with_args(args.collect()).await,
Some("--help" | "-h") => {
println!("usage: spin <chassis|motor> [options]");
println!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::SUCCESS
}
_ => {
eprintln!("usage: spin <chassis|motor> [options]");
eprintln!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::from(2)
}
}
@@ -45,3 +45,6 @@ plotters = { workspace = true, optional = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/tuning"
@@ -122,7 +122,7 @@ pub mod report {
#[cfg(feature = "plot")]
if let Some(trace) = &ctx.best_trace {
let svg_path = ctx.out_dir.join("best.svg");
crate::tune::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?;
crate::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?;
written.push(svg_path);
}
@@ -932,14 +932,15 @@ use esc_bus::EscConfig;
use experiment::SetpointProfile;
use indicatif::ProgressBar;
use motor_control::VelocityGains;
use motor_model::Gm3508Model;
use motor_protocol::EscModel;
use serde::Serialize;
use sim_harness::{LoopLimits, PlantEnsemble};
use tuning::{
AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH,
Evaluator, Objective, PlantSpec, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, ScaleSpec,
ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict, parse_axis,
sweep,
Evaluator, Objective, PlantProfile, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked,
ScaleSpec, ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict,
parse_axis, sweep,
};
/// 子命令。
@@ -1121,7 +1122,7 @@ struct CommonArgs {
/// 仿真后端参数(覆盖预设)。
#[derive(Debug, Clone, ClapArgs)]
struct SimArgs {
/// 被控对象模型(内置默认 `gm3508-bench`)
/// 被控对象模型(内置默认 `bench`)
#[arg(long, value_enum)]
plant: Option<PlantArg>,
/// 集成成员数(域随机化;1 = 只用名义模型;内置默认 8)
@@ -1268,16 +1269,16 @@ impl From<SolverArg> for SolverSpec {
#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)]
enum PlantArg {
/// 理想一阶模型(无摩擦、无延迟)
Gm3508,
Ideal,
/// 台架量级:静 / 库仑摩擦 + 2 拍延迟
Gm3508Bench,
Bench,
}
impl From<PlantArg> for PlantSpec {
impl From<PlantArg> for PlantProfile {
fn from(value: PlantArg) -> Self {
match value {
PlantArg::Gm3508 => Self::Gm3508,
PlantArg::Gm3508Bench => Self::Gm3508Bench,
PlantArg::Ideal => Self::Ideal,
PlantArg::Bench => Self::Bench,
}
}
}
@@ -1411,7 +1412,9 @@ impl CommonArgs {
let source =
if prefix.is_empty() { "内置默认".to_owned() } else { format!("{prefix}{name}") };
let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{source}' 失败"))?;
let tuning = merged
.to_tuning::<Gm3508Model>()
.with_context(|| format!("解算预设 '{source}' 失败"))?;
let envelope = merged
.bench_envelope()
.with_context(|| format!("解算预设 '{source}' 的安全包络失败"))?;
@@ -1922,7 +1925,8 @@ fn presets_cmd(args: &PresetsArgs) -> Result<ExitCode> {
println!("共 {} 个预设", file.preset.len());
for (name, preset) in &file.preset {
let merged = Preset::builtin().overlay(preset);
let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{name}' 失败"))?;
let tuning =
merged.to_tuning::<Gm3508Model>().with_context(|| format!("解算预设 '{name}' 失败"))?;
let envelope =
merged.bench_envelope().with_context(|| format!("解算预设 '{name}' 的安全包络失败"))?;
let mark =
@@ -2369,7 +2373,6 @@ fn open_bench(
let mut esc = EscConfig::gm3508(hardware.name.clone(), hardware.esc_id, hardware.direction);
esc.gear_ratio = hardware.gear_ratio;
esc.current_limit = limits.current_limit_raw;
esc.model = limits.model;
let envelope = SafetyEnvelope::new()
.with_overspeed(envelope_spec.max_rpm, Duration::from_millis(60))
@@ -2452,7 +2455,14 @@ feedforward = 0.0
/// 仓库里随代码一起评审的预设文件。
fn repo_preset_path() -> PathBuf {
Path::new(env!("CARGO_MANIFEST_DIR")).join("../../config/tune.toml")
let mut directory = Path::new(env!("CARGO_MANIFEST_DIR"));
loop {
let candidate = directory.join("config/tune.toml");
if candidate.exists() {
return candidate;
}
directory = directory.parent().expect("workspace root must exist");
}
}
fn args_with(extra: &[&str]) -> CommonArgs {
@@ -2573,7 +2583,7 @@ feedforward = 0.0
assert_eq!(tuning.search.seed, 20_260_101);
assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6);
assert!((tuning.acceptance.max_overshoot_pct - 20.0).abs() < 1e-6);
assert_eq!(tuning.plant_spec, PlantSpec::Gm3508Bench);
assert_eq!(tuning.plant_spec, PlantProfile::Bench);
assert_eq!(settings.envelope.validate_runs, 3);
}
@@ -2722,7 +2732,7 @@ samples = 99
for (name, preset) in &file.preset {
let merged = Preset::builtin().overlay(preset);
let tuning = merged
.to_tuning()
.to_tuning::<Gm3508Model>()
.unwrap_or_else(|error| panic!("预设 '{name}' 无法解算:{error}"));
merged
.bench_envelope()
@@ -10,6 +10,7 @@ license.workspace = true
workspace = true
[dependencies]
motor = { workspace = true }
motor-control = { workspace = true }
mecanum = { workspace = true }
esc-bus = { workspace = true }
@@ -20,3 +21,6 @@ thiserror = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "config/chassis"
@@ -12,6 +12,7 @@ use std::time::Duration;
use esc_bus::{BusOptions, EscConfig};
use mecanum::Mecanum;
use motor::Gm3508;
use motor_control::VelocityGains;
use motor_protocol::EscModel;
use serde::{Deserialize, Serialize};
@@ -37,6 +38,9 @@ pub struct Config {
/// 底盘控制参数。
#[serde(default)]
pub chassis: ChassisConfig,
/// 可选遥控输入;缺省时守护进程保持安全静止。
#[serde(default)]
pub remote: Option<RemoteConfig>,
/// 麦轮几何。
pub mecanum: MecanumConfig,
/// 四电机配置,顺序固定:前左、前右、后左、后右。
@@ -128,6 +132,69 @@ impl Default for ChassisConfig {
}
}
/// 遥控输入类型。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)]
#[serde(rename_all = "lowercase")]
pub enum RemoteKind {
/// DJI DR16 接收机的 D-BUS 串口。
Dr16,
}
/// 遥控输入配置。
#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
pub struct RemoteConfig {
/// 接收机协议。
pub kind: RemoteKind,
/// T153 UART 设备节点。
pub device: String,
/// 合法遥控帧断流超时(毫秒)。
#[serde(default = "default_remote_timeout")]
pub timeout_ms: u64,
/// 摇杆死区,按满量程比例计。
#[serde(default = "default_remote_deadband")]
pub deadband: f32,
/// 从零到满量程的速度斜坡时间(毫秒)。
#[serde(default = "default_remote_ramp")]
pub ramp_ms: u64,
/// 反转前后方向。
#[serde(default)]
pub invert_vx: bool,
/// 反转左右方向。
#[serde(default)]
pub invert_vy: bool,
/// 反转旋转方向。
#[serde(default)]
pub invert_omega: bool,
}
impl Default for RemoteConfig {
fn default() -> Self {
Self {
kind: RemoteKind::Dr16,
device: "/dev/ttyS2".into(),
timeout_ms: default_remote_timeout(),
deadband: default_remote_deadband(),
ramp_ms: default_remote_ramp(),
invert_vx: false,
invert_vy: false,
invert_omega: false,
}
}
}
fn default_remote_timeout() -> u64 {
100
}
fn default_remote_deadband() -> f32 {
0.05
}
fn default_remote_ramp() -> u64 {
500
}
/// 麦轮几何(米)。
#[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)]
@@ -341,6 +408,23 @@ impl Config {
if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) {
return Err(ConfigError::Invalid("speed limits must be > 0".into()));
}
if let Some(remote) = &self.remote {
if remote.device.trim().is_empty() {
return Err(ConfigError::Invalid("remote.device must not be empty".into()));
}
if remote.timeout_ms == 0 || remote.ramp_ms == 0 {
return Err(ConfigError::Invalid("remote timeout/ramp must be > 0 ms".into()));
}
if !remote.deadband.is_finite() || !(0.0..0.5).contains(&remote.deadband) {
return Err(ConfigError::Invalid("remote.deadband must be in [0, 0.5)".into()));
}
if remote.timeout_ms > c.command_timeout_ms {
return Err(ConfigError::Invalid(format!(
"remote.timeout_ms ({}) must be <= command_timeout_ms ({})",
remote.timeout_ms, c.command_timeout_ms
)));
}
}
for motor in &self.motors {
let gear_ratio_valid = motor.gear_ratio > 0.0;
if !gear_ratio_valid {
@@ -390,6 +474,7 @@ impl Config {
Self {
can: CanConfig { interface: "vcan0".into(), function: None },
chassis: ChassisConfig::default(),
remote: None,
mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 },
motors: vec![
motor("front_left", 1, 1.0),
@@ -402,16 +487,14 @@ impl Config {
/// 转换为 `EscBus` 配置(顺序 FL、FR、RL、RR)。
#[must_use]
pub fn to_esc_configs(&self) -> Vec<EscConfig> {
pub fn to_esc_configs(&self) -> Vec<EscConfig<Gm3508>> {
self.motors
.iter()
.map(|m| EscConfig {
name: m.name.clone(),
esc_id: m.esc_id,
model: m.model.into(),
gear_ratio: m.gear_ratio,
direction: m.direction,
current_limit: m.current_limit,
.map(|m| {
let mut cfg = EscConfig::<Gm3508>::gm3508(m.name.clone(), m.esc_id, m.direction);
cfg.gear_ratio = m.gear_ratio;
cfg.current_limit = m.current_limit;
cfg
})
.collect()
}
@@ -441,6 +524,16 @@ mod tests {
[can]
interface = "can0"
[remote]
kind = "dr16"
device = "/dev/ttyS2"
timeout_ms = 100
deadband = 0.05
ramp_ms = 500
invert_vx = false
invert_vy = false
invert_omega = false
[chassis]
control_period_ms = 1
command_timeout_ms = 200
@@ -497,6 +590,10 @@ feedforward = 9.0
fn parses_example() {
let cfg = Config::from_toml(EXAMPLE).unwrap();
assert_eq!(cfg.can.interface, "can0");
let remote = cfg.remote.as_ref().unwrap();
assert_eq!(remote.kind, RemoteKind::Dr16);
assert_eq!(remote.device, "/dev/ttyS2");
assert_eq!(remote.timeout_ms, 100);
assert_eq!(cfg.motors.len(), 4);
assert_eq!(cfg.motors[0].name, "front_left");
assert_eq!(cfg.motors[3].esc_id, 4);
@@ -536,6 +633,7 @@ gear_ratio = 19.2
)
.unwrap();
assert_eq!(cfg.chassis, ChassisConfig::default());
assert!(cfg.remote.is_none());
assert_eq!(cfg.motors[0].direction.to_bits(), 1.0f32.to_bits());
assert_eq!(cfg.motors[0].current_limit, 12_000);
assert_eq!(cfg.motors[0].model, EscModelSerde::C620);
@@ -611,6 +709,7 @@ gear_ratio = 19.2
let reparsed = Config::from_toml(&text).unwrap();
assert_eq!(reparsed, original);
// 落盘文本必须仍然是合法配置(守护进程直接读它)。
assert!(text.contains("[remote]"), "{text}");
assert!(text.contains("[[motor]]"), "{text}");
assert!(text.contains("interface = \"can0\""), "{text}");
}
@@ -620,9 +719,11 @@ gear_ratio = 19.2
let mut original = Config::mock_default();
original.can.function = Some(FunctionBusConfig { interface: "can1".into() });
original.chassis.motor_command_timeout_ms = 37;
original.remote = Some(RemoteConfig { invert_omega: true, ..RemoteConfig::default() });
let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap();
assert_eq!(reparsed, original);
assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37);
assert!(reparsed.remote.unwrap().invert_omega);
assert_eq!(reparsed.can.function.unwrap().interface, "can1");
}
@@ -666,6 +767,19 @@ gear_ratio = 19.2
));
}
#[test]
fn remote_validation_rejects_unsafe_values() {
let mut cfg = Config::mock_default();
cfg.remote = Some(RemoteConfig { timeout_ms: 0, ..RemoteConfig::default() });
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
cfg.remote = Some(RemoteConfig { deadband: 0.5, ..RemoteConfig::default() });
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
cfg.remote = Some(RemoteConfig { timeout_ms: 201, ..RemoteConfig::default() });
assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_))));
}
#[test]
fn save_and_load_file_round_trip() {
let dir = std::env::temp_dir().join("chassis-config-roundtrip");
@@ -17,3 +17,6 @@ tokio = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "contract/can"
@@ -11,3 +11,6 @@ workspace = true
[dependencies]
[package.metadata.sentry-chassis]
layer = "contract/imu"
@@ -14,3 +14,6 @@ workspace = true
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/algorithm"
@@ -15,3 +15,6 @@ workspace = true
[dev-dependencies]
proptest = { workspace = true }
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/chassis"
@@ -15,3 +15,6 @@ motor = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/motor"
@@ -18,9 +18,8 @@ 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 }
[package.metadata.sentry-chassis]
layer = "device/esc"
@@ -1,132 +1,96 @@
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
//! `EscBus` 异步驱动:协议状态机与 1kHz 控制节拍。
//!
//! [`EscBus::spawn`] 消费一个 [`CanLink`],启动收发两个任务:
//!
//! - 接收:解码反馈帧,更新各电机共享状态(含多圈累计与方向修正);
//! - 发送:按 `period` 节拍为每台电机求值当前命令(速度/位置环 PID / 直发
//! effort),按 `0x200`/`0x1FF` 分组组帧发送。
//!
//! 电机离线(从未收到反馈或反馈超时)时强制 0 输出并复位控制器。
//! 命令模式切换时同样复位控制器,避免积分冲击。
//! Generic asynchronous ESC frame pump, state, leases, and safety.
use std::marker::PhantomData;
use std::sync::{Arc, Mutex, MutexGuard};
use std::time::Duration;
use can::{CanFrame, CanLink};
use motor_protocol::common::{
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
use motor::{
Command, CurrentEscDrive, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorSpec,
MotorState, PolicyInput, PolicySample, PositionDrive, VelocityDrive,
};
use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker};
use motor_protocol::{EffortScale, FeedbackFields, MotorProtocol, 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 motor::{
Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput,
PolicySample, PositionDrive, VelocityDrive,
};
/// 锁辅助:中毒锁直接恢复内部数据(锁中毒只意味着持锁线程恐慌,
/// 状态本身仍一致,驱动机不应因此停摆)。
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
}
/// 单台电机的配置。
/// Configuration for one typed motor.
#[derive(Debug, Clone)]
pub struct EscConfig {
/// 便于日志辨认的名字(如 `front_left`)。
pub struct EscConfig<S: MotorSpec = motor::Gm3508> {
pub name: String,
/// 电调 ID(1–8,即 CAN 拨码)。
pub esc_id: u8,
/// 电调型号。
pub model: EscModel,
/// 减速比(电机转子 : 输出轴)。
///
/// 驱动层**不使用**此字段;由上层 `crate::OutputShaft` 应用。
/// 保留于此作为配置元数据,`chassisd` 包装轮子时必须读取本字段(单一来源)。
pub gear_ratio: f32,
/// 安装方向:`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转,
/// 上层永远以"输出轴正转 = 前进方向"工作。
pub direction: f32,
/// effort 限幅(raw,`1..=型号满量程`)。
pub current_limit: i16,
marker: PhantomData<S>,
}
impl EscConfig {
/// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值,需按整车整定。
impl<S: MotorSpec> EscConfig<S> {
#[must_use]
pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
pub fn new(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
Self {
name: name.into(),
esc_id,
model: EscModel::C620,
// 3591 / 187 ≈ 19.203
gear_ratio: 3591.0 / 187.0,
gear_ratio: S::GEAR_RATIO,
direction,
current_limit: 12_000,
}
}
/// M2006 + C610(拨弹轮)的常用配置。
///
/// 减速比按 P36 起步(36.0),接硬件前需核对;电流标度见 [`EscModel::C610`]。
#[must_use]
pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
Self {
name: name.into(),
esc_id,
model: EscModel::C610,
// M2006 P36 减速比(接入前核对)
gear_ratio: 36.0,
direction,
current_limit: 8_000,
current_limit: S::scale().max_raw() / 4 * 3,
marker: PhantomData,
}
}
fn validate(&self) -> Result<(), String> {
if command_frame_id(self.esc_id).is_none() {
if <S::Protocol as MotorProtocol>::command_frame_id(self.esc_id).is_none() {
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
}
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
}
// 逐位比较:direction 只允许精确的 ±1.0。
let dir_bits = self.direction.to_bits();
if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() {
return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction));
}
if !(1..=self.model.max_current_raw()).contains(&self.current_limit) {
if !(1..=S::scale().max_raw()).contains(&self.current_limit) {
return Err(format!(
"current_limit {} out of range 1..={} for {:?}",
"current_limit {} out of range 1..={}",
self.current_limit,
self.model.max_current_raw(),
self.model
S::scale().max_raw()
));
}
Ok(())
}
}
/// 总线运行参数。
impl EscConfig<motor::Gm3508> {
#[must_use]
pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
let mut cfg = Self::new(name, esc_id, direction);
cfg.current_limit = 12_000;
cfg
}
}
impl EscConfig<motor::M2006> {
#[must_use]
pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
let mut cfg = Self::new(name, esc_id, direction);
cfg.current_limit = 8_000;
cfg
}
}
/// Bus timing and lease options.
#[derive(Debug, Clone)]
pub struct BusOptions {
/// 控制节拍(C620 期望 1kHz 命令)。
pub period: Duration,
/// 反馈离线阈值:超过该时长未收到某电机反馈 → 强制 0 输出。
pub feedback_timeout: Duration,
/// **命令租约**:上层超过该时长未重新下发命令 → 视为 `Disabled`(0 输出)。
///
/// 命令是**租约**而非一次性设定值:`set_velocity` / `set_effort_raw` /
/// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
/// (`chassisd` 的 1kHz 循环天然满足;台架脚本每拍重发即可),
/// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
///
/// 设置为 `Duration::MAX` 可退化为纯设定值语义(仅建议在无失控风险的台架场景)。
pub command_timeout: Duration,
}
@@ -135,45 +99,34 @@ impl Default for BusOptions {
Self {
period: Duration::from_millis(1),
feedback_timeout: Duration::from_millis(100),
// 1kHz 下约 50 拍未刷新即失效。
command_timeout: Duration::from_millis(50),
}
}
}
/// 启动错误。
/// Startup error.
#[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum EscBusError {
/// 两台电机配置了同一电调 ID。
#[error("duplicate ESC id {0}")]
DuplicateEscId(u8),
/// 电机配置非法(原因见 `reason`)。
#[error("invalid config for motor {name}: {reason}")]
InvalidConfig {
/// 电机名。
name: String,
/// 非法原因。
reason: String,
},
/// 一台电机都没有配置。
InvalidConfig { name: String, reason: String },
#[error("no motors configured")]
NoMotors,
}
/// 单电机共享状态(驱动任务与 [`EscMotor`] 句柄之间)。
struct MotorShared {
cfg: EscConfig,
struct MotorShared<S: MotorSpec> {
cfg: EscConfig<S>,
state: Mutex<MotorState>,
cmd: Mutex<Command>,
/// 命令写入时刻(看门狗用)。
cmd_at: Mutex<Option<Instant>>,
turn: Mutex<TurnTracker>,
last_feedback: Mutex<Option<Instant>>,
offline_warned: std::sync::atomic::AtomicBool,
}
impl MotorShared {
fn new(cfg: EscConfig) -> Self {
impl<S: MotorSpec> MotorShared<S> {
fn new(cfg: EscConfig<S>) -> Self {
Self {
cfg,
state: Mutex::new(MotorState::default()),
@@ -190,24 +143,22 @@ impl MotorShared {
*lock(&self.cmd_at) = Some(Instant::now());
}
/// 应用一帧反馈(原始电调侧值 → 方向修正后的物理量)。
fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
let esc_rev = lock(&self.turn).update(fb.angle_raw);
fn apply_feedback<F: FeedbackFields>(&self, fb: &F, now: Instant) {
let esc_rev = lock(&self.turn).update(fb.angle_raw());
let dir = self.cfg.direction;
{
let mut st = lock(&self.state);
st.rotor_angle_raw = fb.angle_raw;
st.rotor_angle_raw = fb.angle_raw();
st.rotor_revolutions = dir * esc_rev;
st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm);
st.effort_raw = (dir * f32::from(fb.current_raw)) as i16;
st.temperature_c = fb.temperature_c;
st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm());
st.effort_raw = (dir * f32::from(fb.current_raw())) as i16;
st.temperature_c = fb.temperature_c();
st.feedback_count = st.feedback_count.saturating_add(1);
}
*lock(&self.last_feedback) = Some(now);
self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
}
/// Produce one policy sample while enforcing feedback and command leases.
fn sample(
&self,
feedback_timeout: Duration,
@@ -219,7 +170,7 @@ impl MotorShared {
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");
warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero output");
}
let fresh = matches!(
*lock(&self.cmd_at),
@@ -230,103 +181,87 @@ impl MotorShared {
}
}
/// 电机的公共句柄:可克隆,跨任务共享。
///
/// # 命令租约
///
/// 下发命令([`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
/// [`PositionDrive::set_position`])即续租,租期见 [`BusOptions::command_timeout`]。
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电(0 输出),
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
#[derive(Clone)]
pub struct EscMotor {
shared: Arc<MotorShared>,
/// Typed motor handle.
pub struct EscMotor<S: MotorSpec = motor::Gm3508> {
shared: Arc<MotorShared<S>>,
}
impl EscMotor {
/// 电机名。
impl<S: MotorSpec> Clone for EscMotor<S> {
fn clone(&self) -> Self {
Self { shared: Arc::clone(&self.shared) }
}
}
impl<S: MotorSpec> EscMotor<S> {
#[must_use]
pub fn config(&self) -> &EscConfig<S> {
&self.shared.cfg
}
#[must_use]
pub fn name(&self) -> &str {
&self.shared.cfg.name
}
/// 电机配置。
#[must_use]
pub fn config(&self) -> &EscConfig {
&self.shared.cfg
}
/// 当前命令。
#[must_use]
pub fn command(&self) -> Command {
*lock(&self.shared.cmd)
}
/// 最近一帧反馈的时刻(诊断用)。
#[must_use]
pub fn last_feedback(&self) -> Option<Instant> {
*lock(&self.shared.last_feedback)
}
}
impl MotorFeedback for EscMotor {
impl<S: MotorSpec> MotorFeedback for EscMotor<S> {
fn id(&self) -> u8 {
self.shared.cfg.esc_id
}
fn state(&self) -> MotorState {
*lock(&self.shared.state)
}
fn feedback_age(&self) -> Option<std::time::Duration> {
let t = *lock(&self.shared.last_feedback);
Some(Instant::now().saturating_duration_since(t?))
fn feedback_age(&self) -> Option<Duration> {
Some(Instant::now().saturating_duration_since(*lock(&self.shared.last_feedback).as_ref()?))
}
}
impl VelocityDrive for EscMotor {
/// 设定目标转子转速(**租约**,见 [`EscMotor`] 的命令租约说明)。
impl<S: MotorSpec> VelocityDrive for EscMotor<S> {
fn set_velocity(&self, rotor_rpm: f32) {
self.shared.store_command(Command::Velocity(rotor_rpm));
}
}
impl EffortDrive for EscMotor {
/// 直发开环 raw(**租约**,见 [`EscMotor`] 的命令租约说明)。
impl<S: MotorSpec> EffortDrive for EscMotor<S> {
fn set_effort_raw(&self, raw: i16) {
self.shared.store_command(Command::EffortRaw(raw));
}
}
impl PositionDrive for EscMotor {
/// 设定目标转子多圈位置(**租约**,见 [`EscMotor`] 的命令租约说明)。
impl<S: MotorSpec> PositionDrive for EscMotor<S> {
fn set_position(&self, rotor_rev: f32) {
self.shared.store_command(Command::Position(rotor_rev));
}
}
impl Disable for EscMotor {
/// 停用(0 输出);同样受租约保护——上层若已死,租约到期后依旧归零。
impl<S: MotorSpec> Disable for EscMotor<S> {
fn disable(&self) {
self.shared.store_command(Command::Disabled);
}
}
/// 运行中的电调总线。
pub struct EscBus;
/// Generic current-frame ESC bus.
pub struct EscBus<S: MotorSpec = motor::Gm3508>(PhantomData<S>);
impl EscBus {
/// 启动收发任务,返回句柄。
impl<S: CurrentEscDrive> EscBus<S> {
/// Start receive and transmit tasks.
///
/// # Errors
///
/// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
/// Returns an error when configuration is empty, duplicated, or invalid.
pub fn spawn<P: EffortPolicy>(
link: CanLink,
configs: Vec<EscConfig>,
configs: Vec<EscConfig<S>>,
options: BusOptions,
policy: P,
) -> Result<EscBusHandle<P>, EscBusError> {
) -> Result<EscBusHandle<P, S>, EscBusError> {
if configs.is_empty() {
return Err(EscBusError::NoMotors);
}
@@ -346,46 +281,48 @@ impl EscBus {
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(
tokio::spawn(rx_loop::<S>(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
let tx_handle = tokio::spawn(tx_loop::<S, P>(
outgoing_tx,
Arc::clone(&motors),
Arc::clone(&policy),
options,
shutdown_rx,
));
Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
Ok(EscBusHandle {
motors,
policy,
shutdown_tx,
tasks: vec![rx_handle, tx_handle],
marker: PhantomData,
})
}
}
/// `EscBus` 的控制句柄。
pub struct EscBusHandle<P> {
motors: Arc<Vec<EscMotor>>,
/// Control handle for a running bus.
pub struct EscBusHandle<P, S: MotorSpec = motor::Gm3508> {
motors: Arc<Vec<EscMotor<S>>>,
policy: Arc<Mutex<P>>,
shutdown_tx: watch::Sender<bool>,
tasks: Vec<JoinHandle<()>>,
marker: PhantomData<S>,
}
impl<P: EffortPolicy> EscBusHandle<P> {
/// 按电调 ID 取电机句柄。
impl<P: EffortPolicy, S: CurrentEscDrive> EscBusHandle<P, S> {
#[must_use]
pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> {
pub fn motor(&self, esc_id: u8) -> Option<&EscMotor<S>> {
self.motors.iter().find(|m| m.id() == esc_id)
}
/// 全部电机(按配置顺序)。
#[must_use]
pub fn motors(&self) -> &[EscMotor] {
pub fn motors(&self) -> &[EscMotor<S>] {
&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);
for task in self.tasks {
@@ -394,9 +331,9 @@ impl<P: EffortPolicy> EscBusHandle<P> {
}
}
async fn rx_loop(
async fn rx_loop<S: CurrentEscDrive>(
mut incoming: mpsc::Receiver<CanFrame>,
motors: Arc<Vec<EscMotor>>,
motors: Arc<Vec<EscMotor<S>>>,
mut shutdown: watch::Receiver<bool>,
) {
loop {
@@ -407,22 +344,20 @@ async fn rx_loop(
None => break,
},
};
match decode_feedback(&frame) {
match <S::Protocol as MotorProtocol>::decode_feedback(&frame) {
Ok((esc_id, fb)) => {
if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) {
motor.shared.apply_feedback(fb, Instant::now());
motor.shared.apply_feedback(&fb, Instant::now());
}
}
Err(e) => {
debug!(error = %e, id = frame.id, "ignore non-feedback frame");
}
Err(e) => debug!(error = %e, id = frame.id, "ignore non-feedback frame"),
}
}
}
async fn tx_loop<P: EffortPolicy>(
async fn tx_loop<S: CurrentEscDrive, P: EffortPolicy>(
outgoing: mpsc::Sender<CanFrame>,
motors: Arc<Vec<EscMotor>>,
motors: Arc<Vec<EscMotor<S>>>,
policy: Arc<Mutex<P>>,
options: BusOptions,
mut shutdown: watch::Receiver<bool>,
@@ -453,28 +388,29 @@ async fn tx_loop<P: EffortPolicy>(
}
}
let efforts = lock(&policy).step(&PolicyInput { samples, dt });
for (frame_id, group_start) in
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
{
let group: Vec<&EscMotor> = motors
for group_start in [1u8, 5u8] {
let Some(frame_id) = <S::Protocol as MotorProtocol>::command_frame_id(group_start)
else {
continue;
};
let group: Vec<&EscMotor<S>> = motors
.iter()
.filter(|m| (group_start..group_start + 4).contains(&m.id()))
.collect();
if group.is_empty() {
continue;
}
let mut currents = [0i16; 4];
let mut commands = [S::Protocol::command_from_raw(0); 4];
for motor in &group {
let slot = usize::from(motor.id() - group_start);
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),
);
let limited =
directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit);
commands[slot] = S::Protocol::command_from_raw(S::scale().clamp_raw(limited));
}
if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
if outgoing.send(S::Protocol::encode_group(frame_id, commands)).await.is_err() {
info!("can link closed, tx loop exiting");
return;
}
@@ -484,7 +420,6 @@ async fn tx_loop<P: EffortPolicy>(
fn tick_every(period: Duration) -> Interval {
let mut ticker = tokio::time::interval(period);
// 错过节拍后直接跳到当前节拍,避免补发帧风暴。
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
ticker
}
@@ -492,49 +427,35 @@ fn tick_every(period: Duration) -> Interval {
#[cfg(test)]
mod tests {
use super::*;
use motor::{Gm3508, M2006};
#[test]
fn config_validate_rejects_bad_values() {
let mut cfg = EscConfig::gm3508("m", 1, 1.0);
cfg.esc_id = 0;
assert!(cfg.validate().is_err());
cfg.esc_id = 9;
assert!(cfg.validate().is_err());
cfg.esc_id = 5;
cfg.gear_ratio = 0.0;
assert!(cfg.validate().is_err());
cfg.gear_ratio = 19.2;
cfg.direction = 0.5;
assert!(cfg.validate().is_err());
cfg.direction = -1.0;
cfg.current_limit = 0;
assert!(cfg.validate().is_err());
cfg.current_limit = 16385;
assert!(cfg.validate().is_err());
cfg.current_limit = 12_000;
assert!(cfg.validate().is_ok());
}
#[test]
fn gm3508_defaults_are_sane() {
let cfg = EscConfig::gm3508("front_left", 1, -1.0);
assert_eq!(cfg.model, EscModel::C620);
fn typed_defaults_are_sane() {
let cfg = EscConfig::<Gm3508>::gm3508("front_left", 1, -1.0);
assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
assert_eq!(cfg.current_limit, 12_000);
assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits());
assert!(cfg.validate().is_ok());
}
#[test]
fn m2006_defaults_are_sane() {
let cfg = EscConfig::m2006("feeder", 5, 1.0);
assert_eq!(cfg.model, EscModel::C610);
let cfg = EscConfig::<M2006>::m2006("feeder", 5, 1.0);
assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
assert!(cfg.validate().is_ok());
}
#[test]
fn config_validate_rejects_bad_values() {
let mut cfg = EscConfig::<Gm3508>::gm3508("m", 1, 1.0);
cfg.esc_id = 0;
assert!(cfg.validate().is_err());
cfg.esc_id = 5;
cfg.gear_ratio = 0.0;
assert!(cfg.validate().is_err());
cfg.gear_ratio = 19.2;
cfg.direction = 0.5;
assert!(cfg.validate().is_err());
cfg.direction = -1.0;
cfg.current_limit = 0;
assert!(cfg.validate().is_err());
cfg.current_limit = 16_385;
assert!(cfg.validate().is_err());
}
}
@@ -15,3 +15,6 @@ imu = { workspace = true }
delta10-protocol = { workspace = true }
tokio = { workspace = true }
thiserror = { workspace = true }
[package.metadata.sentry-chassis]
layer = "device/imu"
@@ -16,3 +16,6 @@ thiserror = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "device/motor"
@@ -792,173 +792,121 @@ pub mod testing {
}
}
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::*;
pub mod spec {
//! Generic motor identity and mechanical characteristics.
use motor_protocol::{CurrentC610, CurrentC620, CurrentEsc, Gm6020Voltage, MotorProtocol};
/// Control quantity represented by a motor's wire command.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ControlMode {
/// Current command.
Current,
/// Voltage command.
Voltage,
}
/// Mechanical and protocol identity for one motor model.
pub trait MotorSpec: Send + Sync + 'static {
type Protocol: MotorProtocol;
const NAME: &'static str;
const GEAR_RATIO: f32;
const ENCODER_TICKS: u16;
const CONTROL_MODE: ControlMode;
fn scale() -> <Self::Protocol as MotorProtocol>::Scale;
}
/// Marker for models driven by the shared C620/C610 current frame path.
pub trait CurrentEscDrive: MotorSpec {}
/// Marker for models driven by a voltage frame path.
pub trait VoltageEscDrive: MotorSpec {}
/// GM3508 + C620.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Gm3508;
/// M2006 + C610.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct M2006;
/// GM6020 direct-drive voltage motor.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Gm6020;
impl MotorSpec for Gm3508 {
type Protocol = CurrentEsc<CurrentC620>;
const NAME: &'static str = "GM3508";
const GEAR_RATIO: f32 = 3591.0 / 187.0;
const ENCODER_TICKS: u16 = 8192;
const CONTROL_MODE: ControlMode = ControlMode::Current;
fn scale() -> CurrentC620 {
CurrentC620
}
}
impl MotorSpec for M2006 {
type Protocol = CurrentEsc<CurrentC610>;
const NAME: &'static str = "M2006";
const GEAR_RATIO: f32 = 36.0;
const ENCODER_TICKS: u16 = 8192;
const CONTROL_MODE: ControlMode = ControlMode::Current;
fn scale() -> CurrentC610 {
CurrentC610
}
}
impl MotorSpec for Gm6020 {
type Protocol = Gm6020Voltage;
const NAME: &'static str = "GM6020";
const GEAR_RATIO: f32 = 1.0;
const ENCODER_TICKS: u16 = 8192;
const CONTROL_MODE: ControlMode = ControlMode::Voltage;
fn scale() -> motor_protocol::Voltage30000 {
motor_protocol::Voltage30000
}
}
impl CurrentEscDrive for Gm3508 {}
impl CurrentEscDrive for M2006 {}
impl VoltageEscDrive for Gm6020 {}
/// Keep scale construction available to generic callers.
pub fn scale_of<S: MotorSpec>() -> <S::Protocol as MotorProtocol>::Scale {
S::scale()
}
/// Compile-time assertion helper for current-path models.
pub fn assert_current_drive<S: CurrentEscDrive>() {}
/// Compile-time assertion helper for voltage-path models.
pub fn assert_voltage_drive<S: VoltageEscDrive>() {}
}
pub use spec::{ControlMode, CurrentEscDrive, Gm3508, Gm6020, M2006, MotorSpec, VoltageEscDrive};
#[cfg(test)]
mod spec_tests {
use super::*;
#[test]
fn motor_markers_select_the_right_protocol_family() {
spec::assert_current_drive::<Gm3508>();
spec::assert_current_drive::<M2006>();
spec::assert_voltage_drive::<Gm6020>();
assert_eq!(Gm3508::NAME, "GM3508");
assert_eq!(M2006::NAME, "M2006");
assert_eq!(Gm6020::NAME, "GM6020");
assert_eq!(Gm3508::CONTROL_MODE, ControlMode::Current);
assert_eq!(Gm6020::CONTROL_MODE, ControlMode::Voltage);
}
}
+22
View File
@@ -0,0 +1,22 @@
[package]
name = "dr16"
description = "DJI DR16 D-BUS serial receiver driver"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
[lints]
workspace = true
[dependencies]
dr16-protocol = { workspace = true }
serial = { workspace = true }
tokio = { workspace = true }
tracing = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "device/remote"
+244
View File
@@ -0,0 +1,244 @@
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
//! DJI DR16 D-BUS 串口接收驱动。
//!
//! DR16 使用非标准 100000 baud、8 data bits、even parity、1 stop bit。阻塞串口
//! I/O 运行在专用线程上,异步侧只消费**最新**样本,避免遥控数据形成积压队列。
use std::io::{self, Read};
use std::thread;
use std::time::{Duration, Instant};
use dr16_protocol::{Dr16Sample, FrameDecoder};
use serial::prelude::*;
use tokio::sync::watch;
use tracing::{debug, error};
/// D-BUS UART 波特率。
pub const BAUD_RATE: usize = 100_000;
/// 串口读取参数。
#[derive(Debug, Clone, Copy)]
pub struct Dr16Options {
/// 阻塞串口读超时;也用于让读取线程定期观察接收端关闭。
pub read_timeout: Duration,
}
impl Default for Dr16Options {
fn default() -> Self {
Self { read_timeout: Duration::from_millis(20) }
}
}
/// 带接收时间的 DR16 样本。
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct StampedSample {
/// 解码后的遥控输入。
pub sample: Dr16Sample,
/// 样本到达用户态读取循环的时刻。
pub received_at: Instant,
}
/// 最新样本发送端。
#[derive(Debug, Clone)]
pub struct SampleSender {
tx: watch::Sender<Option<StampedSample>>,
}
impl SampleSender {
/// 发布一个新样本并返回带时间戳的副本。
#[must_use]
pub fn send(&self, sample: Dr16Sample) -> StampedSample {
let stamped = StampedSample { sample, received_at: Instant::now() };
self.tx.send_replace(Some(stamped));
stamped
}
/// 当前是否有异步消费者。
#[must_use]
pub fn is_closed(&self) -> bool {
self.tx.is_closed()
}
}
/// 创建一对最新样本发送端和接收流。
#[must_use]
pub fn sample_channel() -> (SampleSender, SampleStream) {
let (tx, rx) = watch::channel(None);
(SampleSender { tx }, SampleStream { rx })
}
/// 最新 DR16 样本流。
///
/// 内部使用 `watch`:慢消费者看到的是最新遥控状态,不会执行延迟的旧摇杆位置。
#[derive(Debug)]
pub struct SampleStream {
rx: watch::Receiver<Option<StampedSample>>,
}
impl SampleStream {
/// 等待下一个新样本;读取线程退出且没有样本时返回 `None`。
pub async fn recv(&mut self) -> Option<StampedSample> {
loop {
self.rx.changed().await.ok()?;
if let Some(sample) = *self.rx.borrow_and_update() {
return Some(sample);
}
}
}
/// 读取当前最新样本;尚未收到帧时返回 `None`。
#[must_use]
pub fn latest(&self) -> Option<StampedSample> {
*self.rx.borrow()
}
}
/// 打开 DR16 串口并开始接收。
///
/// # Errors
///
/// 设备不存在、权限不足、串口不支持 100000 baud 或配置 8E1 失败。
pub fn open(device: &str) -> io::Result<SampleStream> {
open_with(device, Dr16Options::default())
}
/// 使用指定读取参数打开 DR16 串口。
///
/// # Errors
///
/// 同 [`open`]。
pub fn open_with(device: &str, options: Dr16Options) -> io::Result<SampleStream> {
let mut port = serial::open(device)?;
port.reconfigure(&|settings| {
settings.set_baud_rate(serial::BaudOther(BAUD_RATE))?;
settings.set_char_size(serial::Bits8);
settings.set_parity(serial::ParityEven);
settings.set_stop_bits(serial::Stop1);
settings.set_flow_control(serial::FlowNone);
Ok(())
})?;
port.set_timeout(options.read_timeout)?;
debug!(device, baud = BAUD_RATE, "DR16 serial opened");
Ok(spawn_reader(port))
}
/// 在专用线程上泵送任意字节 reader,返回最新样本流。
#[must_use]
pub fn spawn_reader<R>(reader: R) -> SampleStream
where
R: Read + Send + 'static,
{
let (tx, stream) = sample_channel();
let thread = thread::Builder::new().name("dr16-reader".into()).spawn(move || {
pump_reader(reader, &tx);
});
if let Err(error) = thread {
error!(%error, "failed to start DR16 reader thread");
}
stream
}
fn pump_reader<R: Read>(mut reader: R, tx: &SampleSender) {
let mut decoder = FrameDecoder::new();
let mut buffer = [0u8; 64];
loop {
match reader.read(&mut buffer) {
Ok(0) => {
debug!("DR16 reader reached end of input");
return;
}
Ok(count) => {
for sample in decoder.push(&buffer[..count]) {
let _ = tx.send(sample);
}
}
Err(error) if is_transient(&error) => {
// 读超时内没有任何字节,已经超过正常 D-BUS 帧间隔;清掉半帧,避免错位。
decoder.clear();
if tx.is_closed() {
debug!("DR16 sample consumer closed");
return;
}
}
Err(error) => {
error!(%error, "DR16 serial read failed");
return;
}
}
if tx.is_closed() {
debug!("DR16 sample consumer closed");
return;
}
}
}
fn is_transient(error: &io::Error) -> bool {
matches!(
error.kind(),
io::ErrorKind::TimedOut | io::ErrorKind::WouldBlock | io::ErrorKind::Interrupted
)
}
#[cfg(test)]
mod tests {
use std::cmp;
use dr16_protocol::{FRAME_LEN, SwitchPosition};
use super::*;
struct ChunkReader {
chunks: Vec<Vec<u8>>,
index: usize,
}
impl Read for ChunkReader {
fn read(&mut self, output: &mut [u8]) -> io::Result<usize> {
let Some(chunk) = self.chunks.get(self.index) else {
return Ok(0);
};
self.index += 1;
let count = cmp::min(output.len(), chunk.len());
output[..count].copy_from_slice(&chunk[..count]);
Ok(count)
}
}
fn frame() -> [u8; FRAME_LEN] {
let mut frame = [0u8; FRAME_LEN];
frame[1] = 0x04;
frame[2] = 0x20;
frame[4] = 0x01;
frame[5] = 0x78;
frame
}
#[tokio::test]
async fn generic_reader_publishes_decoded_latest_sample() {
let raw = frame();
let reader = ChunkReader {
chunks: vec![raw[..5].to_vec(), raw[5..13].to_vec(), raw[13..].to_vec()],
index: 0,
};
let mut stream = spawn_reader(reader);
let stamped = stream.recv().await.unwrap();
assert_eq!(stamped.sample.s1, SwitchPosition::Up);
assert_eq!(stamped.sample.s2, SwitchPosition::Mid);
assert_eq!(stream.latest(), Some(stamped));
}
#[tokio::test]
async fn closed_reader_reports_end_of_stream() {
let reader = ChunkReader { chunks: Vec::new(), index: 0 };
let mut stream = spawn_reader(reader);
assert_eq!(stream.recv().await, None);
}
#[test]
fn transient_read_errors_are_not_fatal() {
assert!(is_transient(&io::Error::from(io::ErrorKind::TimedOut)));
assert!(is_transient(&io::Error::from(io::ErrorKind::WouldBlock)));
assert!(!is_transient(&io::Error::from(io::ErrorKind::BrokenPipe)));
}
}
@@ -14,3 +14,6 @@ workspace = true
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "experiment/trace"
@@ -14,3 +14,6 @@ motor-protocol = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "model/motor"
@@ -108,6 +108,24 @@ pub mod dc_motor {
self.load_rpm = rpm;
self
}
/// Whether all parameters are finite and physically admissible.
#[must_use]
pub fn is_valid(&self) -> bool {
self.gain_rpm_per_raw.is_finite()
&& self.gain_rpm_per_raw >= 0.0
&& self.tau_s.is_finite()
&& self.tau_s >= 0.0
&& self.max_rpm.is_finite()
&& self.max_rpm >= 0.0
&& self.static_raw.is_finite()
&& self.static_raw >= 0.0
&& self.coulomb_raw.is_finite()
&& self.coulomb_raw >= 0.0
&& self.viscous_raw_per_rpm.is_finite()
&& self.viscous_raw_per_rpm >= 0.0
&& self.load_rpm.is_finite()
}
}
/// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
@@ -169,6 +187,9 @@ pub mod dc_motor {
/// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`,
/// 即历史的一阶模型。
pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
if !dt.is_finite() || dt <= 0.0 {
return self.feedback(current_raw);
}
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)
@@ -184,6 +205,10 @@ pub mod dc_motor {
let ticks = f32::from(ENCODER_TICKS);
self.angle_raw =
(self.angle_raw + self.speed_rpm / 60.0 * ticks * dt).rem_euclid(ticks);
self.feedback(applied as i16)
}
fn feedback(&self, current_raw: i16) -> EscFeedback {
EscFeedback {
angle_raw: self.angle_raw as u16,
speed_rpm: self.speed_rpm as i16,
@@ -356,6 +381,36 @@ pub mod dc_motor {
assert_eq!(fb.temperature_c, 30);
}
#[test]
fn delayed_feedback_reports_the_effective_command() {
let params = PlantParams::gm3508().with_delay_ticks(2);
let mut plant = PlantMotor::new(params);
assert_eq!(plant.step(1000, DT).current_raw, 0);
assert_eq!(plant.step(2000, DT).current_raw, 0);
assert_eq!(plant.step(3000, DT).current_raw, 1000);
}
#[test]
fn invalid_dt_is_a_strict_no_op() {
let mut plant = PlantMotor::gm3508();
plant.step(5000, DT);
let before_speed = plant.speed_rpm();
let before_angle = plant.angle_raw();
for dt in [0.0, -1.0, f32::NAN, f32::INFINITY] {
plant.step(10_000, dt);
assert_eq!(plant.speed_rpm().to_bits(), before_speed.to_bits());
assert_eq!(plant.angle_raw().to_bits(), before_angle.to_bits());
}
}
#[test]
fn parameter_validation_rejects_non_finite_values() {
let mut params = PlantParams::gm3508();
assert!(params.is_valid());
params.tau_s = f32::NAN;
assert!(!params.is_valid());
}
#[test]
fn reset_returns_to_rest() {
let mut plant = PlantMotor::gm3508();
@@ -371,7 +426,7 @@ pub mod dc_motor {
}
pub mod gm3508 {
//! GM3508 的被控对象参数(ARCHITECTURE.md §5.6 / §8.2)。
//! GM3508 的被控对象参数(docs/chassis.md §7.4)。
//!
//! 这里只放**有实测依据**的两组参数:
//!
@@ -424,3 +479,319 @@ pub mod gm3508 {
pub use dc_motor::{PlantMotor, PlantParams};
pub use gm3508::{bench_params, ideal_params};
pub mod generic {
//! Generic motor dynamic-model seam.
use core::marker::PhantomData;
use motor_protocol::{EscFeedback, FeedbackFields, MotorProtocol};
use crate::{PlantMotor, PlantParams};
/// Error returned by a dynamic model step.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ModelError {
/// The requested time step is not finite and positive.
InvalidDt,
/// A model parameter is outside its valid domain.
InvalidParams,
}
impl core::fmt::Display for ModelError {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
match self {
Self::InvalidDt => write!(f, "dt must be finite and positive"),
Self::InvalidParams => write!(f, "model parameters are invalid"),
}
}
}
impl core::error::Error for ModelError {}
/// Dynamic model contract for one protocol family.
pub trait MotorPlant {
type Protocol: MotorProtocol;
type Feedback: FeedbackFields + Copy + Send + Sync;
type Error;
fn step(
&mut self,
command: <Self::Protocol as MotorProtocol>::Command,
dt: f32,
) -> Result<Self::Feedback, Self::Error>;
fn reset(&mut self);
}
/// Parameter provider for a DC current-controlled model.
pub trait DcModelSpec: Send + Sync + 'static {
type Protocol: MotorProtocol<Command = i16, Feedback = EscFeedback>;
fn ideal_params() -> PlantParams;
fn bench_params() -> PlantParams;
}
/// DC motor plant generic over the motor model profile.
#[derive(Debug, Clone)]
pub struct DcMotorPlant<S: DcModelSpec> {
motor: PlantMotor,
marker: PhantomData<S>,
}
impl<S: DcModelSpec> DcMotorPlant<S> {
#[must_use]
pub fn new(params: PlantParams) -> Self {
Self { motor: PlantMotor::new(params), marker: PhantomData }
}
#[must_use]
pub fn ideal() -> Self {
Self::new(S::ideal_params())
}
#[must_use]
pub fn bench() -> Self {
Self::new(S::bench_params())
}
#[must_use]
pub fn speed_rpm(&self) -> f32 {
self.motor.speed_rpm()
}
#[must_use]
pub fn angle_raw(&self) -> f32 {
self.motor.angle_raw()
}
#[must_use]
pub fn params(&self) -> &PlantParams {
self.motor.params()
}
}
impl<S: DcModelSpec> MotorPlant for DcMotorPlant<S> {
type Protocol = S::Protocol;
type Feedback = EscFeedback;
type Error = ModelError;
fn step(
&mut self,
command: <Self::Protocol as MotorProtocol>::Command,
dt: f32,
) -> Result<Self::Feedback, Self::Error> {
if !dt.is_finite() || dt <= 0.0 {
return Err(ModelError::InvalidDt);
}
if !self.motor.params().is_valid() {
return Err(ModelError::InvalidParams);
}
Ok(self.motor.step(command, dt))
}
fn reset(&mut self) {
self.motor.reset();
}
}
/// GM3508 DC model profile.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Gm3508Model;
/// M2006 DC model profile.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct M2006Model;
impl DcModelSpec for Gm3508Model {
type Protocol = motor_protocol::CurrentEsc<motor_protocol::CurrentC620>;
fn ideal_params() -> PlantParams {
PlantParams::gm3508()
}
fn bench_params() -> PlantParams {
PlantParams::gm3508_bench()
}
}
impl DcModelSpec for M2006Model {
type Protocol = motor_protocol::CurrentEsc<motor_protocol::CurrentC610>;
fn ideal_params() -> PlantParams {
PlantParams::gm3508()
}
fn bench_params() -> PlantParams {
PlantParams::gm3508_bench()
}
}
}
pub use generic::{DcModelSpec, DcMotorPlant, Gm3508Model, M2006Model, ModelError, MotorPlant};
#[cfg(test)]
mod generic_tests {
use super::*;
#[test]
fn typed_dc_models_use_current_protocols() {
let mut plant = DcMotorPlant::<Gm3508Model>::ideal();
assert!(plant.step(1000, 0.001).is_ok());
assert!(plant.speed_rpm().is_finite());
let mut plant = DcMotorPlant::<M2006Model>::bench();
assert!(plant.step(1000, 0.001).is_ok());
}
#[test]
fn typed_models_reject_invalid_steps_and_parameters() {
let mut plant = DcMotorPlant::<Gm3508Model>::ideal();
assert_eq!(plant.step(1, 0.0), Err(ModelError::InvalidDt));
let mut params = PlantParams::gm3508();
params.load_rpm = f32::NAN;
let mut plant = DcMotorPlant::<Gm3508Model>::new(params);
assert_eq!(plant.step(1, 0.001), Err(ModelError::InvalidParams));
}
}
pub mod voltage {
//! Independent voltage-motor dynamics for GM6020-style direct-drive motors.
use core::marker::PhantomData;
use std::collections::VecDeque;
use motor_protocol::{Gm6020Command, Gm6020Feedback, Gm6020Voltage, MotorProtocol};
use crate::generic::{ModelError, MotorPlant};
/// Parameters for a voltage-controlled motor model.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct VoltagePlantParams {
pub gain_rpm_per_raw: f32,
pub tau_s: f32,
pub max_rpm: f32,
pub delay_ticks: u32,
}
impl VoltagePlantParams {
#[must_use]
pub fn is_valid(self) -> bool {
self.gain_rpm_per_raw.is_finite()
&& self.gain_rpm_per_raw >= 0.0
&& self.tau_s.is_finite()
&& self.tau_s >= 0.0
&& self.max_rpm.is_finite()
&& self.max_rpm >= 0.0
}
}
/// Voltage model profile.
pub trait VoltageModelSpec: Send + Sync + 'static {
type Protocol: MotorProtocol<Command = Gm6020Command, Feedback = Gm6020Feedback>;
fn params() -> VoltagePlantParams;
}
/// Direct-drive voltage plant. It deliberately does not reuse DC current friction.
#[derive(Debug, Clone)]
pub struct VoltageMotorPlant<S: VoltageModelSpec> {
params: VoltagePlantParams,
speed_rpm: f32,
angle_raw: f32,
delay_line: VecDeque<i16>,
marker: PhantomData<S>,
}
impl<S: VoltageModelSpec> VoltageMotorPlant<S> {
#[must_use]
pub fn new(params: VoltagePlantParams) -> Self {
Self {
params,
speed_rpm: 0.0,
angle_raw: 0.0,
delay_line: VecDeque::new(),
marker: PhantomData,
}
}
#[must_use]
pub fn speed_rpm(&self) -> f32 {
self.speed_rpm
}
}
impl<S: VoltageModelSpec> MotorPlant for VoltageMotorPlant<S> {
type Protocol = Gm6020Voltage;
type Feedback = Gm6020Feedback;
type Error = ModelError;
fn step(&mut self, command: Gm6020Command, dt: f32) -> Result<Self::Feedback, Self::Error> {
if !dt.is_finite() || dt <= 0.0 {
return Err(ModelError::InvalidDt);
}
if !self.params.is_valid() {
return Err(ModelError::InvalidParams);
}
self.delay_line.push_back(command.voltage_raw);
let applied = if self.delay_line.len() > self.params.delay_ticks as usize {
self.delay_line.pop_front().unwrap_or(0)
} else {
0
};
let target = (f32::from(applied) * self.params.gain_rpm_per_raw)
.clamp(-self.params.max_rpm, self.params.max_rpm);
let alpha =
if self.params.tau_s > 0.0 { 1.0 - (-dt / self.params.tau_s).exp() } else { 1.0 };
self.speed_rpm += (target - self.speed_rpm) * alpha;
self.angle_raw =
(self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0);
Ok(Gm6020Feedback {
angle_deg: self.angle_raw * 360.0 / 8192.0,
angle_raw: self.angle_raw as u16,
speed_rpm: self.speed_rpm as i16,
current_raw: 0,
temperature_c: 30,
})
}
fn reset(&mut self) {
self.speed_rpm = 0.0;
self.angle_raw = 0.0;
self.delay_line.clear();
}
}
/// GM6020 voltage model profile. Values are structural defaults, not field identification.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Gm6020Model;
impl VoltageModelSpec for Gm6020Model {
type Protocol = Gm6020Voltage;
fn params() -> VoltagePlantParams {
VoltagePlantParams {
gain_rpm_per_raw: 0.01,
tau_s: 0.1,
max_rpm: 300.0,
delay_ticks: 0,
}
}
}
}
pub use voltage::{Gm6020Model, VoltageModelSpec, VoltageMotorPlant, VoltagePlantParams};
#[cfg(test)]
mod voltage_tests {
use super::*;
use motor_protocol::Gm6020Command;
#[test]
fn voltage_model_is_independent_and_resets() {
let mut plant = VoltageMotorPlant::<Gm6020Model>::new(Gm6020Model::params());
let feedback = MotorPlant::step(&mut plant, Gm6020Command::new(10_000), 0.001).unwrap();
assert!(feedback.speed_rpm >= 0);
plant.reset();
assert!(plant.speed_rpm().abs() < f32::EPSILON);
assert_eq!(
MotorPlant::step(&mut plant, Gm6020Command::new(1), 0.0),
Err(ModelError::InvalidDt)
);
}
}
@@ -16,3 +16,6 @@ thiserror = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "protocol/imu"
@@ -16,3 +16,6 @@ thiserror = { workspace = true }
[dev-dependencies]
proptest = { workspace = true }
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "protocol/motor"
@@ -909,3 +909,276 @@ pub use id::*;
pub use m2006::C610;
pub use m3508::C620;
pub use turn::*;
pub mod generic {
//! Generic wire-protocol contracts.
//!
//! The concrete codec modules remain the source of wire layout. This module
//! only gives upper layers a type-safe seam for current/voltage families.
use core::marker::PhantomData;
use crate::common::{self, DecodeError, EscFeedback};
use crate::frame::CanFrame;
use crate::gm6020::{self, Gm6020Command, Gm6020Feedback};
/// Protocol decoding error used by the generic codec seam.
pub type ProtocolError = DecodeError;
/// Protocol-independent view of a wire feedback frame.
pub trait FeedbackFields {
fn angle_raw(&self) -> u16;
fn speed_rpm(&self) -> i16;
fn current_raw(&self) -> i16;
fn temperature_c(&self) -> i8;
}
impl FeedbackFields for EscFeedback {
fn angle_raw(&self) -> u16 {
self.angle_raw
}
fn speed_rpm(&self) -> i16 {
self.speed_rpm
}
fn current_raw(&self) -> i16 {
self.current_raw
}
fn temperature_c(&self) -> i8 {
self.temperature_c
}
}
impl FeedbackFields for Gm6020Feedback {
fn angle_raw(&self) -> u16 {
self.angle_raw
}
fn speed_rpm(&self) -> i16 {
self.speed_rpm
}
fn current_raw(&self) -> i16 {
self.current_raw
}
fn temperature_c(&self) -> i8 {
self.temperature_c
}
}
/// Raw effort scaling and saturation for one controller family.
pub trait EffortScale: Copy + Send + Sync + 'static {
fn max_raw(self) -> i16;
fn max_amp(self) -> f32;
fn clamp_raw(self, raw: i16) -> i16;
fn raw_from_amp(self, amp: f32) -> i16;
fn amp_from_raw(self, raw: i16) -> f32;
}
/// C620 current scale: ±16384 raw ↔ ±20 A.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct CurrentC620;
/// C610 current scale: ±10000 raw ↔ ±10 A.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct CurrentC610;
/// GM6020 voltage scale: ±30000 raw.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Voltage30000;
impl EffortScale for CurrentC620 {
fn max_raw(self) -> i16 {
16_384
}
fn max_amp(self) -> f32 {
20.0
}
fn clamp_raw(self, raw: i16) -> i16 {
common::clamp_current(common::EscModel::C620, raw)
}
fn raw_from_amp(self, amp: f32) -> i16 {
common::EscModel::C620.raw_from_amp(amp)
}
fn amp_from_raw(self, raw: i16) -> f32 {
common::EscModel::C620.amp_from_raw(raw)
}
}
impl EffortScale for CurrentC610 {
fn max_raw(self) -> i16 {
10_000
}
fn max_amp(self) -> f32 {
10.0
}
fn clamp_raw(self, raw: i16) -> i16 {
common::clamp_current(common::EscModel::C610, raw)
}
fn raw_from_amp(self, amp: f32) -> i16 {
common::EscModel::C610.raw_from_amp(amp)
}
fn amp_from_raw(self, raw: i16) -> f32 {
common::EscModel::C610.amp_from_raw(raw)
}
}
impl EffortScale for Voltage30000 {
fn max_raw(self) -> i16 {
gm6020::VOLTAGE_MAX_RAW
}
fn max_amp(self) -> f32 {
f32::from(gm6020::VOLTAGE_MAX_RAW)
}
fn clamp_raw(self, raw: i16) -> i16 {
gm6020::clamp_voltage(raw)
}
fn raw_from_amp(self, amp: f32) -> i16 {
let raw = amp.clamp(-f32::from(self.max_raw()), f32::from(self.max_raw()));
self.clamp_raw(raw as i16)
}
fn amp_from_raw(self, raw: i16) -> f32 {
f32::from(self.clamp_raw(raw))
}
}
/// Wire protocol contract for one motor family.
pub trait MotorProtocol: Send + Sync + 'static {
type Command: Copy + Send + Sync;
type Feedback: FeedbackFields + Copy + Send + Sync;
type Scale: EffortScale;
fn command_frame_id(id: u8) -> Option<u32>;
fn feedback_frame_id(id: u8) -> Option<u32>;
fn encode_group(frame_id: u32, commands: [Self::Command; 4]) -> CanFrame;
fn decode_feedback(frame: &CanFrame) -> Result<(u8, Self::Feedback), ProtocolError>;
fn command_from_raw(raw: i16) -> Self::Command;
}
/// C620/C610 share the current frame layout; `S` only selects the scale.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct CurrentEsc<S>(PhantomData<S>);
impl<S: EffortScale + DefaultScale> MotorProtocol for CurrentEsc<S> {
type Command = i16;
type Feedback = EscFeedback;
type Scale = S;
fn command_frame_id(id: u8) -> Option<u32> {
common::command_frame_id(id)
}
fn feedback_frame_id(id: u8) -> Option<u32> {
common::feedback_frame_id(id)
}
fn encode_group(frame_id: u32, commands: [Self::Command; 4]) -> CanFrame {
common::encode_current_frame(frame_id, commands)
}
fn decode_feedback(frame: &CanFrame) -> Result<(u8, Self::Feedback), ProtocolError> {
common::decode_feedback(frame)
}
fn command_from_raw(raw: i16) -> Self::Command {
S::default_scale().clamp_raw(raw)
}
}
/// GM6020 voltage protocol.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Gm6020Voltage;
impl MotorProtocol for Gm6020Voltage {
type Command = Gm6020Command;
type Feedback = Gm6020Feedback;
type Scale = Voltage30000;
fn command_frame_id(id: u8) -> Option<u32> {
gm6020::command_frame_id(id)
}
fn feedback_frame_id(id: u8) -> Option<u32> {
gm6020::feedback_frame_id(id)
}
fn encode_group(frame_id: u32, commands: [Self::Command; 4]) -> CanFrame {
gm6020::encode_voltage_frame(
frame_id,
[
commands[0].voltage_raw,
commands[1].voltage_raw,
commands[2].voltage_raw,
commands[3].voltage_raw,
],
)
}
fn decode_feedback(frame: &CanFrame) -> Result<(u8, Self::Feedback), ProtocolError> {
gm6020::decode_feedback(frame)
}
fn command_from_raw(raw: i16) -> Self::Command {
Gm6020Command::new(raw)
}
}
pub trait DefaultScale: EffortScale {
fn default_scale() -> Self;
}
impl DefaultScale for CurrentC620 {
fn default_scale() -> Self {
Self
}
}
impl DefaultScale for CurrentC610 {
fn default_scale() -> Self {
Self
}
}
}
pub use generic::{
CurrentC610, CurrentC620, CurrentEsc, DefaultScale, EffortScale, FeedbackFields, Gm6020Voltage,
MotorProtocol, ProtocolError, Voltage30000,
};
#[cfg(test)]
mod generic_tests {
use super::*;
#[test]
fn current_protocol_round_trips_feedback_fields() {
let feedback = common::EscFeedback {
angle_raw: 17,
speed_rpm: -123,
current_raw: 456,
temperature_c: 31,
};
let frame = common::encode_feedback_frame(2, &feedback);
let (id, decoded) =
<generic::CurrentEsc<generic::CurrentC620> as generic::MotorProtocol>::decode_feedback(
&frame,
)
.unwrap();
assert_eq!(id, 2);
assert_eq!(decoded.angle_raw(), 17);
assert_eq!(decoded.speed_rpm(), -123);
assert_eq!(decoded.current_raw(), 456);
assert_eq!(decoded.temperature_c(), 31);
}
#[test]
fn current_scales_are_distinct_and_saturating() {
assert_eq!(generic::CurrentC620.max_raw(), 16_384);
assert_eq!(generic::CurrentC610.max_raw(), 10_000);
assert_eq!(generic::CurrentC610.clamp_raw(20_000), 10_000);
assert_eq!(generic::Voltage30000.clamp_raw(i16::MIN), -30_000);
}
#[test]
fn gm6020_uses_voltage_commands() {
let command = <generic::Gm6020Voltage as generic::MotorProtocol>::command_from_raw(1234);
assert_eq!(command.voltage_raw, 1234);
assert_eq!(
<generic::Gm6020Voltage as generic::MotorProtocol>::command_frame_id(1),
Some(gm6020::COMMAND_FRAME_ID_PRIMARY)
);
}
}
+19
View File
@@ -0,0 +1,19 @@
[package]
name = "dr16-protocol"
description = "DJI DR16 D-BUS frame protocol"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
[lints]
workspace = true
[dependencies]
thiserror = { workspace = true }
[dev-dependencies]
proptest = { workspace = true }
[package.metadata.sentry-chassis]
layer = "protocol/remote"
+380
View File
@@ -0,0 +1,380 @@
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
//! DJI DR16 接收机 D-BUS 帧协议。
//!
//! 协议帧固定为 18 字节:四个 11 bit 摇杆通道、两个三档开关、鼠标、按键与拨轮。
//! 本实现只描述线上的字节布局,不依赖串口、Tokio 或底盘控制类型。
//!
//! DT7/DR16 手册没有给出 D-BUS 字节布局;字段结构与开关编码参考
//! `RoboMaster/RoboRTS-Firmware`,本实现为独立 Rust 重写。
use std::collections::VecDeque;
use thiserror::Error;
/// 一帧 D-BUS 数据的字节数。
pub const FRAME_LEN: usize = 18;
/// 遥控器摇杆的协议中心值。
pub const CHANNEL_CENTER: i16 = 1024;
/// 摇杆有效范围:中心值 ±660。
pub const CHANNEL_MAX_ABS: i16 = 660;
/// 三档开关位置。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum SwitchPosition {
/// 上档,线值 1。
Up,
/// 下档,线值 2。
Down,
/// 中档,线值 3。
Mid,
}
impl SwitchPosition {
/// 开关的协议线值。
#[must_use]
pub const fn wire_value(self) -> u8 {
match self {
Self::Up => 1,
Self::Down => 2,
Self::Mid => 3,
}
}
}
impl TryFrom<u8> for SwitchPosition {
type Error = DecodeError;
fn try_from(value: u8) -> Result<Self, Self::Error> {
match value {
1 => Ok(Self::Up),
2 => Ok(Self::Down),
3 => Ok(Self::Mid),
_ => Err(DecodeError::InvalidSwitch(value)),
}
}
}
/// DR16 扩展输入:鼠标、按键与拨轮。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
pub struct ExtendedInput {
/// 鼠标 x 位移。
pub mouse_x: i16,
/// 鼠标 y 位移。
pub mouse_y: i16,
/// 鼠标 z 位移。
pub mouse_z: i16,
/// 鼠标左键。
pub mouse_left: bool,
/// 鼠标右键。
pub mouse_right: bool,
/// 键盘按键位图。
pub key_code: u16,
/// 拨轮中心值 ±1024 后的值。
pub wheel: i16,
}
/// 一帧完整 DR16 输入。
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Dr16Sample {
/// 摇杆通道 A/E/T/R,均已减去 1024,范围 -660..=660。
pub channels: [i16; 4],
/// 左侧三档开关 S1。
pub s1: SwitchPosition,
/// 右侧三档开关 S2。
pub s2: SwitchPosition,
/// 鼠标、按键和拨轮。
pub extended: ExtendedInput,
}
impl Dr16Sample {
/// 右摇杆左右通道 A/J1。
#[must_use]
pub const fn channel_a(self) -> i16 {
self.channels[0]
}
/// 右摇杆上下通道 E/J2。
#[must_use]
pub const fn channel_e(self) -> i16 {
self.channels[1]
}
/// 左摇杆上下通道 T/J3。
#[must_use]
pub const fn channel_t(self) -> i16 {
self.channels[2]
}
/// 左摇杆左右通道 R/J4。
#[must_use]
pub const fn channel_r(self) -> i16 {
self.channels[3]
}
}
/// D-BUS 解码错误。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum DecodeError {
/// 输入不是 18 字节完整帧。
#[error("invalid D-BUS frame length: expected {expected}, got {actual}")]
InvalidLength {
/// 协议要求的长度。
expected: usize,
/// 实际长度。
actual: usize,
},
/// 摇杆超出遥控器有效范围。
#[error("invalid D-BUS channel {channel}: {value}")]
InvalidChannel {
/// 通道下标 0..4。
channel: usize,
/// 中心化后的值。
value: i16,
},
/// 开关线值不是 1/2/3。
#[error("invalid D-BUS switch value: {0}")]
InvalidSwitch(u8),
/// 拨轮超出 ±1024。
#[error("invalid D-BUS wheel value: {0}")]
InvalidWheel(i16),
/// 鼠标按键字节不是 0/1。
#[error("invalid D-BUS mouse button byte: {0}")]
InvalidMouseButton(u8),
}
/// 解码一帧完整 D-BUS 数据。
///
/// # Errors
///
/// 任一摇杆超出 ±660 或开关线值非法时返回 [`DecodeError`]。
pub fn decode_frame(data: &[u8]) -> Result<Dr16Sample, DecodeError> {
if data.len() != FRAME_LEN {
return Err(DecodeError::InvalidLength { expected: FRAME_LEN, actual: data.len() });
}
let mut packed = [0u8; FRAME_LEN];
packed.copy_from_slice(data);
let raw_channels = [
(u16::from(packed[0]) | (u16::from(packed[1]) << 8)) & 0x07FF,
(u16::from(packed[1]) >> 3 | (u16::from(packed[2]) << 5)) & 0x07FF,
(u16::from(packed[2]) >> 6 | (u16::from(packed[3]) << 2) | (u16::from(packed[4]) << 10))
& 0x07FF,
(u16::from(packed[4]) >> 1 | (u16::from(packed[5]) << 7)) & 0x07FF,
];
let mut channels = [0i16; 4];
for (index, raw) in raw_channels.into_iter().enumerate() {
let value = raw as i16 - CHANNEL_CENTER;
if !(-CHANNEL_MAX_ABS..=CHANNEL_MAX_ABS).contains(&value) {
return Err(DecodeError::InvalidChannel { channel: index, value });
}
channels[index] = value;
}
let switch_bits = packed[5] >> 4;
let s1 = SwitchPosition::try_from((switch_bits & 0x0C) >> 2)?;
let s2 = SwitchPosition::try_from(switch_bits & 0x03)?;
let wheel = u16::from_le_bytes([packed[16], packed[17]]) as i16 - CHANNEL_CENTER;
if !(-1024..=1024).contains(&wheel) {
return Err(DecodeError::InvalidWheel(wheel));
}
if packed[12] > 1 {
return Err(DecodeError::InvalidMouseButton(packed[12]));
}
if packed[13] > 1 {
return Err(DecodeError::InvalidMouseButton(packed[13]));
}
Ok(Dr16Sample {
channels,
s1,
s2,
extended: ExtendedInput {
mouse_x: i16::from_le_bytes([packed[6], packed[7]]),
mouse_y: i16::from_le_bytes([packed[8], packed[9]]),
mouse_z: i16::from_le_bytes([packed[10], packed[11]]),
mouse_left: packed[12] == 1,
mouse_right: packed[13] == 1,
key_code: u16::from_le_bytes([packed[14], packed[15]]),
wheel,
},
})
}
/// 增量帧解码器:接受任意大小的串口读块,并在噪声后重新同步。
///
/// D-BUS 帧没有同步头或校验和,因此同步依靠固定长度和摇杆/开关的有效范围。
#[derive(Debug, Default)]
pub struct FrameDecoder {
bytes: VecDeque<u8>,
discarded_bytes: usize,
}
impl FrameDecoder {
/// 创建空解码器。
#[must_use]
pub const fn new() -> Self {
Self { bytes: VecDeque::new(), discarded_bytes: 0 }
}
/// 追加串口字节并返回其中可识别的完整帧。
#[must_use]
pub fn push(&mut self, input: &[u8]) -> Vec<Dr16Sample> {
self.bytes.extend(input.iter().copied());
let mut samples = Vec::new();
while self.bytes.len() >= FRAME_LEN {
let mut candidate: Option<(usize, Dr16Sample, usize)> = None;
let windows = self.bytes.len() - FRAME_LEN + 1;
for offset in 0..windows {
let mut frame = [0u8; FRAME_LEN];
for (index, byte) in self.bytes.iter().skip(offset).take(FRAME_LEN).enumerate() {
frame[index] = *byte;
}
if let Ok(sample) = decode_frame(&frame) {
// D-BUS 没有同步头。多个窗口都能通过范围校验时,优先选择
// 与 18 字节帧边界一致的候选,避免被噪声向前错一位。
let residue = (self.bytes.len() - (offset + FRAME_LEN)) % FRAME_LEN;
if candidate.is_none_or(|(_, _, best)| residue < best) {
candidate = Some((offset, sample, residue));
}
}
}
if let Some((offset, sample, _)) = candidate {
self.bytes.drain(0..offset + FRAME_LEN);
self.discarded_bytes += offset;
samples.push(sample);
} else {
let discard = self.bytes.len() - (FRAME_LEN - 1);
self.bytes.drain(0..discard);
self.discarded_bytes += discard;
}
}
samples
}
/// 因噪声或不完整候选帧而丢弃的累计字节数。
#[must_use]
pub const fn discarded_bytes(&self) -> usize {
self.discarded_bytes
}
/// 清空尚未组成帧的字节。
pub fn clear(&mut self) {
self.bytes.clear();
}
}
#[cfg(test)]
mod tests {
use super::*;
fn encode(channels: [i16; 4], s1: u8, s2: u8) -> [u8; FRAME_LEN] {
let raw: Vec<u16> = channels.iter().map(|value| (value + CHANNEL_CENTER) as u16).collect();
let mut frame = [0u8; FRAME_LEN];
frame[0] = (raw[0] & 0xFF) as u8;
frame[1] = ((raw[0] >> 8) as u8) | ((raw[1] & 0x1F) as u8) << 3;
frame[2] = ((raw[1] >> 5) as u8) | ((raw[2] & 0x03) as u8) << 6;
frame[3] = (raw[2] >> 2) as u8;
frame[4] = ((raw[2] >> 10) as u8) | ((raw[3] & 0x7F) as u8) << 1;
frame[5] = ((raw[3] >> 7) as u8) | (s2 & 0x03) << 4 | (s1 & 0x03) << 6;
frame
}
#[test]
fn decodes_channels_switches_mouse_keys_and_wheel() {
let mut frame = encode([-660, -5, 321, 660], 1, 3);
frame[5] = (frame[5] & 0x0F) | (1 << 6) | (3 << 4);
frame[6..8].copy_from_slice(&(-123i16).to_le_bytes());
frame[8..10].copy_from_slice(&456i16.to_le_bytes());
frame[10..12].copy_from_slice(&(-7i16).to_le_bytes());
frame[12] = 1;
frame[13] = 0;
frame[14..16].copy_from_slice(&0xA55Au16.to_le_bytes());
frame[16..18].copy_from_slice(&(2048u16).to_le_bytes());
let sample = decode_frame(&frame).unwrap();
assert_eq!(sample.channels, [-660, -5, 321, 660]);
assert_eq!(sample.s1, SwitchPosition::Up);
assert_eq!(sample.s2, SwitchPosition::Mid);
assert_eq!(sample.channel_a(), -660);
assert_eq!(sample.channel_t(), 321);
assert_eq!(
sample.extended,
ExtendedInput {
mouse_x: -123,
mouse_y: 456,
mouse_z: -7,
mouse_left: true,
mouse_right: false,
key_code: 0xA55A,
wheel: 1024,
}
);
}
#[test]
fn switch_wire_values_are_explicit() {
assert_eq!(SwitchPosition::Up.wire_value(), 1);
assert_eq!(SwitchPosition::Down.wire_value(), 2);
assert_eq!(SwitchPosition::Mid.wire_value(), 3);
assert_eq!(SwitchPosition::try_from(0), Err(DecodeError::InvalidSwitch(0)));
assert_eq!(SwitchPosition::try_from(4), Err(DecodeError::InvalidSwitch(4)));
}
#[test]
fn rejects_short_frame_and_out_of_range_values() {
assert_eq!(
decode_frame(&[0; FRAME_LEN - 1]),
Err(DecodeError::InvalidLength { expected: FRAME_LEN, actual: FRAME_LEN - 1 })
);
let frame = encode([661, 0, 0, 0], 1, 1);
assert!(matches!(
decode_frame(&frame),
Err(DecodeError::InvalidChannel { channel: 0, .. })
));
let frame = encode([0, 0, 0, 0], 0, 1);
assert_eq!(decode_frame(&frame), Err(DecodeError::InvalidSwitch(0)));
}
#[test]
fn decoder_accepts_fragmented_frames() {
let frame = encode([100, -200, 300, -400], 1, 2);
let mut decoder = FrameDecoder::new();
assert!(decoder.push(&frame[..7]).is_empty());
assert!(decoder.push(&frame[7..13]).is_empty());
let samples = decoder.push(&frame[13..]);
assert_eq!(samples.len(), 1);
assert_eq!(samples[0].channels, [100, -200, 300, -400]);
}
#[test]
fn decoder_resynchronizes_after_noise() {
let frame = encode([660, -660, 659, -659], 1, 3);
let mut input = vec![0xFF; 37];
input.extend_from_slice(&frame);
let mut decoder = FrameDecoder::new();
let samples = decoder.push(&input);
assert_eq!(samples.len(), 1, "噪声后应找到下一帧");
assert_eq!(samples[0].channels, [660, -660, 659, -659]);
assert!(decoder.discarded_bytes() >= 37);
}
#[test]
fn decoder_returns_multiple_frames_from_one_read() {
let first = encode([1, 2, 3, 4], 1, 1);
let second = encode([-1, -2, -3, -4], 2, 3);
let mut input = first.to_vec();
input.extend_from_slice(&second);
let mut decoder = FrameDecoder::new();
let samples = decoder.push(&input);
assert_eq!(samples.len(), 2);
assert_eq!(samples[0].channels, [1, 2, 3, 4]);
assert_eq!(samples[1].channels, [-1, -2, -3, -4]);
}
}
@@ -24,3 +24,6 @@ rand = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "simulation/motor"
@@ -706,7 +706,7 @@ pub mod plant {
use motor_protocol::{EscFeedback, clamp_current};
use crate::harness::LoopLimits;
use motor_model::{PlantMotor, PlantParams};
use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams};
/// 把被测电机的**转速**(rpm)当作输出、**effort raw** 当作输入的被控对象。
///
@@ -715,7 +715,7 @@ pub mod plant {
/// 不同的饱和行为。
#[derive(Debug, Clone)]
pub struct SimPlant {
motor: PlantMotor,
motor: DcMotorPlant<Gm3508Model>,
limits: LoopLimits,
steps: u64,
}
@@ -724,7 +724,7 @@ pub mod plant {
/// 由参数构造(默认限幅与 [`LoopLimits::gm3508`] 一致)。
#[must_use]
pub fn new(params: PlantParams) -> Self {
Self { motor: PlantMotor::new(params), limits: LoopLimits::gm3508(), steps: 0 }
Self { motor: DcMotorPlant::new(params), limits: LoopLimits::gm3508(), steps: 0 }
}
/// 换限幅与电调型号。
@@ -789,15 +789,17 @@ pub mod plant {
self.limits.model,
raw.clamp(-self.limits.current_limit_raw, self.limits.current_limit_raw),
);
let feedback = self.motor.step(command, dt);
self.steps += 1;
return Ok(f32::from(feedback.speed_rpm));
if let Ok(feedback) = MotorPlant::step(&mut self.motor, command, dt) {
self.steps += 1;
return Ok(f32::from(feedback.speed_rpm));
}
return Ok(self.motor.speed_rpm());
}
Ok(self.motor.speed_rpm())
}
fn reset(&mut self) {
self.motor.reset();
MotorPlant::reset(&mut self.motor);
self.steps = 0;
}
@@ -926,7 +928,7 @@ pub mod rig {
use tokio::time::{MissedTickBehavior, interval};
use tracing::trace;
use motor_model::{PlantMotor, PlantParams};
use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams};
/// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。
///
@@ -943,8 +945,8 @@ pub mod rig {
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 motors: HashMap<u8, DcMotorPlant<Gm3508Model>> =
specs.iter().map(|&(id, params)| (id, DcMotorPlant::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);
@@ -960,7 +962,9 @@ pub mod rig {
_ = ticker.tick() => {
for (&id, motor) in &mut motors {
let cmd = commands.get(&id).copied().unwrap_or(0);
let feedback = motor.step(cmd, dt);
let Ok(feedback) = MotorPlant::step(motor, cmd, dt) else {
continue;
};
if link
.outgoing_tx
.send(encode_feedback_frame(id, &feedback))
@@ -29,3 +29,6 @@ motor-model = { workspace = true }
[dev-dependencies]
approx = { workspace = true }
proptest = { workspace = true }
[package.metadata.sentry-chassis]
layer = "tooling/tuning"
@@ -1591,7 +1591,7 @@ pub mod preset {
use std::path::{Path, PathBuf};
use experiment::{Scenario, SetpointProfile};
use motor_model::PlantParams;
use motor_model::{DcModelSpec, PlantParams};
use serde::{Deserialize, Serialize};
use sim_harness::PlantTolerances;
use thiserror::Error;
@@ -1724,29 +1724,29 @@ pub mod preset {
/// 离线被控对象模型。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")]
pub enum PlantSpec {
pub enum PlantProfile {
/// 理想一阶模型(无摩擦、无延迟)。
Gm3508,
Ideal,
/// 台架量级:静 / 库仑摩擦 + 2 拍延迟。
Gm3508Bench,
Bench,
}
impl PlantSpec {
impl PlantProfile {
/// 文本名。
#[must_use]
pub const fn name(self) -> &'static str {
match self {
Self::Gm3508 => "gm3508",
Self::Gm3508Bench => "gm3508-bench",
Self::Ideal => "ideal",
Self::Bench => "bench",
}
}
/// 展开为被控对象参数。
#[must_use]
pub fn params(self) -> PlantParams {
pub fn params<M: DcModelSpec>(self) -> PlantParams {
match self {
Self::Gm3508 => PlantParams::gm3508(),
Self::Gm3508Bench => PlantParams::gm3508_bench(),
Self::Ideal => M::ideal_params(),
Self::Bench => M::bench_params(),
}
}
}
@@ -1921,7 +1921,7 @@ pub mod preset {
/// 离线被控对象与域随机化。
SimSpec {
/// 名义模型。
plant: PlantSpec,
plant: PlantProfile,
/// 集成成员数(1 = 只用名义模型)。
ensemble: usize,
/// 鲁棒权重:0 = 只看均值,1 = 只看最坏成员。
@@ -2040,7 +2040,7 @@ pub mod preset {
threads: Some(0),
},
sim: SimSpec {
plant: Some(PlantSpec::Gm3508Bench),
plant: Some(PlantProfile::Bench),
ensemble: Some(8),
robust_weight: Some(0.5),
gain_pct: Some(0.2),
@@ -2082,8 +2082,8 @@ pub mod preset {
///
/// 缺项([`PresetError::Missing`])或区间 / 阈值 / 预算非法
/// ([`PresetError::Invalid`])时返回错误。
pub fn to_tuning(&self) -> Result<Tuning, PresetError> {
let plant_spec = self.sim.plant.unwrap_or(PlantSpec::Gm3508Bench);
pub fn to_tuning<M: DcModelSpec>(&self) -> Result<Tuning, PresetError> {
let plant_spec = self.sim.plant.unwrap_or(PlantProfile::Bench);
Ok(Tuning {
backend: self.backend.unwrap_or(BackendSpec::Sim),
space: self.gain_space()?,
@@ -2092,7 +2092,7 @@ pub mod preset {
acceptance: self.acceptance()?,
search: self.search()?,
plant_spec,
plant: plant_spec.params(),
plant: plant_spec.params::<M>(),
tolerances: self.tolerances()?,
ensemble: self.ensemble()?,
robust_weight: self.robust_weight()?,
@@ -2417,7 +2417,7 @@ pub mod preset {
/// 搜索配置。
pub search: SearchConfig,
/// 名义被控对象(枚举,供显示 / 复现)。
pub plant_spec: PlantSpec,
pub plant_spec: PlantProfile,
/// 名义被控对象(展开后的参数,供仿真重放)。
pub plant: PlantParams,
/// 域随机化幅度。
@@ -2641,14 +2641,16 @@ top_k = 3
let text = toml::to_string(&PresetFile::builtin()).unwrap();
let round_trip = PresetFile::parse_str(&text).unwrap();
assert_eq!(round_trip, PresetFile::builtin());
assert!(round_trip.select(None).unwrap().to_tuning().is_ok());
assert!(
round_trip.select(None).unwrap().to_tuning::<motor_model::Gm3508Model>().is_ok()
);
}
#[test]
fn builtin_matches_the_historical_cli_defaults() {
// 这些数字是 chassis-tune 历史的 CLI 默认值,改动等于改变整定的默认行为,
// 必须是有意为之。
let tuning = Preset::builtin().to_tuning().unwrap();
let tuning = Preset::builtin().to_tuning::<motor_model::Gm3508Model>().unwrap();
assert_eq!(tuning.backend, BackendSpec::Sim);
assert!((tuning.space.kp.min - 0.5).abs() < 1e-12);
assert!((tuning.space.kp.max - 30.0).abs() < 1e-12);
@@ -2671,7 +2673,7 @@ top_k = 3
fn preset_overlays_only_the_fields_it_sets() {
let file = PresetFile::parse_str(MINIMAL).unwrap();
let merged = Preset::builtin().overlay(file.select(None).unwrap());
let tuning = merged.to_tuning().unwrap();
let tuning = merged.to_tuning::<motor_model::Gm3508Model>().unwrap();
assert_eq!(tuning.search.samples, 32, "预设必须生效");
assert_eq!(tuning.search.top_k, 3);
// 未提及的项沿用内置默认。
@@ -2686,7 +2688,7 @@ top_k = 3
let mut overlay = Preset::default();
overlay.space.kp = Some(RangeSpec::new(1.0, 5.0));
let merged = Preset::builtin().overlay(&overlay);
let tuning = merged.to_tuning().unwrap();
let tuning = merged.to_tuning::<motor_model::Gm3508Model>().unwrap();
assert!((tuning.space.kp.min - 1.0).abs() < 1e-12);
assert!((tuning.space.kp.max - 5.0).abs() < 1e-12);
assert_eq!(tuning.space.kp.scale, Scale::Log, "刻度必须从内置默认继承");
@@ -2759,7 +2761,9 @@ top_k = 3
let build = |body: &str| -> Result<Tuning, PresetError> {
let text = format!("version = 1\n[preset.a]\n{body}");
let file = PresetFile::parse_str(&text)?;
Preset::builtin().overlay(file.select(None)?).to_tuning()
Preset::builtin()
.overlay(file.select(None)?)
.to_tuning::<motor_model::Gm3508Model>()
};
let error =
@@ -2794,13 +2798,13 @@ top_k = 3
// 缺 scale 的轴(内置换行默认提供了刻度,这里显式抹掉)。
let mut preset = Preset::builtin();
preset.space.kd = Some(RangeSpec { min: 0.1, max: 1.0, scale: None });
let error = preset.to_tuning().unwrap_err();
let error = preset.to_tuning::<motor_model::Gm3508Model>().unwrap_err();
assert!(matches!(&error, PresetError::Invalid { field: "kd", .. }), "{error}");
}
#[test]
fn missing_keys_are_reported_for_a_bare_preset() {
let error = Preset::default().to_tuning().unwrap_err();
let error = Preset::default().to_tuning::<motor_model::Gm3508Model>().unwrap_err();
assert!(matches!(error, PresetError::Missing { .. }), "{error}");
}
@@ -2810,16 +2814,16 @@ top_k = 3
preset.scenario.profile = Some(ProfileSpec::Staircase);
preset.scenario.stair_levels = None;
assert!(matches!(
preset.to_tuning(),
preset.to_tuning::<motor_model::Gm3508Model>(),
Err(PresetError::Missing { field: "scenario.stair_levels" })
));
preset.scenario.stair_levels = Some(0);
assert!(matches!(
preset.to_tuning(),
preset.to_tuning::<motor_model::Gm3508Model>(),
Err(PresetError::Invalid { field: "scenario.stair_levels", .. })
));
preset.scenario.stair_levels = Some(5);
assert!(preset.to_tuning().is_ok());
assert!(preset.to_tuning::<motor_model::Gm3508Model>().is_ok());
}
#[test]
@@ -2854,7 +2858,7 @@ top_k = 3
assert_eq!(spec.name(), text);
assert_eq!(parse(text).value, spec);
}
assert_eq!(PlantSpec::Gm3508Bench.name(), "gm3508-bench");
assert_eq!(PlantProfile::Bench.name(), "bench");
assert_eq!(SolverSpec::NelderMead.name(), "nelder-mead");
assert_eq!(BackendSpec::Bench.name(), "bench");
let profile: Wrapper<ProfileSpec> =
@@ -16,11 +16,10 @@ 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 }
[package.metadata.sentry-chassis]
layer = "transport/can"
@@ -19,3 +19,6 @@ serial = { workspace = true }
[dev-dependencies]
tokio = { workspace = true }
[package.metadata.sentry-chassis]
layer = "transport/serial"
@@ -0,0 +1,28 @@
[package]
name = "integration-tests"
description = "Cross-layer integration and hardware-simulation tests"
version.workspace = true
edition.workspace = true
rust-version.workspace = true
license.workspace = true
[lints]
workspace = true
[dependencies]
can = { workspace = true }
esc-bus = { workspace = true }
experiment = { workspace = true }
motor = { workspace = true }
motor-control = { workspace = true }
motor-model = { workspace = true }
motor-protocol = { workspace = true }
sim-harness = { workspace = true }
socketcan = { workspace = true }
tokio = { workspace = true }
[dev-dependencies]
tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "verification/integration"
@@ -0,0 +1 @@
//! Cross-layer integration test package.
+21 -7
View File
@@ -280,7 +280,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp
| 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 |
| 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 |
### 7.4 被控对象模型(`sim-harness::motor_model`)
### 7.4 被控对象模型(`motor-model::MotorPlant`)
逐拍离散(`dt` 秒,电调坐标系:正电流 → 正转速):
@@ -299,7 +299,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp
## 8. 配置参考(chassis.toml)
样例:[`config/chassis.toml.example`](../config/chassis.toml.example)。
schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程与整定工具共用同一份**——
schema 由 [`chassis-config`](../crates/config/chassis) 定义,**守护进程与整定工具共用同一份**——
这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。
### 8.1 顶层
@@ -308,11 +308,23 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程
| --- | --- |
| `[can] interface` | 底盘总线 SocketCAN 接口名(如 `can0`) |
| `[can.function] interface` | 预留的功能总线(云台 / 拨弹),必须与底盘总线**不同名** |
| `[remote]` | 可选 DR16/D-BUS 遥控输入;缺省时保持静止 |
| `[chassis]` | 控制循环与安全超时、速度上限 |
| `[mecanum]` | `half_length` / `half_width` / `wheel_radius`(米) |
| `[[motor]]` × 4 | 顺序固定 FL / FR / RL / RR |
### 8.2 `[chassis]`(缺省值)
### 8.2 `[remote]`(可选)
| 键 | 默认 | 含义 |
| --- | --- | --- |
| `kind` | 必填 | 当前支持 `dr16` |
| `device` | `/dev/ttyS2` | D-BUS UART 设备 |
| `timeout_ms` | 100 | 合法帧断流后立即失能 |
| `deadband` | 0.05 | 摇杆满量程死区,之后使用平方曲线 |
| `ramp_ms` | 500 | 零到满量程逐轴斜坡;失能/断流立即归零 |
| `invert_vx/vy/omega` | false | 实车方向校正 |
### 8.3 `[chassis]`(缺省值)
| 键 | 默认 | 含义 |
| --- | --- | --- |
@@ -322,7 +334,7 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程
| `motor_command_timeout_ms` | 50 | 驱动层命令租约(必须 ≥ `control_period_ms`) |
| `max_vx` / `max_vy` / `max_omega` | 3.0 / 3.0 / 6.0 | 速度限幅(m/s、m/s、rad/s) |
### 8.3 `[[motor]]`
### 8.4 `[[motor]]`
| 键 | 含义 |
| --- | --- |
@@ -334,12 +346,14 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程
| `current_limit` | effort 限幅(raw),同时是 PID 钳位点 |
| `kp` / `ki` / `kd` / `feedforward` | 速度环增益(整定的产物写回这里) |
### 8.4 校验规则(`Config::validate`)
### 8.5 校验规则(`Config::validate`)
- 电机数必须**恰好 4**;麦轮几何必须为正;四个超时必须 > 0;
`motor_command_timeout_ms ≥ control_period_ms`;速度上限 > 0;`gear_ratio > 0`;
- 底盘总线**只允许一种协议族**(四轮不能混用 C620 / C610);
- 功能总线接口名不得与底盘总线相同(双 CAN 物理隔离);
- `[remote]` 若启用:设备名非空,`timeout_ms` / `ramp_ms` > 0,`deadband ∈ [0, 0.5)`,
且遥控断流超时不得晚于底盘指令超时;
- `deny_unknown_fields`:**拼错的键直接报错**,不会被静默忽略。
---
@@ -441,7 +455,7 @@ chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预
| `field_updown` | bench | **up-down** 1200 rpm | 实战验收:升 / 降 / 回零双向,阈值收紧到 12% / 3% |
**预设 schema**(`version = 1`,未知键报错,见
[`crates/tuning/src/lib.rs`](../crates/tuning/src/lib.rs)):
[`crates/tooling/tuning/src/lib.rs`](../crates/tooling/tuning/src/lib.rs)):
```toml
version = 1
@@ -588,7 +602,7 @@ cargo run --release -p spin -- motor -- --interface can0 --dry-run
| 整定 | 指标(合成轨迹解析解)、参数空间(界内 / 确定性 / 对数分布 / 分层覆盖)、成本(硬约束罚值)、搜索(同 seed 逐位复现、优于手调基线)、报告(CSV/TOML/JSON 往返) | 全平台(CI 跑 `just tune-smoke`) |
| **预设** | schema 往返、逐字段覆盖、优先级(CLI > 预设 > 内置)、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 |
| vcan 集成 | vcan0 双 socket 收发,真实内核 CAN 栈 | Linux(`#[ignore]`,CI 显式跑) |
| 实车台架 | `spin chassis` 四轮定速:链路/反馈、方向、转速稳态、过速保护、安全停机;`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN(人工) |
| 实车台架 | `spin chassis` 四轮定速:链路/反馈、方向、转速稳态、过速保护、安全停机;`spin rc` 只读监视 DR16;`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN(人工) |
| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check;armv7 静态构建 `chassisd` | CI |
### 13.2 每个阶段完成后的门禁(`just lint` + 以下)
+50
View File
@@ -0,0 +1,50 @@
# 依赖分层与 Trait 边界
`config/dependency-policy.toml` 是依赖规则的唯一机器可读来源。crate 名称保持不变,manifest 按 `crates/<layer>/<domain>` 组织,并在 `[package.metadata.sentry-chassis]` 中声明 `layer`。
## 单向分层
```text
contract can/imu
protocol imu/motor/remote
transport can/serial
device imu/motor/esc/remote
control algorithm/chassis/motor
experiment trace
model motor
simulation motor
tooling tuning
config chassis
application bench/daemon/tools/tuning
verification integration
```
依赖只能从高层指向低层;正常依赖和 dev 依赖都必须如此。跨层集成测试集中在 `verification/integration`,不允许测试回边渗入低层 crate。
## Trait 组织
- `protocol/motor`:`MotorProtocol`、`EffortScale`、`FeedbackFields`,只处理线上的命令、反馈、帧 ID 和量程。
- `device/motor`:`MotorSpec`、`MotorFeedback`、`EffortDrive`、`VelocityDrive`、`PositionDrive`、`Disable`,只处理机械型号和设备能力。
- `device/esc`:`EscConfig<S>`、`EscMotor<S>`、`EscBus<S>` 通过 `MotorSpec::Protocol` 组合协议能力;电流路径要求 `CurrentEscDrive`,GM6020 使用独立的 `VoltageEscDrive` 路径。
- `model/motor`:`MotorPlant`、`DcMotorPlant<S>`,只处理动态模型和反馈量化。
`Gm3508`、`M2006`、`Gm6020` 是 marker type;型号事实、协议族和控制能力由关联类型组合,不在 bus 内用运行时 `match` 判断。
## 外部库边界
外部依赖必须从根 `workspace.dependencies` 继承,并在 policy 的 `external_roles` 中声明用途。异步/传输库只进入 transport/device;配置和 CLI 库只进入 config/application;搜索、并行、绘图库只进入 tuning/application;`approx`、`proptest` 只允许作为测试依赖。
## 审查脚本
`scripts/audit-rules.rs` 是单文件 Rust `cargo-script`。脚本通过内嵌 manifest 声明
`serde`、`serde_json`、`toml` 和 `regex`,不需要额外 Python 工具链。运行时使用
Nightly 的 `-Zscript`;workspace 本身仍由 `rust-toolchain.toml` 固定 Stable。
```sh
cargo +nightly -Zscript scripts/audit-rules.rs
cargo +nightly -Zscript scripts/audit-rules.rs --self-test
```
## 变更流程
新增 crate、内部边或外部库时,必须同步更新 `config/dependency-policy.toml`、crate 的 layer 声明、架构文档和 `Cargo.lock`。运行 `just audit`,提交时由 `.githooks/pre-commit` 执行同一规则。
+70
View File
@@ -0,0 +1,70 @@
# DT7 + DR16 遥控底盘接入
这里的 `dt16` 目录沿用现有资料目录名;硬件是 **DT7 遥控器 + DR16 接收机**。接收机通过
D-BUS 输出 18 字节帧,软件中的协议名称统一为 `DR16`。
## 协议与接线
- DR16 `DBUS` 接 T153 原生 UART RX,默认设备 `/dev/ttyS2`。
- UART 参数固定为 `100000 baud, 8 data bits, even parity, 1 stop bit`,无流控。
- 接收机 `VCC/GND/DBUS` 使用 3-Pin 线;确认 T153 UART RX 电平与 DBUS 信号兼容后再上电。
- 本实现的字段布局参考 DT7/DR16 手册和 `RoboMaster/RoboRTS-Firmware` 的 DR16 解码,
Rust 代码为独立重写,不复制参考仓库源码。
## 摇杆与安全语义
| 输入 | 通道 | 底盘动作 |
| --- | --- | --- |
| 左摇杆上下 | `T/ch3` | 前后平移,上为前进 |
| 左摇杆左右 | `R/ch4` | 旋转,右为顺时针(负 omega) |
| 右摇杆左右 | `A/ch1` | 左右平移,右为右移(负 vy) |
| 右摇杆上下 | `E/ch2` | 本次不绑定动作 |
| S1 上档 | wire value `1` | 允许运动 |
| S1 中/下档 | wire value `3`/`2` | 立即停车 |
摇杆满量程为协议中心值 ±660。5% 死区外使用平方曲线,满杆对应
`chassis.max_vx/max_vy/max_omega`。速度从零到满量程使用 `ramp_ms = 500` 的逐轴斜坡;
S1 离开上档、合法帧断流超过 `timeout_ms = 100`、串口关闭时立即归零,不走斜坡。
## 配置
在 `/etc/chassisd/chassis.toml` 增加:
```toml
[remote]
kind = "dr16"
device = "/dev/ttyS2"
timeout_ms = 100
deadband = 0.05
ramp_ms = 500
invert_vx = false
invert_vy = false
invert_omega = false
```
如果某个轴的实车方向相反,只翻转对应的 `invert_*`。`chassisd --rc-device /dev/ttyS2`
可以覆盖配置中的设备节点;没有 `[remote]` 段时守护进程保持安全静止。
## 只读监视
先不要让车轮着地,使用只读命令观察帧和方向:
```sh
cargo run --release -p spin -- rc --device /dev/ttyS2
```
输出包含帧龄、A/E/T/R 原始值、S1/S2 位置和显示用 `vx/vy/omega`(默认满量程 3.0/3.0/6.0)。它不打开 CAN,也不驱动电机。
## 实车验收
1. S1 置中档或下档,执行 `spin rc --device /dev/ttyS2`,确认帧龄约 14 ms、四个通道随摇杆变化、S1 上档 wire value 为 `1`。
2. 架空四轮,执行 `chassisd --dry-run --config /etc/chassisd/chassis.toml`,确认 UART 与四个电调反馈正常。
3. S1 上档后低速验证:左杆上/下 = 前/后,右杆左/右 = 左/右平移,左杆左/右 = 逆/顺时针。
4. 运动中把 S1 拨离上档,必须立即停车。
5. 运动中关闭遥控器或拔掉 DBUS,必须在 100 ms 内停车。
6. 若方向相反,修改对应 `invert_*` 后重复步骤 3,再落地低速复验。
## 参考资料
- `DT7&DR16_RC_System_User_Manual_v2.00_cn.pdf`
- [RoboMaster/RoboRTS-Firmware](https://github.com/RoboMaster/RoboRTS-Firmware)
+21 -3
View File
@@ -11,18 +11,36 @@ lint:
test:
cargo test --workspace --all-targets
audit:
cargo +nightly -Zscript scripts/audit-rules.rs
audit-test:
cargo +nightly -Zscript scripts/audit-rules.rs --self-test
install-hooks:
git config --local core.hooksPath .githooks
chmod +x .githooks/pre-commit
check:
just fmt
just lint
just test
just audit
vcan-test:
sudo modprobe vcan
sudo ip link add dev vcan0 type vcan || true
sudo ip link set up vcan0
cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1
cargo test -p integration-tests --test socketcan_vcan -- --ignored --test-threads 1
run-mock:
cargo run -p chassisd -- --mock
rc-monitor device="/dev/ttyS2":
cargo run --release -p spin -- rc --device {{device}}
check-structure:
test -z "$(find crates/*/src -mindepth 2 -type d -print)"
! rg -n '\bunsafe\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml'
cargo +nightly -Zscript scripts/audit-rules.rs
build-armv7:
rustup target add armv7-unknown-linux-musleabihf
+961
View File
@@ -0,0 +1,961 @@
#!/usr/bin/env -S cargo +nightly -Zscript
---
[package]
edition = "2024"
[dependencies]
regex = "1"
serde = { version = "1", features = ["derive"] }
serde_json = "1"
toml = "0.9"
---
use regex::Regex;
use serde::Serialize;
use serde_json::Value;
use std::collections::{BTreeMap, BTreeSet, HashSet};
use std::env;
use std::error::Error;
use std::fs;
use std::path::{Path, PathBuf};
use std::process::{Command, Output};
use toml::{Table, Value as TomlValue};
type AnyError = Box<dyn Error>;
const POLICY_RELATIVE: &str = "config/dependency-policy.toml";
#[derive(Debug, Clone, Serialize)]
struct Issue {
code: String,
message: String,
#[serde(skip_serializing_if = "Option::is_none")]
package: Option<String>,
#[serde(skip_serializing_if = "Option::is_none")]
path: Option<String>,
}
impl Issue {
fn new(code: impl Into<String>, message: impl Into<String>) -> Self {
Self {
code: code.into(),
message: message.into(),
package: None,
path: None,
}
}
fn package(mut self, package: impl Into<String>) -> Self {
self.package = Some(package.into());
self
}
fn path(mut self, path: impl AsRef<Path>) -> Self {
self.path = Some(path.as_ref().display().to_string());
self
}
}
#[derive(Debug, Clone)]
struct Package {
name: String,
manifest_path: PathBuf,
package_dir: PathBuf,
metadata: Value,
dependencies: Vec<Value>,
}
#[derive(Debug, Default)]
struct Graph {
normal: BTreeMap<String, BTreeSet<String>>,
dev: BTreeMap<String, BTreeSet<String>>,
}
fn root_dir() -> Result<PathBuf, AnyError> {
let start = env::current_dir()?;
start
.ancestors()
.find(|directory| directory.join(POLICY_RELATIVE).is_file())
.map(Path::to_path_buf)
.ok_or_else(|| "run from inside the sentry-chassis workspace".into())
}
fn read_toml(path: &Path) -> Result<Table, AnyError> {
let text = fs::read_to_string(path)?;
Ok(text.parse::<Table>()?)
}
fn run(command: &mut Command) -> Result<Output, AnyError> {
let output = command.output()?;
if !output.status.success() {
let stderr = String::from_utf8_lossy(&output.stderr);
return Err(format!("command failed: {stderr}").into());
}
Ok(output)
}
fn cargo_metadata(root: &Path) -> Result<Value, AnyError> {
let output = run(Command::new("cargo")
.current_dir(root)
.args(["metadata", "--locked", "--format-version", "1", "--no-deps"]))?;
Ok(serde_json::from_slice(&output.stdout)?)
}
fn string_array(value: Option<&TomlValue>) -> BTreeSet<String> {
value
.and_then(TomlValue::as_array)
.into_iter()
.flatten()
.filter_map(TomlValue::as_str)
.map(str::to_owned)
.collect()
}
fn toml_string<'a>(table: &'a Table, key: &str) -> Result<&'a str, AnyError> {
table
.get(key)
.and_then(TomlValue::as_str)
.ok_or_else(|| format!("policy field {key} must be a string").into())
}
fn toml_i64(table: &Table, key: &str) -> Result<i64, AnyError> {
table
.get(key)
.and_then(TomlValue::as_integer)
.ok_or_else(|| format!("policy field {key} must be an integer").into())
}
fn toml_table<'a>(table: &'a Table, key: &str) -> Result<&'a Table, AnyError> {
table
.get(key)
.and_then(TomlValue::as_table)
.ok_or_else(|| format!("policy field {key} must be a table").into())
}
fn package_map(metadata: &Value) -> Result<BTreeMap<String, Package>, AnyError> {
let members: HashSet<&str> = metadata["workspace_members"]
.as_array()
.ok_or("workspace_members must be an array")?
.iter()
.filter_map(Value::as_str)
.collect();
let mut packages = BTreeMap::new();
for item in metadata["packages"]
.as_array()
.ok_or("packages must be an array")?
{
if !members.contains(item["id"].as_str().unwrap_or_default()) {
continue;
}
let name = item["name"]
.as_str()
.ok_or("workspace package has no name")?
.to_owned();
let manifest_path = PathBuf::from(
item["manifest_path"]
.as_str()
.ok_or_else(|| format!("package {name} has no manifest_path"))?,
);
let package_dir = manifest_path
.parent()
.ok_or_else(|| format!("package {name} has no package directory"))?
.to_path_buf();
packages.insert(
name.clone(),
Package {
name,
package_dir,
manifest_path,
metadata: item.clone(),
dependencies: item["dependencies"]
.as_array()
.cloned()
.unwrap_or_default(),
},
);
}
Ok(packages)
}
fn package_name_by_path(packages: &BTreeMap<String, Package>) -> BTreeMap<PathBuf, String> {
packages
.values()
.map(|package| (package.package_dir.clone(), package.name.clone()))
.collect()
}
fn dependency_target(
dependency: &Value,
by_path: &BTreeMap<PathBuf, String>,
) -> Option<String> {
let path = dependency.get("path")?.as_str()?;
by_path.get(&PathBuf::from(path)).cloned()
}
fn build_graph(
packages: &BTreeMap<String, Package>,
) -> Graph {
let by_path = package_name_by_path(packages);
let mut graph = Graph::default();
for package in packages.values() {
graph.normal.entry(package.name.clone()).or_default();
graph.dev.entry(package.name.clone()).or_default();
for dependency in &package.dependencies {
let Some(target) = dependency_target(dependency, &by_path) else {
continue;
};
let kind = dependency["kind"].as_str().unwrap_or("normal");
let edges = if kind == "dev" {
&mut graph.dev
} else {
&mut graph.normal
};
edges
.entry(package.name.clone())
.or_default()
.insert(target);
}
}
graph
}
fn check_rank(source: &str, target: &str, ranks: &BTreeMap<String, i64>) -> bool {
match (ranks.get(source), ranks.get(target)) {
(Some(source_rank), Some(target_rank)) => target_rank < source_rank,
_ => false,
}
}
fn check_layer_path(layer: &str, relative_path: &str) -> bool {
let parts: Vec<String> = Path::new(relative_path)
.components()
.map(|part| part.as_os_str().to_string_lossy().into_owned())
.collect();
parts.len() == 3 && parts[0] == "crates" && format!("{}/{}", parts[1], parts[2]) == layer
}
fn contains_forbidden_marker(text: &str) -> bool {
let marker = Regex::new(r"\bunsafe\b|libc|no_std").expect("valid marker regex");
marker.is_match(text)
}
fn declares_trait(text: &str, symbol: &str) -> bool {
let pattern = format!(r"\bpub trait {}\b", regex::escape(symbol));
Regex::new(&pattern)
.expect("valid trait regex")
.is_match(text)
}
fn contains_forbidden_symbol(text: &str, symbol: &str) -> bool {
let pattern = format!(r"\b{}\b", regex::escape(symbol));
Regex::new(&pattern)
.expect("valid symbol regex")
.is_match(text)
}
fn dependency_issues(
kind: &str,
source: &str,
actual: &BTreeSet<String>,
allowed: &BTreeSet<String>,
) -> Vec<Issue> {
let mut issues = actual
.difference(allowed)
.map(|target| {
Issue::new(
"dependency-edge",
format!("{kind} dependency {source} -> {target} is not declared"),
)
.package(source)
})
.collect::<Vec<_>>();
issues.extend(allowed.difference(actual).map(|target| {
Issue::new(
"dependency-edge",
format!("declared {kind} dependency {source} -> {target} is absent"),
)
.package(source)
}));
issues
}
fn layer_declaration_matches(declared: Option<&str>, expected: &str) -> bool {
declared == Some(expected)
}
fn find_cycles(edges: &BTreeMap<String, BTreeSet<String>>) -> Vec<Vec<String>> {
#[derive(Clone, Copy, PartialEq)]
enum Color {
White,
Gray,
Black,
}
fn visit(
node: &str,
edges: &BTreeMap<String, BTreeSet<String>>,
colors: &mut BTreeMap<String, Color>,
trail: &mut Vec<String>,
cycles: &mut Vec<Vec<String>>,
) {
colors.insert(node.to_owned(), Color::Gray);
trail.push(node.to_owned());
for target in edges.get(node).into_iter().flatten() {
match colors.get(target).copied().unwrap_or(Color::White) {
Color::Gray => {
let mut cycle = trail.clone();
cycle.push(target.clone());
cycles.push(cycle);
}
Color::White => visit(target, edges, colors, trail, cycles),
Color::Black => {}
}
}
trail.pop();
colors.insert(node.to_owned(), Color::Black);
}
let mut colors = BTreeMap::new();
let mut cycles = Vec::new();
for node in edges.keys() {
if colors.get(node).copied().unwrap_or(Color::White) == Color::White {
visit(node, edges, &mut colors, &mut Vec::new(), &mut cycles);
}
}
cycles
}
fn package_declared_layer(package: &Package) -> Option<String> {
package
.metadata
.get("metadata")
.and_then(|metadata| metadata.get("sentry-chassis"))
.and_then(|metadata| metadata.get("layer"))
.and_then(Value::as_str)
.map(str::to_owned)
}
fn policy_packages(policy: &Table) -> Result<&Table, AnyError> {
toml_table(policy, "packages")
}
fn policy_package<'a>(policy: &'a Table, name: &str) -> Result<&'a Table, AnyError> {
policy_packages(policy)?
.get(name)
.and_then(TomlValue::as_table)
.ok_or_else(|| format!("policy package {name} is missing").into())
}
fn rust_files_in(directory: &Path) -> Vec<PathBuf> {
fs::read_dir(directory)
.into_iter()
.flatten()
.filter_map(Result::ok)
.map(|entry| entry.path())
.filter(|path| path.extension().is_some_and(|extension| extension == "rs"))
.collect()
}
fn check_layout_and_markers(
root: &Path,
packages: &BTreeMap<String, Package>,
) -> Result<Vec<Issue>, AnyError> {
let mut issues = Vec::new();
for package in packages.values() {
let src = package.package_dir.join("src");
let tests = package.package_dir.join("tests");
let mut rust_files = rust_files_in(&src);
rust_files.extend(rust_files_in(&tests));
if !src.is_dir() || rust_files.iter().all(|path| path.parent() != Some(src.as_path())) {
issues.push(
Issue::new("layout", "package must have src/*.rs")
.package(&package.name)
.path(&src),
);
}
for directory in [&src, &tests] {
if !directory.is_dir() {
continue;
}
for entry in fs::read_dir(directory)? {
let entry = entry?;
if entry.file_type()?.is_dir() {
issues.push(
Issue::new("layout", "source/test directories must remain flat")
.package(&package.name)
.path(entry.path()),
);
}
}
}
let mut scan_paths = vec![package.manifest_path.clone()];
scan_paths.extend(rust_files);
for path in scan_paths {
let text = fs::read_to_string(&path)?;
if contains_forbidden_marker(&text) {
issues.push(
Issue::new("forbidden-marker", "unsafe/libc/no_std marker found")
.package(&package.name)
.path(path),
);
}
}
}
let _ = root;
Ok(issues)
}
fn dependency_tables(manifest: &Table) -> Vec<&Table> {
let mut tables = Vec::new();
for key in ["dependencies", "dev-dependencies"] {
if let Some(table) = manifest.get(key).and_then(TomlValue::as_table) {
tables.push(table);
}
}
if let Some(targets) = manifest.get("target").and_then(TomlValue::as_table) {
for target in targets.values().filter_map(TomlValue::as_table) {
for key in ["dependencies", "dev-dependencies"] {
if let Some(table) = target.get(key).and_then(TomlValue::as_table) {
tables.push(table);
}
}
}
}
tables
}
fn check_workspace_inheritance(
root: &Path,
packages: &BTreeMap<String, Package>,
) -> Result<Vec<Issue>, AnyError> {
let root_manifest = read_toml(&root.join("Cargo.toml"))?;
let workspace = toml_table(&root_manifest, "workspace")?;
let aliases = toml_table(workspace, "dependencies")?
.keys()
.cloned()
.collect::<BTreeSet<_>>();
let mut issues = Vec::new();
for package in packages.values() {
let manifest = read_toml(&package.manifest_path)?;
for table in dependency_tables(&manifest) {
for (dependency, specification) in table {
if !aliases.contains(dependency) {
issues.push(
Issue::new(
"workspace-inheritance",
format!("{dependency} is not declared in workspace.dependencies"),
)
.package(&package.name),
);
}
let Some(specification) = specification.as_table() else {
continue;
};
if !specification
.get("workspace")
.and_then(TomlValue::as_bool)
.unwrap_or(false)
{
issues.push(
Issue::new(
"workspace-inheritance",
format!("{dependency} must use workspace = true"),
)
.package(&package.name),
);
}
if specification.contains_key("git") || specification.contains_key("path") {
issues.push(
Issue::new(
"external-source",
format!("{dependency} cannot use git/path directly"),
)
.package(&package.name),
);
}
if specification.get("version").and_then(TomlValue::as_str) == Some("*") {
issues.push(
Issue::new(
"version-policy",
format!("{dependency} cannot use wildcard version"),
)
.package(&package.name),
);
}
}
}
}
Ok(issues)
}
fn check_trait_boundaries(root: &Path, policy: &Table) -> Result<Vec<Issue>, AnyError> {
let boundaries = toml_table(policy, "trait_boundaries")?;
let required = [
("protocol_motor", root.join("crates/protocol/motor/src/lib.rs")),
("device_motor", root.join("crates/device/motor/src/lib.rs")),
("model_motor", root.join("crates/model/motor/src/lib.rs")),
];
let mut issues = Vec::new();
for (key, path) in required {
let text = fs::read_to_string(&path)?;
for symbol in string_array(boundaries.get(key)) {
if !declares_trait(&text, &symbol) {
issues.push(
Issue::new(
"trait-boundary",
format!("{symbol} must be declared in {}", path.display()),
)
.path(&path),
);
}
}
}
let esc_path = root.join("crates/device/esc/src/lib.rs");
let esc_text = fs::read_to_string(&esc_path)?;
for symbol in string_array(boundaries.get("esc_bus_forbidden_symbols")) {
if contains_forbidden_symbol(&esc_text, &symbol) {
issues.push(
Issue::new(
"trait-boundary",
format!("{symbol} must not branch in esc-bus"),
)
.path(&esc_path),
);
}
}
Ok(issues)
}
fn external_dependencies(package: &Package) -> (BTreeSet<String>, BTreeSet<String>) {
let mut normal = BTreeSet::new();
let mut dev = BTreeSet::new();
for dependency in &package.dependencies {
if !dependency
.get("source")
.is_some_and(|source| !source.is_null())
{
continue;
}
let Some(name) = dependency["name"].as_str() else {
continue;
};
if dependency["kind"].as_str() == Some("dev") {
dev.insert(name.to_owned());
} else {
normal.insert(name.to_owned());
}
}
(normal, dev)
}
fn audit(root: &Path) -> Result<Vec<Issue>, AnyError> {
let policy = read_toml(&root.join(POLICY_RELATIVE))?;
let metadata = cargo_metadata(root)?;
let packages = package_map(&metadata)?;
let graph = build_graph(&packages);
let mut issues = Vec::new();
let expected = policy_packages(&policy)?
.keys()
.cloned()
.collect::<BTreeSet<_>>();
let actual = packages.keys().cloned().collect::<BTreeSet<_>>();
for name in expected.difference(&actual) {
issues.push(
Issue::new("workspace", format!("missing workspace package {name}"))
.package(name),
);
}
for name in actual.difference(&expected) {
issues.push(
Issue::new("workspace", format!("undeclared workspace package {name}"))
.package(name),
);
}
let layer_ranks = toml_table(&policy, "layers")?
.iter()
.map(|(layer, rank)| {
Ok((
layer.clone(),
rank.as_integer()
.ok_or_else(|| format!("layer {layer} rank must be an integer"))?,
))
})
.collect::<Result<BTreeMap<String, i64>, AnyError>>()?;
let package_ranks = policy_packages(&policy)?
.iter()
.map(|(name, specification)| {
let specification = specification
.as_table()
.ok_or_else(|| format!("package {name} policy must be a table"))?;
Ok((name.clone(), toml_i64(specification, "rank")?))
})
.collect::<Result<BTreeMap<String, i64>, AnyError>>()?;
for package in packages.values() {
let specification = match policy_package(&policy, &package.name) {
Ok(specification) => specification,
Err(error) => {
issues.push(Issue::new("workspace", error.to_string()).package(&package.name));
continue;
}
};
let expected_layer = toml_string(specification, "layer")?;
let declared_layer = package_declared_layer(package);
if !layer_declaration_matches(declared_layer.as_deref(), expected_layer) {
issues.push(
Issue::new(
"layer-declaration",
format!(
"declared layer {:?} != policy {expected_layer}",
declared_layer
),
)
.package(&package.name),
);
}
let relative = package
.package_dir
.strip_prefix(root)
.map_err(|_| format!("package path escapes workspace root"))?
.display()
.to_string();
let expected_path = toml_string(specification, "path")?;
if relative != expected_path {
issues.push(
Issue::new(
"layer-path",
format!("path {relative} != policy {expected_path}"),
)
.package(&package.name),
);
}
if !check_layer_path(expected_layer, &relative) {
issues.push(
Issue::new(
"layer-path",
format!("path does not match exactly two-part layer {expected_layer}"),
)
.package(&package.name),
);
}
let rank = toml_i64(specification, "rank")?;
if layer_ranks.get(expected_layer) != Some(&rank) {
issues.push(
Issue::new("layer-rank", "package rank differs from layer rank")
.package(&package.name),
);
}
for (kind, actual_edges) in [
("normal", graph.normal.get(&package.name).cloned().unwrap_or_default()),
("dev", graph.dev.get(&package.name).cloned().unwrap_or_default()),
] {
let allowed = string_array(specification.get(kind));
issues.extend(dependency_issues(
kind,
&package.name,
&actual_edges,
&allowed,
));
for target in &actual_edges {
if !check_rank(&package.name, target, &package_ranks) {
issues.push(
Issue::new(
"layer-order",
format!("{kind} dependency {} -> {target} is not downward", package.name),
)
.package(&package.name),
);
}
}
}
let (external, dev_external) = external_dependencies(package);
for (key, actual_external) in [
("external", external.clone()),
("dev_external", dev_external.clone()),
] {
let allowed = string_array(specification.get(key));
for dependency in actual_external.difference(&allowed) {
issues.push(
Issue::new(
"external-edge",
format!("{key} dependency {dependency} is not declared"),
)
.package(&package.name),
);
}
for dependency in allowed.difference(&actual_external) {
issues.push(
Issue::new(
"external-edge",
format!("declared {key} dependency {dependency} is absent"),
)
.package(&package.name),
);
}
}
let top_layer = expected_layer
.split('/')
.next()
.ok_or("layer must contain a slash")?;
let roles = toml_table(&policy, "external_roles")?;
for dependency in external.union(&dev_external) {
let Some(allowed_layers) = roles.get(dependency).and_then(TomlValue::as_array) else {
issues.push(
Issue::new(
"external-role",
format!("external dependency {dependency} has no role declaration"),
)
.package(&package.name),
);
continue;
};
let allowed = allowed_layers
.iter()
.filter_map(TomlValue::as_str)
.any(|layer| layer == top_layer);
if !allowed {
issues.push(
Issue::new(
"external-role",
format!(
"external dependency {dependency} is not allowed in {expected_layer}"
),
)
.package(&package.name),
);
}
}
}
let forbidden = toml_table(&policy, "forbidden")?;
let forbidden_normal = toml_table(forbidden, "normal")?;
for (source, targets) in forbidden_normal {
let source = source.replace('_', "-");
for target in string_array(Some(targets)) {
if graph
.normal
.get(&source)
.is_some_and(|edges| edges.contains(&target))
{
issues.push(
Issue::new(
"forbidden-edge",
format!("{source} -> {target} is forbidden"),
)
.package(&source),
);
}
}
}
let mut all_edges = graph.normal.clone();
for (source, targets) in &graph.dev {
all_edges.entry(source.clone()).or_default().extend(targets.clone());
}
for cycle in find_cycles(&all_edges) {
issues.push(Issue::new(
"dependency-cycle",
cycle.join(" -> "),
));
}
issues.extend(check_layout_and_markers(root, &packages)?);
issues.extend(check_workspace_inheritance(root, &packages)?);
issues.extend(check_trait_boundaries(root, &policy)?);
Ok(issues)
}
fn git_lines(root: &Path, args: &[&str]) -> Result<Vec<String>, AnyError> {
let output = run(Command::new("git").current_dir(root).args(args))?;
Ok(String::from_utf8(output.stdout)?
.lines()
.map(str::to_owned)
.collect())
}
fn is_audit_path(path: &str) -> bool {
const EXACT: &[&str] = &[
"Cargo.toml",
"Cargo.lock",
"justfile",
"ARCHITECTURE.md",
"README.md",
];
const PREFIXES: &[&str] = &[
"crates/",
"config/",
"scripts/",
".githooks/",
".github/",
"docs/",
];
EXACT.contains(&path) || PREFIXES.iter().any(|prefix| path.starts_with(prefix))
}
fn staged_guard(root: &Path) -> Result<Vec<Issue>, AnyError> {
let staged: BTreeSet<String> = git_lines(
root,
&["diff", "--cached", "--name-only", "--diff-filter=ACMR"],
)?
.into_iter()
.collect();
let unstaged: BTreeSet<String> = git_lines(
root,
&["diff", "--name-only", "--diff-filter=ACMR"],
)?
.into_iter()
.collect();
Ok(staged
.intersection(&unstaged)
.filter(|path| is_audit_path(path))
.map(|path| {
Issue::new(
"staged-mixed",
format!("{path} has both staged and unstaged changes; stage or restore it before committing"),
)
.path(path)
})
.collect())
}
fn self_test() -> Result<(), AnyError> {
let mut ranks = BTreeMap::new();
ranks.insert("device/esc".to_owned(), 4_i64);
ranks.insert("control/motor".to_owned(), 6);
ranks.insert("device/motor".to_owned(), 3);
assert!(check_rank("device/esc", "device/motor", &ranks));
assert!(!check_rank("device/motor", "device/esc", &ranks));
assert!(!check_rank("device/esc", "device/esc", &ranks));
assert!(check_layer_path("protocol/motor", "crates/protocol/motor"));
assert!(!check_layer_path(
"protocol/motor",
"crates/protocol/motor/motor-protocol"
));
let mut edges = BTreeMap::new();
edges.insert("a".to_owned(), BTreeSet::from(["b".to_owned()]));
edges.insert("b".to_owned(), BTreeSet::from(["a".to_owned()]));
assert_eq!(find_cycles(&edges), vec![vec!["a", "b", "a"]]);
let issues = dependency_issues(
"dev",
"device/esc",
&BTreeSet::from(["control/motor".to_owned()]),
&BTreeSet::new(),
);
assert_eq!(issues.len(), 1);
assert_eq!(issues[0].code, "dependency-edge");
assert!(layer_declaration_matches(Some("protocol/motor"), "protocol/motor"));
assert!(!layer_declaration_matches(Some("device/motor"), "protocol/motor"));
assert!(contains_forbidden_marker("use libc::c_int;"));
assert!(!contains_forbidden_marker("use std::num::NonZeroU8;"));
assert!(declares_trait("pub trait MotorProtocol {}", "MotorProtocol"));
assert!(!declares_trait("struct MotorProtocol;", "MotorProtocol"));
assert!(contains_forbidden_symbol("match EscModel {}", "EscModel"));
println!("audit-rules self-test: ok");
Ok(())
}
fn parse_args() -> Result<(bool, bool, String), AnyError> {
let mut staged = false;
let mut self_test_flag = false;
let mut format = "text".to_owned();
let mut args = env::args().skip(1);
while let Some(argument) = args.next() {
match argument.as_str() {
"--staged" => staged = true,
"--self-test" => self_test_flag = true,
"--format" => {
format = args
.next()
.ok_or("--format requires text or json")?;
if format != "text" && format != "json" {
return Err("--format must be text or json".into());
}
}
other => return Err(format!("unknown argument: {other}").into()),
}
}
Ok((staged, self_test_flag, format))
}
fn main() -> Result<(), AnyError> {
let (staged, self_test_flag, format) = parse_args()?;
if self_test_flag {
return self_test();
}
let root = root_dir()?;
let mut issues = if staged { staged_guard(&root)? } else { Vec::new() };
match audit(&root) {
Ok(mut audit_issues) => issues.append(&mut audit_issues),
Err(error) => issues.push(Issue::new("audit-failed", error.to_string())),
}
if format == "json" {
println!("{}", serde_json::to_string_pretty(&issues)?);
} else {
for issue in &issues {
let package = issue
.package
.as_ref()
.map(|package| format!(" ({package})"))
.unwrap_or_default();
let path = issue
.path
.as_ref()
.map(|path| format!(" [{path}]"))
.unwrap_or_default();
println!("{}{package}{path}: {}", issue.code, issue.message);
}
}
if issues.is_empty() {
Ok(())
} else {
Err("audit rules found violations".into())
}
}