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 - uses: dtolnay/rust-toolchain@stable
with: with:
components: rustfmt, clippy components: rustfmt, clippy
- uses: dtolnay/rust-toolchain@nightly
- uses: Swatinem/rust-cache@v2 - 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 fmt --all -- --check
- run: cargo clippy --workspace --all-targets -- -D warnings - run: cargo clippy --workspace --all-targets -- -D warnings
- run: cargo test --workspace --all-targets - 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 - name: optional plot feature
run: cargo check -p chassis-tune --features plot --all-targets run: cargo check -p chassis-tune --features plot --all-targets
- name: deterministic tuning smoke - name: deterministic tuning smoke
@@ -42,7 +41,7 @@ jobs:
sudo modprobe vcan sudo modprobe vcan
sudo ip link add dev vcan0 type vcan sudo ip link add dev vcan0 type vcan
sudo ip link set up vcan0 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: cross-check:
runs-on: ubuntu-latest runs-on: ubuntu-latest
+3
View File
@@ -1 +1,4 @@
/target /target
__pycache__/
*.pyc
+17 -4
View File
@@ -13,6 +13,7 @@
- **依赖必须无环**:允许依赖同层或更低抽象,但禁止反向控制依赖和隐藏的完整链路打包。 - **依赖必须无环**:允许依赖同层或更低抽象,但禁止反向控制依赖和隐藏的完整链路打包。
- **std,禁止 unsafe**:workspace 使用 `unsafe_code = "forbid"`;允许使用提供安全 API 的第三方依赖。 - **std,禁止 unsafe**:workspace 使用 `unsafe_code = "forbid"`;允许使用提供安全 API 的第三方依赖。
- **实时调度交给 systemd**:进程不调用 libc FFI,不实现 `SCHED_FIFO` 或 `mlockall`。 - **实时调度交给 systemd**:进程不调用 libc FFI,不实现 `SCHED_FIFO` 或 `mlockall`。
- **二级语义分层**:crate 名不变,manifest 按 `crates/<layer>/<domain>` 组织;依赖只能由高层指向低层。
- **crate 内保持扁平**:只允许 Cargo 惯例目录 `src`、`tests`;`src` 内不建立模块子目录。 - **crate 内保持扁平**:只允许 Cargo 惯例目录 `src`、`tests`;`src` 内不建立模块子目录。
- **安全边界独立于控制律**:总线负责反馈新鲜度、命令租约和零电流;控制器通过策略接口组合。 - **安全边界独立于控制律**:总线负责反馈新鲜度、命令租约和零电流;控制器通过策略接口组合。
@@ -26,11 +27,13 @@
协议 协议
motor-protocol C620/C610、GM6020 编解码、多圈与 ID motor-protocol C620/C610、GM6020 编解码、多圈与 ID
delta10-protocol DETA10 帧布局与解码 delta10-protocol DETA10 帧布局与解码
dr16-protocol DJI DR16 D-BUS 帧布局与解码
驱动 驱动
socketcan Linux SocketCAN ↔ CanLink socketcan Linux SocketCAN ↔ CanLink
slcan Lawicel 串口 CAN ↔ CanLink slcan Lawicel 串口 CAN ↔ CanLink
delta10 DETA10 IMU 驱动 delta10 DETA10 IMU 驱动
dr16 DR16 D-BUS 串口接收驱动
设备与运行时 设备与运行时
motor Motor 能力 trait、状态、命令、减速比、注册表、EffortPolicy motor Motor 能力 trait、状态、命令、减速比、注册表、EffortPolicy
@@ -53,20 +56,29 @@
chassisd 底盘守护进程二进制 chassisd 底盘守护进程二进制
spin chassis / motor 台架二进制 spin chassis / motor 台架二进制
chassis-tune 自动整定二进制与报告/绘图适配 chassis-tune 自动整定二进制与报告/绘图适配
验证
integration-tests 跨层闭环、语义一致性与 vcan 集成测试
``` ```
共 21 个 crate。crate 名不使用 `rm-` 等无领域含义的统一前缀。 共 24 个 crate(含顶层 `verification/integration` 集成测试包)。crate 名不使用 `rm-` 等无领域含义的统一前缀。
## 3. 允许依赖 ## 3. 允许依赖
机器可读规则见 [`config/dependency-policy.toml`](config/dependency-policy.toml) 和
[`docs/dependencies.md`](docs/dependencies.md)。`just audit` 同时检查层 rank、正常/dev 依赖、
外部库职责和 trait 边界;正常与 dev 依赖都必须单向。
主要边如下: 主要边如下:
```text ```text
motor-protocol -> can motor-protocol -> can
delta10-protocol -> can, imu delta10-protocol -> can, imu
dr16-protocol -> (无内部依赖)
socketcan -> can socketcan -> can
slcan -> can slcan -> can
delta10 -> can, imu, delta10-protocol delta10 -> can, imu, delta10-protocol
dr16 -> dr16-protocol
motor -> can, motor-protocol motor -> can, motor-protocol
esc-bus -> can, motor, 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 chassis-config -> esc-bus, motor-control, motor-protocol, mecanum
bench -> can, esc-bus, experiment, motor, motor-control, socketcan, slcan bench -> can, esc-bus, experiment, motor, motor-control, socketcan, slcan
chassisd -> chassis-config, esc-bus, mecanum, motor-control, sim-harness, socketcan chassisd -> chassis-config, dr16, esc-bus, mecanum, motor-control, sim-harness, socketcan
spin -> bench, esc-bus, motor-control, socketcan, slcan spin -> bench, dr16, esc-bus, motor-control, socketcan, slcan
chassis-tune -> bench, chassis-config, experiment, sim-harness, tuning chassis-tune -> bench, chassis-config, experiment, sim-harness, tuning
``` ```
@@ -136,7 +148,8 @@ tests/*.rs # 可选集成测试
- 四轮 ESC 总线、命令租约、反馈离线保护、外部控制策略。 - 四轮 ESC 总线、命令租约、反馈离线保护、外部控制策略。
- PID、速度/位置/串级/前馈、麦轮运动学。 - 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", "approx",
"esc-bus", "esc-bus",
"mecanum", "mecanum",
"motor",
"motor-control", "motor-control",
"motor-protocol", "motor-protocol",
"serde", "serde",
@@ -256,6 +257,8 @@ dependencies = [
"can", "can",
"chassis-config", "chassis-config",
"clap", "clap",
"dr16",
"dr16-protocol",
"esc-bus", "esc-bus",
"mecanum", "mecanum",
"motor", "motor",
@@ -540,6 +543,24 @@ dependencies = [
"libloading", "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]] [[package]]
name = "dwrote" name = "dwrote"
version = "0.11.5" version = "0.11.5"
@@ -595,12 +616,8 @@ version = "0.1.0"
dependencies = [ dependencies = [
"approx", "approx",
"can", "can",
"experiment",
"motor", "motor",
"motor-control",
"motor-model",
"motor-protocol", "motor-protocol",
"sim-harness",
"thiserror", "thiserror",
"tokio", "tokio",
"tracing", "tracing",
@@ -873,6 +890,22 @@ dependencies = [
"web-time", "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]] [[package]]
name = "ioctl-rs" name = "ioctl-rs"
version = "0.1.6" version = "0.1.6"
@@ -1662,10 +1695,6 @@ name = "socketcan"
version = "0.1.0" version = "0.1.0"
dependencies = [ dependencies = [
"can", "can",
"esc-bus",
"motor",
"motor-control",
"motor-protocol",
"socketcan 4.0.0", "socketcan 4.0.0",
"thiserror", "thiserror",
"tokio", "tokio",
@@ -1699,6 +1728,8 @@ version = "0.1.0"
dependencies = [ dependencies = [
"bench", "bench",
"clap", "clap",
"dr16",
"dr16-protocol",
"esc-bus", "esc-bus",
"experiment", "experiment",
"mecanum", "mecanum",
+48 -42
View File
@@ -1,27 +1,30 @@
[workspace] [workspace]
resolver = "3" resolver = "3"
members = [ members = [
"crates/can", "crates/contract/can",
"crates/imu", "crates/contract/imu",
"crates/motor-protocol", "crates/protocol/motor",
"crates/delta10-protocol", "crates/protocol/imu",
"crates/socketcan", "crates/transport/can",
"crates/slcan", "crates/transport/serial",
"crates/delta10", "crates/device/imu",
"crates/motor", "crates/device/motor",
"crates/esc-bus", "crates/device/esc",
"crates/pid", "crates/control/algorithm",
"crates/motor-control", "crates/control/motor",
"crates/mecanum", "crates/control/chassis",
"crates/motor-model", "crates/protocol/remote",
"crates/experiment", "crates/device/remote",
"crates/sim-harness", "crates/model/motor",
"crates/tuning", "crates/experiment/trace",
"crates/chassis-config", "crates/simulation/motor",
"crates/bench", "crates/tooling/tuning",
"crates/chassisd", "crates/config/chassis",
"crates/spin", "crates/application/bench",
"crates/chassis-tune", "crates/application/daemon",
"crates/application/tools",
"crates/application/tuning",
"crates/verification/integration",
] ]
[workspace.package] [workspace.package]
@@ -71,27 +74,30 @@ anyhow = "1"
plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] } plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] }
# ---- 职责型内部 crate ---- # ---- 职责型内部 crate ----
can = { path = "crates/can" } can = { path = "crates/contract/can" }
imu = { path = "crates/imu" } imu = { path = "crates/contract/imu" }
motor-protocol = { path = "crates/motor-protocol" } motor-protocol = { path = "crates/protocol/motor" }
delta10-protocol = { path = "crates/delta10-protocol" } delta10-protocol = { path = "crates/protocol/imu" }
socketcan = { path = "crates/socketcan" } socketcan = { path = "crates/transport/can" }
slcan = { path = "crates/slcan" } slcan = { path = "crates/transport/serial" }
delta10 = { path = "crates/delta10" } delta10 = { path = "crates/device/imu" }
motor = { path = "crates/motor" } motor = { path = "crates/device/motor" }
esc-bus = { path = "crates/esc-bus" } esc-bus = { path = "crates/device/esc" }
pid = { path = "crates/pid" } pid = { path = "crates/control/algorithm" }
motor-control = { path = "crates/motor-control" } motor-control = { path = "crates/control/motor" }
mecanum = { path = "crates/mecanum" } mecanum = { path = "crates/control/chassis" }
motor-model = { path = "crates/motor-model" } dr16-protocol = { path = "crates/protocol/remote" }
experiment = { path = "crates/experiment" } dr16 = { path = "crates/device/remote" }
sim-harness = { path = "crates/sim-harness" } motor-model = { path = "crates/model/motor" }
tuning = { path = "crates/tuning" } experiment = { path = "crates/experiment/trace" }
chassis-config = { path = "crates/chassis-config" } sim-harness = { path = "crates/simulation/motor" }
bench = { path = "crates/bench" } tuning = { path = "crates/tooling/tuning" }
chassisd = { path = "crates/chassisd" } chassis-config = { path = "crates/config/chassis" }
spin = { path = "crates/spin" } bench = { path = "crates/application/bench" }
chassis-tune = { path = "crates/chassis-tune" } 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] [workspace.lints.rust]
unsafe_code = "forbid" unsafe_code = "forbid"
+12 -1
View File
@@ -2,7 +2,17 @@
RoboMaster 哨兵机器人麦轮底盘:Linux ARM 主控、4 × GM3508/C620、SocketCAN 或 SLCAN、tokio 异步运行时。 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`:设备契约与安全总线运行时。 - `motor` / `esc-bus`:设备契约与安全总线运行时。
- `pid` / `motor-control` / `mecanum`:纯控制算法。 - `pid` / `motor-control` / `mecanum`:纯控制算法。
- `motor-model` / `experiment` / `sim-harness` / `tuning`:仿真与自动整定。 - `motor-model` / `experiment` / `sim-harness` / `tuning`:仿真与自动整定。
- `dr16-protocol` / `dr16`:DJI DR16/D-BUS 协议和串口接收。
- `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`:配置、台架和应用入口。 - `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`:配置、台架和应用入口。
`esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为 `esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为
+15
View File
@@ -4,6 +4,21 @@
[can] [can]
interface = "can0" 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] [chassis]
# 控制循环周期(毫秒)。C620 期望 1kHz 命令。 # 控制循环周期(毫秒)。C620 期望 1kHz 命令。
control_period_ms = 1 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——否则预设文件就成了绕过限流的口子。 # chassis.toml 或显式 CLI——否则预设文件就成了绕过限流的口子。
# #
# schema:见 crates/chassis-tune/src/preset.rs(version = 1,未知键报错)。 # schema:见 crates/application/tuning/src/main.rs(version = 1,未知键报错)。
version = 1 version = 1
default_preset = "sim_bench_model" default_preset = "sim_bench_model"
@@ -68,7 +68,7 @@ max_evals = 4000
threads = 0 threads = 0
[preset.sim_bench_model.sim] [preset.sim_bench_model.sim]
plant = "gm3508-bench" plant = "bench"
# 域随机化:单点模型调出来的增益对增益/时间常数/延迟高度敏感, # 域随机化:单点模型调出来的增益对增益/时间常数/延迟高度敏感,
# 因此默认在 8 个成员的集成上做鲁棒优化,而不是对着一条理想曲线最优化。 # 因此默认在 8 个成员的集成上做鲁棒优化,而不是对着一条理想曲线最优化。
ensemble = 8 ensemble = 8
@@ -128,7 +128,7 @@ max_evals = 1000
threads = 0 threads = 0
[preset.sim_ideal.sim] [preset.sim_ideal.sim]
plant = "gm3508" plant = "ideal"
# 理想模型本身就是确定的,单成员即可。 # 理想模型本身就是确定的,单成员即可。
ensemble = 1 ensemble = 1
robust_weight = 0.0 robust_weight = 0.0
@@ -193,7 +193,7 @@ threads = 1
[preset.bench_noload.sim] [preset.bench_noload.sim]
# 真机后端不用这些,但保留"最接近台架的模型",方便先离线预演再上车。 # 真机后端不用这些,但保留"最接近台架的模型",方便先离线预演再上车。
plant = "gm3508-bench" plant = "bench"
ensemble = 8 ensemble = 8
robust_weight = 0.5 robust_weight = 0.5
gain_pct = 0.2 gain_pct = 0.2
@@ -254,7 +254,7 @@ max_evals = 80
threads = 1 threads = 1
[preset.bench_loaded.sim] [preset.bench_loaded.sim]
plant = "gm3508-bench" plant = "bench"
ensemble = 8 ensemble = 8
robust_weight = 0.5 robust_weight = 0.5
gain_pct = 0.2 gain_pct = 0.2
@@ -314,7 +314,7 @@ max_evals = 40
threads = 1 threads = 1
[preset.field_updown.sim] [preset.field_updown.sim]
plant = "gm3508-bench" plant = "bench"
ensemble = 8 ensemble = 8
robust_weight = 0.5 robust_weight = 0.5
gain_pct = 0.2 gain_pct = 0.2
@@ -24,3 +24,6 @@ anyhow = { workspace = true }
[dev-dependencies] [dev-dependencies]
tokio = { workspace = true, features = ["test-util"] } tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "application/bench"
@@ -15,6 +15,8 @@ motor = { workspace = true }
esc-bus = { workspace = true } esc-bus = { workspace = true }
motor-control = { workspace = true } motor-control = { workspace = true }
mecanum = { workspace = true } mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
chassis-config = { workspace = true } chassis-config = { workspace = true }
sim-harness = { workspace = true } sim-harness = { workspace = true }
socketcan = { workspace = true } socketcan = { workspace = true }
@@ -26,3 +28,6 @@ tracing-subscriber = { workspace = true }
[dev-dependencies] [dev-dependencies]
tokio = { workspace = true, features = ["test-util"] } tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/daemon"
@@ -166,62 +166,63 @@ pub mod messages {
pub use messages::*; pub use messages::*;
pub mod source { pub mod source {
//! 上层速度指令源(twist source)。 //! 上层底盘指令源。
//! //!
//! 守护进程通过 [`watch`] 通道把指令交给控制循环;指令源在独立任务中 //! 守护进程通过 [`watch`] 通道把指令交给控制循环;指令源持续产出
//! 持续产出 twist。遥控器(DR16)、自主导航等后续接入时各自实现 //! [`RemoteCommand`]。遥控器、自主导航等来源共享同一接口,失能语义不会在
//! [`TwistSource`] 即可。 //! 各来源里重复实现。
use std::future::Future; use std::future::Future;
use mecanum::Twist;
use tokio::sync::{mpsc, watch}; use tokio::sync::{mpsc, watch};
/// 速度指令源。 use crate::RemoteCommand;
pub trait TwistSource: Send {
/// 底盘指令源。
pub trait CommandSource: Send {
/// 取下一条指令;`None` 表示源关闭。 /// 取下一条指令;`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; pub struct ZeroSource;
impl TwistSource for ZeroSource { impl CommandSource for ZeroSource {
async fn next_twist(&mut self) -> Option<Twist> { async fn next_command(&mut self) -> Option<RemoteCommand> {
std::future::pending().await std::future::pending().await
} }
} }
/// 通道指令源(测试 / 简单进程内注入用)。 /// 通道指令源(测试 / 简单进程内注入用)。
pub struct ChannelSource { pub struct ChannelSource {
rx: mpsc::Receiver<Twist>, rx: mpsc::Receiver<RemoteCommand>,
} }
impl ChannelSource { impl ChannelSource {
/// 由 mpsc 接收端构造。 /// 由 mpsc 接收端构造。
#[must_use] #[must_use]
pub const fn new(rx: mpsc::Receiver<Twist>) -> Self { pub const fn new(rx: mpsc::Receiver<RemoteCommand>) -> Self {
Self { rx } Self { rx }
} }
/// 创建一对(发送端, 源)。 /// 创建一对(发送端, 源)。
#[must_use] #[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); let (tx, rx) = mpsc::channel(bound);
(tx, Self { rx }) (tx, Self { rx })
} }
} }
impl TwistSource for ChannelSource { impl CommandSource for ChannelSource {
async fn next_twist(&mut self) -> Option<Twist> { async fn next_command(&mut self) -> Option<RemoteCommand> {
self.rx.recv().await self.rx.recv().await
} }
} }
/// 把任意指令源持续泵入 watch 通道(源关闭或接收端关闭时退出)。 /// 把任意指令源持续泵入 watch 通道(源关闭或接收端关闭时退出)。
pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender<Twist>) { pub async fn pump_source(mut source: impl CommandSource, tx: watch::Sender<RemoteCommand>) {
while let Some(twist) = source.next_twist().await { while let Some(command) = source.next_command().await {
if tx.send(twist).is_err() { if tx.send(command).is_err() {
break; break;
} }
} }
@@ -229,28 +230,249 @@ pub mod source {
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use mecanum::Twist;
use super::*; use super::*;
#[tokio::test(start_paused = true)] #[tokio::test(start_paused = true)]
async fn channel_source_pumps_into_watch() { async fn channel_source_pumps_into_watch() {
let (tx, source) = ChannelSource::pair(8); 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)); 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; 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); drop(tx);
pump.await.unwrap(); 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 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 { pub mod telemetry {
//! 诊断输出:控制周期抖动统计(ARCHITECTURE.md §7.2)。 //! 诊断输出:控制周期抖动统计(ARCHITECTURE.md §7.2)。
//! //!
@@ -569,11 +791,52 @@ pub mod controller {
use tokio::time::{Instant, MissedTickBehavior, interval}; use tokio::time::{Instant, MissedTickBehavior, interval};
use tracing::{info, warn}; 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 节拍): /// 职责(1kHz 节拍):
/// 1. 读取最新 twist 指令,**指令超时则归零**; /// 1. 读取最新 [`RemoteCommand`],超时或失能时立即归零;
/// 2. 速度限幅(保持方向按比例收缩); /// 2. 速度限幅(保持方向按比例收缩),再逐轴斜坡平滑;
/// 3. **上电门控**:四台电机都收到过反馈才下发速度,否则全部停用; /// 3. **上电门控**:四台电机都收到过反馈才下发速度,否则全部停用;
/// 4. 麦轮逆解 → 各轮目标转速(rpm,轮端)→ 经 [`OutputShaft`] 换算到转子。 /// 4. 麦轮逆解 → 各轮目标转速(rpm,轮端)→ 经 [`OutputShaft`] 换算到转子。
/// ///
@@ -585,6 +848,7 @@ pub mod controller {
/// (max_vx, max_vy, max_omega)。 /// (max_vx, max_vy, max_omega)。
limits: (f32, f32, f32), limits: (f32, f32, f32),
command_timeout: Duration, command_timeout: Duration,
ramp_time: Duration,
} }
impl<W> ChassisController<W> impl<W> ChassisController<W>
@@ -598,19 +862,25 @@ pub mod controller {
wheels: [W; 4], wheels: [W; 4],
limits: (f32, f32, f32), limits: (f32, f32, f32),
command_timeout: Duration, command_timeout: Duration,
ramp_time: Duration,
) -> Self { ) -> 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)) 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); let mut ticker = interval(period);
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); 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 last_command_at: Option<Instant> = None;
let mut ticks: u64 = 0; let mut ticks: u64 = 0;
@@ -627,13 +897,21 @@ pub mod controller {
// 尚未收到任何指令:保持静止。 // 尚未收到任何指令:保持静止。
None => true, 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 Twist::ZERO
} else { } 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 { if all_seen_feedback {
let wheel_rpms = self.kinematics.wheel_rpms(&twist); let wheel_rpms = self.kinematics.wheel_rpms(&twist);
for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) { for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) {
@@ -696,6 +974,46 @@ pub mod controller {
m 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` 原样工作。 /// 证明 `ChassisController` 只依赖能力 trait:换 `MockMotor` 原样工作。
#[tokio::test(start_paused = true)] #[tokio::test(start_paused = true)]
async fn controller_drives_wheels_through_capability_traits() { async fn controller_drives_wheels_through_capability_traits() {
@@ -706,12 +1024,13 @@ pub mod controller {
wrap_wheels(mocks.clone(), [GEAR; 4]), wrap_wheels(mocks.clone(), [GEAR; 4]),
(3.0, 3.0, 6.0), (3.0, 3.0, 6.0),
Duration::from_millis(200), 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 handle = ctrl.spawn(rx, Duration::from_millis(1));
let twist = Twist { vx: 1.0, ..Twist::ZERO }; 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; tokio::time::sleep(Duration::from_millis(5)).await;
let expected = kinematics.wheel_rpms(&twist); let expected = kinematics.wheel_rpms(&twist);
@@ -721,7 +1040,7 @@ pub mod controller {
let want = expected[i] * GEAR; let want = expected[i] * GEAR;
assert!( assert!(
(rotor_rpm - want).abs() < 1e-2, (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:?}"), ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
@@ -736,9 +1055,7 @@ pub mod controller {
async fn disables_wheels_until_all_feedback_seen() { async fn disables_wheels_until_all_feedback_seen() {
let kinematics = Mecanum::new(0.2, 0.25, 0.0762); let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
let ready = mock_wheel(1); let ready = mock_wheel(1);
let missing = MockMotor::new(2); // 无反馈 let missing = MockMotor::new(2);
let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
let _ = mocks_ready;
let wheels = wrap_wheels( let wheels = wrap_wheels(
[ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)],
[GEAR; 4], [GEAR; 4],
@@ -748,11 +1065,12 @@ pub mod controller {
wheels, wheels,
(3.0, 3.0, 6.0), (3.0, 3.0, 6.0),
Duration::from_millis(200), 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)); 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; tokio::time::sleep(Duration::from_millis(5)).await;
assert_eq!(ready.last_command(), Command::Disabled); assert_eq!(ready.last_command(), Command::Disabled);
@@ -760,6 +1078,29 @@ pub mod controller {
handle.abort(); 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 std::time::Duration;
use clap::Parser; use clap::Parser;
use mecanum::Twist;
use tokio::sync::watch; use tokio::sync::watch;
use tracing::{error, info}; use tracing::{error, info};
use tracing_subscriber::EnvFilter; use tracing_subscriber::EnvFilter;
use crate::remote::{Dr16CommandSource, RemoteMapper};
use crate::{ChassisController, wrap_wheels}; use crate::{ChassisController, wrap_wheels};
use can::CanLink; use can::CanLink;
use chassis_config::Config; use chassis_config::Config;
@@ -802,6 +1143,10 @@ struct Args {
/// 只装配配置、链路、总线和控制任务,短跑后安全退出。 /// 只装配配置、链路、总线和控制任务,短跑后安全退出。
#[arg(long)] #[arg(long)]
dry_run: bool, dry_run: bool,
/// 覆盖配置文件中的 DR16 串口设备(如 `/dev/ttyS2`)。
#[arg(long)]
rc_device: Option<String>,
} }
fn main() -> ExitCode { fn main() -> ExitCode {
@@ -811,13 +1156,18 @@ fn main() -> ExitCode {
let args = Args::parse(); let args = Args::parse();
let config = match load_config(&args) { let mut config = match load_config(&args) {
Ok(config) => config, Ok(config) => config,
Err(e) => { Err(e) => {
error!("{e}"); error!("{e}");
return ExitCode::FAILURE; 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 interface = args.interface.clone().unwrap_or_else(|| config.can.interface.clone());
let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() { let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() {
@@ -856,6 +1206,25 @@ async fn run(
dry_run: bool, dry_run: bool,
) -> Result<(), Box<dyn std::error::Error>> { ) -> Result<(), Box<dyn std::error::Error>> {
let bus_options = config.bus_options(); 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。 // 1) CAN 链路:仿真或真实 SocketCAN。
let link = if mock { let link = if mock {
@@ -884,10 +1253,8 @@ async fn run(
"chassisd 启动" "chassisd 启动"
); );
// 3) 控制循环(当前无指令源:watch 保持 ZERO,安全静止; // 3) 控制循环与遥控指令源。
// 遥控 / 自主导航后续经 chassis_config::source 接入)。 let (command_tx, command_rx) = watch::channel(crate::RemoteCommand::DISABLED);
let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega);
let motors: [EscMotor; 4] = let motors: [EscMotor; 4] =
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let gears: [f32; 4] = config let gears: [f32; 4] = config
@@ -902,11 +1269,16 @@ async fn run(
wrap_wheels(motors, gears), wrap_wheels(motors, gears),
limits, limits,
Duration::from_millis(config.chassis.command_timeout_ms), 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); .spawn(command_rx, period);
let _twist_tx = twist_tx; // 保留发送端供后续指令源使用 let remote_task = remote_source
.map(|(source, _)| tokio::spawn(crate::source::pump_source(source, command_tx.clone())));
if dry_run { if dry_run {
if let Some(task) = remote_task {
task.abort();
}
tokio::time::sleep(Duration::from_millis(20)).await; tokio::time::sleep(Duration::from_millis(20)).await;
for motor in bus.motors() { for motor in bus.motors() {
motor.disable(); motor.disable();
@@ -927,6 +1299,9 @@ async fn run(
} }
// 5) 停车:显式停用电机并等若干节拍把 0 电流帧刷入总线。 // 5) 停车:显式停用电机并等若干节拍把 0 电流帧刷入总线。
if let Some(task) = remote_task {
task.abort();
}
for motor in bus.motors() { for motor in bus.motors() {
motor.disable(); motor.disable();
} }
@@ -953,6 +1328,7 @@ fn open_real_link(interface: &str) -> Result<CanLink, Box<dyn std::error::Error>
mod daemon_integration { mod daemon_integration {
use super::*; use super::*;
use esc_bus::EscBusHandle; use esc_bus::EscBusHandle;
use mecanum::Twist;
use motor::MotorFeedback; use motor::MotorFeedback;
use sim_harness::spawn_plant; use sim_harness::spawn_plant;
use tokio::time::sleep; use tokio::time::sleep;
@@ -964,7 +1340,7 @@ mod daemon_integration {
fn stack( fn stack(
config: &Config, config: &Config,
command_timeout_ms: u64, command_timeout_ms: u64,
) -> (EscBusHandle<MotorPolicy>, watch::Sender<Twist>) { ) -> (EscBusHandle<MotorPolicy>, watch::Sender<RemoteCommand>) {
let (bus_side, plant_side) = CanLink::pair(512); let (bus_side, plant_side) = CanLink::pair(512);
let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect(); let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
spawn_plant(plant_side, &esc_ids, config.bus_options().period); spawn_plant(plant_side, &esc_ids, config.bus_options().period);
@@ -976,7 +1352,7 @@ mod daemon_integration {
let bus = let bus =
EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options(), policy).unwrap(); 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] = let motors: [EscMotor; 4] =
bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
let gears: [f32; 4] = config let gears: [f32; 4] = config
@@ -991,6 +1367,7 @@ mod daemon_integration {
wrap_wheels(motors, gears), wrap_wheels(motors, gears),
(config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega), (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
Duration::from_millis(command_timeout_ms), Duration::from_millis(command_timeout_ms),
Duration::from_millis(2),
); );
controller.spawn(twist_rx, config.bus_options().period); controller.spawn(twist_rx, config.bus_options().period);
(bus, twist_tx) (bus, twist_tx)
@@ -1025,7 +1402,7 @@ mod daemon_integration {
let config = Config::mock_default(); let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000); 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 target = RPM_PER_MPS;
let ok = wait_until(3000, || { let ok = wait_until(3000, || {
wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target) wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
@@ -1035,7 +1412,7 @@ mod daemon_integration {
assert!(ok, "未收敛到 {target} rpm: {rpm:?}"); assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
// 停车:指令归零后各轮减速到停。 // 停车:指令归零后各轮减速到停。
twist_tx.send_replace(Twist::ZERO); twist_tx.send_replace(RemoteCommand::STOP);
let stopped = let stopped =
wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
let rpm = wheel_rpms(&bus); let rpm = wheel_rpms(&bus);
@@ -1048,7 +1425,7 @@ mod daemon_integration {
let config = Config::mock_default(); let config = Config::mock_default();
let (bus, twist_tx) = stack(&config, 10_000); 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 的轮线速度。 // a·ω = (0.2 + 0.25) m/s 的轮线速度。
let target = 0.45 * RPM_PER_MPS; let target = 0.45 * RPM_PER_MPS;
let ok = wait_until(3000, || { let ok = wait_until(3000, || {
@@ -1076,7 +1453,7 @@ mod daemon_integration {
let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms); let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
// 只发一次指令,随后沉默 → 超过 command_timeout(200ms)后应停车。 // 只发一次指令,随后沉默 → 超过 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 = let moving =
wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)) wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS))
.await; .await;
@@ -1096,11 +1473,11 @@ mod daemon_integration {
config.chassis.max_vx = 1.0; // 限速 1 m/s config.chassis.max_vx = 1.0; // 限速 1 m/s
let (bus, twist_tx) = stack(&config, 10_000); let (bus, twist_tx) = stack(&config, 10_000);
twist_tx.send_replace(Twist { twist_tx.send_replace(RemoteCommand::new(Twist {
vx: 3.0, // 超限指令 vx: 3.0, // 超限指令
vy: 2.0, vy: 2.0,
omega: 0.0, omega: 0.0,
}); }));
// 限幅按比例收缩:(3, 2) → (1, 2/3)。四轮 = vx ∓ vy: // 限幅按比例收缩:(3, 2) → (1, 2/3)。四轮 = vx ∓ vy:
// FL/RR = (1 - 2/3) m/s,FR/RL = (1 + 2/3) m/s。 // FL/RR = (1 - 2/3) m/s,FR/RL = (1 + 2/3) m/s。
let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS; let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
@@ -16,6 +16,8 @@ esc-bus = { workspace = true }
experiment = { workspace = true } experiment = { workspace = true }
motor-control = { workspace = true } motor-control = { workspace = true }
mecanum = { workspace = true } mecanum = { workspace = true }
dr16-protocol = { workspace = true }
dr16 = { workspace = true }
slcan = { workspace = true } slcan = { workspace = true }
socketcan = { workspace = true } socketcan = { workspace = true }
tokio = { workspace = true } tokio = { workspace = true }
@@ -25,3 +27,6 @@ tracing-subscriber = { workspace = true }
[dev-dependencies] [dev-dependencies]
tokio = { workspace = true, features = ["test-util"] } 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 { mod motor_one {
//! 单电机验证工具:让一个 C620 / GM3508 转起来,验证电机抽象全链路。 //! 单电机验证工具:让一个 C620 / GM3508 转起来,验证电机抽象全链路。
//! //!
@@ -710,12 +797,13 @@ async fn main() -> std::process::ExitCode {
motor_one::main_with_args(args.collect()).await; motor_one::main_with_args(args.collect()).await;
std::process::ExitCode::SUCCESS std::process::ExitCode::SUCCESS
} }
Some("rc") => rc_monitor::main_with_args(args.collect()).await,
Some("--help" | "-h") => { Some("--help" | "-h") => {
println!("usage: spin <chassis|motor> [options]"); println!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::SUCCESS std::process::ExitCode::SUCCESS
} }
_ => { _ => {
eprintln!("usage: spin <chassis|motor> [options]"); eprintln!("usage: spin <chassis|motor|rc> [options]");
std::process::ExitCode::from(2) std::process::ExitCode::from(2)
} }
} }
@@ -45,3 +45,6 @@ plotters = { workspace = true, optional = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "application/tuning"
@@ -122,7 +122,7 @@ pub mod report {
#[cfg(feature = "plot")] #[cfg(feature = "plot")]
if let Some(trace) = &ctx.best_trace { if let Some(trace) = &ctx.best_trace {
let svg_path = ctx.out_dir.join("best.svg"); 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); written.push(svg_path);
} }
@@ -932,14 +932,15 @@ use esc_bus::EscConfig;
use experiment::SetpointProfile; use experiment::SetpointProfile;
use indicatif::ProgressBar; use indicatif::ProgressBar;
use motor_control::VelocityGains; use motor_control::VelocityGains;
use motor_model::Gm3508Model;
use motor_protocol::EscModel; use motor_protocol::EscModel;
use serde::Serialize; use serde::Serialize;
use sim_harness::{LoopLimits, PlantEnsemble}; use sim_harness::{LoopLimits, PlantEnsemble};
use tuning::{ use tuning::{
AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH, AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH,
Evaluator, Objective, PlantSpec, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, ScaleSpec, Evaluator, Objective, PlantProfile, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked,
ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict, parse_axis, ScaleSpec, ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict,
sweep, parse_axis, sweep,
}; };
/// 子命令。 /// 子命令。
@@ -1121,7 +1122,7 @@ struct CommonArgs {
/// 仿真后端参数(覆盖预设)。 /// 仿真后端参数(覆盖预设)。
#[derive(Debug, Clone, ClapArgs)] #[derive(Debug, Clone, ClapArgs)]
struct SimArgs { struct SimArgs {
/// 被控对象模型(内置默认 `gm3508-bench`) /// 被控对象模型(内置默认 `bench`)
#[arg(long, value_enum)] #[arg(long, value_enum)]
plant: Option<PlantArg>, plant: Option<PlantArg>,
/// 集成成员数(域随机化;1 = 只用名义模型;内置默认 8) /// 集成成员数(域随机化;1 = 只用名义模型;内置默认 8)
@@ -1268,16 +1269,16 @@ impl From<SolverArg> for SolverSpec {
#[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] #[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)]
enum PlantArg { enum PlantArg {
/// 理想一阶模型(无摩擦、无延迟) /// 理想一阶模型(无摩擦、无延迟)
Gm3508, Ideal,
/// 台架量级:静 / 库仑摩擦 + 2 拍延迟 /// 台架量级:静 / 库仑摩擦 + 2 拍延迟
Gm3508Bench, Bench,
} }
impl From<PlantArg> for PlantSpec { impl From<PlantArg> for PlantProfile {
fn from(value: PlantArg) -> Self { fn from(value: PlantArg) -> Self {
match value { match value {
PlantArg::Gm3508 => Self::Gm3508, PlantArg::Ideal => Self::Ideal,
PlantArg::Gm3508Bench => Self::Gm3508Bench, PlantArg::Bench => Self::Bench,
} }
} }
} }
@@ -1411,7 +1412,9 @@ impl CommonArgs {
let source = let source =
if prefix.is_empty() { "内置默认".to_owned() } else { format!("{prefix}{name}") }; 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 let envelope = merged
.bench_envelope() .bench_envelope()
.with_context(|| format!("解算预设 '{source}' 的安全包络失败"))?; .with_context(|| format!("解算预设 '{source}' 的安全包络失败"))?;
@@ -1922,7 +1925,8 @@ fn presets_cmd(args: &PresetsArgs) -> Result<ExitCode> {
println!("共 {} 个预设", file.preset.len()); println!("共 {} 个预设", file.preset.len());
for (name, preset) in &file.preset { for (name, preset) in &file.preset {
let merged = Preset::builtin().overlay(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 = let envelope =
merged.bench_envelope().with_context(|| format!("解算预设 '{name}' 的安全包络失败"))?; merged.bench_envelope().with_context(|| format!("解算预设 '{name}' 的安全包络失败"))?;
let mark = let mark =
@@ -2369,7 +2373,6 @@ fn open_bench(
let mut esc = EscConfig::gm3508(hardware.name.clone(), hardware.esc_id, hardware.direction); let mut esc = EscConfig::gm3508(hardware.name.clone(), hardware.esc_id, hardware.direction);
esc.gear_ratio = hardware.gear_ratio; esc.gear_ratio = hardware.gear_ratio;
esc.current_limit = limits.current_limit_raw; esc.current_limit = limits.current_limit_raw;
esc.model = limits.model;
let envelope = SafetyEnvelope::new() let envelope = SafetyEnvelope::new()
.with_overspeed(envelope_spec.max_rpm, Duration::from_millis(60)) .with_overspeed(envelope_spec.max_rpm, Duration::from_millis(60))
@@ -2452,7 +2455,14 @@ feedforward = 0.0
/// 仓库里随代码一起评审的预设文件。 /// 仓库里随代码一起评审的预设文件。
fn repo_preset_path() -> PathBuf { 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 { fn args_with(extra: &[&str]) -> CommonArgs {
@@ -2573,7 +2583,7 @@ feedforward = 0.0
assert_eq!(tuning.search.seed, 20_260_101); assert_eq!(tuning.search.seed, 20_260_101);
assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6); assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6);
assert!((tuning.acceptance.max_overshoot_pct - 20.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); assert_eq!(settings.envelope.validate_runs, 3);
} }
@@ -2722,7 +2732,7 @@ samples = 99
for (name, preset) in &file.preset { for (name, preset) in &file.preset {
let merged = Preset::builtin().overlay(preset); let merged = Preset::builtin().overlay(preset);
let tuning = merged let tuning = merged
.to_tuning() .to_tuning::<Gm3508Model>()
.unwrap_or_else(|error| panic!("预设 '{name}' 无法解算:{error}")); .unwrap_or_else(|error| panic!("预设 '{name}' 无法解算:{error}"));
merged merged
.bench_envelope() .bench_envelope()
@@ -10,6 +10,7 @@ license.workspace = true
workspace = true workspace = true
[dependencies] [dependencies]
motor = { workspace = true }
motor-control = { workspace = true } motor-control = { workspace = true }
mecanum = { workspace = true } mecanum = { workspace = true }
esc-bus = { workspace = true } esc-bus = { workspace = true }
@@ -20,3 +21,6 @@ thiserror = { workspace = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "config/chassis"
@@ -12,6 +12,7 @@ use std::time::Duration;
use esc_bus::{BusOptions, EscConfig}; use esc_bus::{BusOptions, EscConfig};
use mecanum::Mecanum; use mecanum::Mecanum;
use motor::Gm3508;
use motor_control::VelocityGains; use motor_control::VelocityGains;
use motor_protocol::EscModel; use motor_protocol::EscModel;
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
@@ -37,6 +38,9 @@ pub struct Config {
/// 底盘控制参数。 /// 底盘控制参数。
#[serde(default)] #[serde(default)]
pub chassis: ChassisConfig, pub chassis: ChassisConfig,
/// 可选遥控输入;缺省时守护进程保持安全静止。
#[serde(default)]
pub remote: Option<RemoteConfig>,
/// 麦轮几何。 /// 麦轮几何。
pub mecanum: MecanumConfig, 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)] #[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)]
#[serde(deny_unknown_fields)] #[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) { 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())); 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 { for motor in &self.motors {
let gear_ratio_valid = motor.gear_ratio > 0.0; let gear_ratio_valid = motor.gear_ratio > 0.0;
if !gear_ratio_valid { if !gear_ratio_valid {
@@ -390,6 +474,7 @@ impl Config {
Self { Self {
can: CanConfig { interface: "vcan0".into(), function: None }, can: CanConfig { interface: "vcan0".into(), function: None },
chassis: ChassisConfig::default(), chassis: ChassisConfig::default(),
remote: None,
mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 }, mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 },
motors: vec![ motors: vec![
motor("front_left", 1, 1.0), motor("front_left", 1, 1.0),
@@ -402,16 +487,14 @@ impl Config {
/// 转换为 `EscBus` 配置(顺序 FL、FR、RL、RR)。 /// 转换为 `EscBus` 配置(顺序 FL、FR、RL、RR)。
#[must_use] #[must_use]
pub fn to_esc_configs(&self) -> Vec<EscConfig> { pub fn to_esc_configs(&self) -> Vec<EscConfig<Gm3508>> {
self.motors self.motors
.iter() .iter()
.map(|m| EscConfig { .map(|m| {
name: m.name.clone(), let mut cfg = EscConfig::<Gm3508>::gm3508(m.name.clone(), m.esc_id, m.direction);
esc_id: m.esc_id, cfg.gear_ratio = m.gear_ratio;
model: m.model.into(), cfg.current_limit = m.current_limit;
gear_ratio: m.gear_ratio, cfg
direction: m.direction,
current_limit: m.current_limit,
}) })
.collect() .collect()
} }
@@ -441,6 +524,16 @@ mod tests {
[can] [can]
interface = "can0" 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] [chassis]
control_period_ms = 1 control_period_ms = 1
command_timeout_ms = 200 command_timeout_ms = 200
@@ -497,6 +590,10 @@ feedforward = 9.0
fn parses_example() { fn parses_example() {
let cfg = Config::from_toml(EXAMPLE).unwrap(); let cfg = Config::from_toml(EXAMPLE).unwrap();
assert_eq!(cfg.can.interface, "can0"); 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.len(), 4);
assert_eq!(cfg.motors[0].name, "front_left"); assert_eq!(cfg.motors[0].name, "front_left");
assert_eq!(cfg.motors[3].esc_id, 4); assert_eq!(cfg.motors[3].esc_id, 4);
@@ -536,6 +633,7 @@ gear_ratio = 19.2
) )
.unwrap(); .unwrap();
assert_eq!(cfg.chassis, ChassisConfig::default()); 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].direction.to_bits(), 1.0f32.to_bits());
assert_eq!(cfg.motors[0].current_limit, 12_000); assert_eq!(cfg.motors[0].current_limit, 12_000);
assert_eq!(cfg.motors[0].model, EscModelSerde::C620); assert_eq!(cfg.motors[0].model, EscModelSerde::C620);
@@ -611,6 +709,7 @@ gear_ratio = 19.2
let reparsed = Config::from_toml(&text).unwrap(); let reparsed = Config::from_toml(&text).unwrap();
assert_eq!(reparsed, original); assert_eq!(reparsed, original);
// 落盘文本必须仍然是合法配置(守护进程直接读它)。 // 落盘文本必须仍然是合法配置(守护进程直接读它)。
assert!(text.contains("[remote]"), "{text}");
assert!(text.contains("[[motor]]"), "{text}"); assert!(text.contains("[[motor]]"), "{text}");
assert!(text.contains("interface = \"can0\""), "{text}"); assert!(text.contains("interface = \"can0\""), "{text}");
} }
@@ -620,9 +719,11 @@ gear_ratio = 19.2
let mut original = Config::mock_default(); let mut original = Config::mock_default();
original.can.function = Some(FunctionBusConfig { interface: "can1".into() }); original.can.function = Some(FunctionBusConfig { interface: "can1".into() });
original.chassis.motor_command_timeout_ms = 37; 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(); let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap();
assert_eq!(reparsed, original); assert_eq!(reparsed, original);
assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37); assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37);
assert!(reparsed.remote.unwrap().invert_omega);
assert_eq!(reparsed.can.function.unwrap().interface, "can1"); 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] #[test]
fn save_and_load_file_round_trip() { fn save_and_load_file_round_trip() {
let dir = std::env::temp_dir().join("chassis-config-roundtrip"); let dir = std::env::temp_dir().join("chassis-config-roundtrip");
@@ -17,3 +17,6 @@ tokio = { workspace = true }
[dev-dependencies] [dev-dependencies]
tokio = { workspace = true, features = ["test-util"] } tokio = { workspace = true, features = ["test-util"] }
[package.metadata.sentry-chassis]
layer = "contract/can"
@@ -11,3 +11,6 @@ workspace = true
[dependencies] [dependencies]
[package.metadata.sentry-chassis]
layer = "contract/imu"
@@ -14,3 +14,6 @@ workspace = true
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/algorithm"
@@ -15,3 +15,6 @@ workspace = true
[dev-dependencies] [dev-dependencies]
proptest = { workspace = true } proptest = { workspace = true }
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/chassis"
@@ -15,3 +15,6 @@ motor = { workspace = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "control/motor"
@@ -18,9 +18,8 @@ thiserror = { workspace = true }
tracing = { workspace = true } tracing = { workspace = true }
[dev-dependencies] [dev-dependencies]
sim-harness = { workspace = true }
motor-control = { workspace = true }
motor-model = { workspace = true }
experiment = { workspace = true }
tokio = { workspace = true, features = ["test-util"] } tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "device/esc"
@@ -1,132 +1,96 @@
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] #![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
//! `EscBus` 异步驱动:协议状态机与 1kHz 控制节拍。 //! Generic asynchronous ESC frame pump, state, leases, and safety.
//!
//! [`EscBus::spawn`] 消费一个 [`CanLink`],启动收发两个任务:
//!
//! - 接收:解码反馈帧,更新各电机共享状态(含多圈累计与方向修正);
//! - 发送:按 `period` 节拍为每台电机求值当前命令(速度/位置环 PID / 直发
//! effort),按 `0x200`/`0x1FF` 分组组帧发送。
//!
//! 电机离线(从未收到反馈或反馈超时)时强制 0 输出并复位控制器。
//! 命令模式切换时同样复位控制器,避免积分冲击。
use std::marker::PhantomData;
use std::sync::{Arc, Mutex, MutexGuard}; use std::sync::{Arc, Mutex, MutexGuard};
use std::time::Duration; use std::time::Duration;
use can::{CanFrame, CanLink}; use can::{CanFrame, CanLink};
use motor_protocol::common::{ use motor::{
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame, 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 thiserror::Error;
use tokio::sync::{mpsc, watch}; use tokio::sync::{mpsc, watch};
use tokio::task::JoinHandle; use tokio::task::JoinHandle;
use tokio::time::{Instant, Interval, MissedTickBehavior}; use tokio::time::{Instant, Interval, MissedTickBehavior};
use tracing::{debug, info, warn}; 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> { fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
} }
/// 单台电机的配置。 /// Configuration for one typed motor.
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct EscConfig { pub struct EscConfig<S: MotorSpec = motor::Gm3508> {
/// 便于日志辨认的名字(如 `front_left`)。
pub name: String, pub name: String,
/// 电调 ID(1–8,即 CAN 拨码)。
pub esc_id: u8, pub esc_id: u8,
/// 电调型号。
pub model: EscModel,
/// 减速比(电机转子 : 输出轴)。
///
/// 驱动层**不使用**此字段;由上层 `crate::OutputShaft` 应用。
/// 保留于此作为配置元数据,`chassisd` 包装轮子时必须读取本字段(单一来源)。
pub gear_ratio: f32, pub gear_ratio: f32,
/// 安装方向:`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转,
/// 上层永远以"输出轴正转 = 前进方向"工作。
pub direction: f32, pub direction: f32,
/// effort 限幅(raw,`1..=型号满量程`)。
pub current_limit: i16, pub current_limit: i16,
marker: PhantomData<S>,
} }
impl EscConfig { impl<S: MotorSpec> EscConfig<S> {
/// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值,需按整车整定。
#[must_use] #[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 { Self {
name: name.into(), name: name.into(),
esc_id, esc_id,
model: EscModel::C620, gear_ratio: S::GEAR_RATIO,
// 3591 / 187 ≈ 19.203
gear_ratio: 3591.0 / 187.0,
direction, direction,
current_limit: 12_000, current_limit: S::scale().max_raw() / 4 * 3,
} marker: PhantomData,
}
/// 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,
} }
} }
fn validate(&self) -> Result<(), String> { 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)); return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
} }
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) { if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio)); return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
} }
// 逐位比较:direction 只允许精确的 ±1.0。
let dir_bits = self.direction.to_bits(); let dir_bits = self.direction.to_bits();
if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).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)); 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!( return Err(format!(
"current_limit {} out of range 1..={} for {:?}", "current_limit {} out of range 1..={}",
self.current_limit, self.current_limit,
self.model.max_current_raw(), S::scale().max_raw()
self.model
)); ));
} }
Ok(()) 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)] #[derive(Debug, Clone)]
pub struct BusOptions { pub struct BusOptions {
/// 控制节拍(C620 期望 1kHz 命令)。
pub period: Duration, pub period: Duration,
/// 反馈离线阈值:超过该时长未收到某电机反馈 → 强制 0 输出。
pub feedback_timeout: Duration, pub feedback_timeout: Duration,
/// **命令租约**:上层超过该时长未重新下发命令 → 视为 `Disabled`(0 输出)。
///
/// 命令是**租约**而非一次性设定值:`set_velocity` / `set_effort_raw` /
/// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
/// (`chassisd` 的 1kHz 循环天然满足;台架脚本每拍重发即可),
/// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
///
/// 设置为 `Duration::MAX` 可退化为纯设定值语义(仅建议在无失控风险的台架场景)。
pub command_timeout: Duration, pub command_timeout: Duration,
} }
@@ -135,45 +99,34 @@ impl Default for BusOptions {
Self { Self {
period: Duration::from_millis(1), period: Duration::from_millis(1),
feedback_timeout: Duration::from_millis(100), feedback_timeout: Duration::from_millis(100),
// 1kHz 下约 50 拍未刷新即失效。
command_timeout: Duration::from_millis(50), command_timeout: Duration::from_millis(50),
} }
} }
} }
/// 启动错误。 /// Startup error.
#[derive(Debug, Clone, PartialEq, Eq, Error)] #[derive(Debug, Clone, PartialEq, Eq, Error)]
pub enum EscBusError { pub enum EscBusError {
/// 两台电机配置了同一电调 ID。
#[error("duplicate ESC id {0}")] #[error("duplicate ESC id {0}")]
DuplicateEscId(u8), DuplicateEscId(u8),
/// 电机配置非法(原因见 `reason`)。
#[error("invalid config for motor {name}: {reason}")] #[error("invalid config for motor {name}: {reason}")]
InvalidConfig { InvalidConfig { name: String, reason: String },
/// 电机名。
name: String,
/// 非法原因。
reason: String,
},
/// 一台电机都没有配置。
#[error("no motors configured")] #[error("no motors configured")]
NoMotors, NoMotors,
} }
/// 单电机共享状态(驱动任务与 [`EscMotor`] 句柄之间)。 struct MotorShared<S: MotorSpec> {
struct MotorShared { cfg: EscConfig<S>,
cfg: EscConfig,
state: Mutex<MotorState>, state: Mutex<MotorState>,
cmd: Mutex<Command>, cmd: Mutex<Command>,
/// 命令写入时刻(看门狗用)。
cmd_at: Mutex<Option<Instant>>, cmd_at: Mutex<Option<Instant>>,
turn: Mutex<TurnTracker>, turn: Mutex<TurnTracker>,
last_feedback: Mutex<Option<Instant>>, last_feedback: Mutex<Option<Instant>>,
offline_warned: std::sync::atomic::AtomicBool, offline_warned: std::sync::atomic::AtomicBool,
} }
impl MotorShared { impl<S: MotorSpec> MotorShared<S> {
fn new(cfg: EscConfig) -> Self { fn new(cfg: EscConfig<S>) -> Self {
Self { Self {
cfg, cfg,
state: Mutex::new(MotorState::default()), state: Mutex::new(MotorState::default()),
@@ -190,24 +143,22 @@ impl MotorShared {
*lock(&self.cmd_at) = Some(Instant::now()); *lock(&self.cmd_at) = Some(Instant::now());
} }
/// 应用一帧反馈(原始电调侧值 → 方向修正后的物理量)。 fn apply_feedback<F: FeedbackFields>(&self, fb: &F, now: Instant) {
fn apply_feedback(&self, fb: EscFeedback, now: Instant) { let esc_rev = lock(&self.turn).update(fb.angle_raw());
let esc_rev = lock(&self.turn).update(fb.angle_raw);
let dir = self.cfg.direction; let dir = self.cfg.direction;
{ {
let mut st = lock(&self.state); 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_revolutions = dir * esc_rev;
st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm); st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm());
st.effort_raw = (dir * f32::from(fb.current_raw)) as i16; st.effort_raw = (dir * f32::from(fb.current_raw())) as i16;
st.temperature_c = fb.temperature_c; st.temperature_c = fb.temperature_c();
st.feedback_count = st.feedback_count.saturating_add(1); st.feedback_count = st.feedback_count.saturating_add(1);
} }
*lock(&self.last_feedback) = Some(now); *lock(&self.last_feedback) = Some(now);
self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed); self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
} }
/// Produce one policy sample while enforcing feedback and command leases.
fn sample( fn sample(
&self, &self,
feedback_timeout: Duration, feedback_timeout: Duration,
@@ -219,7 +170,7 @@ impl MotorShared {
Some(t) if now.saturating_duration_since(t) <= feedback_timeout Some(t) if now.saturating_duration_since(t) <= feedback_timeout
); );
if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) { 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!( let fresh = matches!(
*lock(&self.cmd_at), *lock(&self.cmd_at),
@@ -230,103 +181,87 @@ impl MotorShared {
} }
} }
/// 电机的公共句柄:可克隆,跨任务共享。 /// Typed motor handle.
/// pub struct EscMotor<S: MotorSpec = motor::Gm3508> {
/// # 命令租约 shared: Arc<MotorShared<S>>,
///
/// 下发命令([`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
/// [`PositionDrive::set_position`])即续租,租期见 [`BusOptions::command_timeout`]。
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电(0 输出),
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
#[derive(Clone)]
pub struct EscMotor {
shared: Arc<MotorShared>,
} }
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] #[must_use]
pub fn name(&self) -> &str { pub fn name(&self) -> &str {
&self.shared.cfg.name &self.shared.cfg.name
} }
/// 电机配置。
#[must_use]
pub fn config(&self) -> &EscConfig {
&self.shared.cfg
}
/// 当前命令。
#[must_use] #[must_use]
pub fn command(&self) -> Command { pub fn command(&self) -> Command {
*lock(&self.shared.cmd) *lock(&self.shared.cmd)
} }
/// 最近一帧反馈的时刻(诊断用)。
#[must_use] #[must_use]
pub fn last_feedback(&self) -> Option<Instant> { pub fn last_feedback(&self) -> Option<Instant> {
*lock(&self.shared.last_feedback) *lock(&self.shared.last_feedback)
} }
} }
impl MotorFeedback for EscMotor { impl<S: MotorSpec> MotorFeedback for EscMotor<S> {
fn id(&self) -> u8 { fn id(&self) -> u8 {
self.shared.cfg.esc_id self.shared.cfg.esc_id
} }
fn state(&self) -> MotorState { fn state(&self) -> MotorState {
*lock(&self.shared.state) *lock(&self.shared.state)
} }
fn feedback_age(&self) -> Option<Duration> {
fn feedback_age(&self) -> Option<std::time::Duration> { Some(Instant::now().saturating_duration_since(*lock(&self.shared.last_feedback).as_ref()?))
let t = *lock(&self.shared.last_feedback);
Some(Instant::now().saturating_duration_since(t?))
} }
} }
impl VelocityDrive for EscMotor { impl<S: MotorSpec> VelocityDrive for EscMotor<S> {
/// 设定目标转子转速(**租约**,见 [`EscMotor`] 的命令租约说明)。
fn set_velocity(&self, rotor_rpm: f32) { fn set_velocity(&self, rotor_rpm: f32) {
self.shared.store_command(Command::Velocity(rotor_rpm)); self.shared.store_command(Command::Velocity(rotor_rpm));
} }
} }
impl EffortDrive for EscMotor { impl<S: MotorSpec> EffortDrive for EscMotor<S> {
/// 直发开环 raw(**租约**,见 [`EscMotor`] 的命令租约说明)。
fn set_effort_raw(&self, raw: i16) { fn set_effort_raw(&self, raw: i16) {
self.shared.store_command(Command::EffortRaw(raw)); self.shared.store_command(Command::EffortRaw(raw));
} }
} }
impl PositionDrive for EscMotor { impl<S: MotorSpec> PositionDrive for EscMotor<S> {
/// 设定目标转子多圈位置(**租约**,见 [`EscMotor`] 的命令租约说明)。
fn set_position(&self, rotor_rev: f32) { fn set_position(&self, rotor_rev: f32) {
self.shared.store_command(Command::Position(rotor_rev)); self.shared.store_command(Command::Position(rotor_rev));
} }
} }
impl Disable for EscMotor { impl<S: MotorSpec> Disable for EscMotor<S> {
/// 停用(0 输出);同样受租约保护——上层若已死,租约到期后依旧归零。
fn disable(&self) { fn disable(&self) {
self.shared.store_command(Command::Disabled); self.shared.store_command(Command::Disabled);
} }
} }
/// 运行中的电调总线。 /// Generic current-frame ESC bus.
pub struct EscBus; pub struct EscBus<S: MotorSpec = motor::Gm3508>(PhantomData<S>);
impl EscBus { impl<S: CurrentEscDrive> EscBus<S> {
/// 启动收发任务,返回句柄。 /// Start receive and transmit tasks.
/// ///
/// # Errors /// # Errors
/// ///
/// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。 /// Returns an error when configuration is empty, duplicated, or invalid.
pub fn spawn<P: EffortPolicy>( pub fn spawn<P: EffortPolicy>(
link: CanLink, link: CanLink,
configs: Vec<EscConfig>, configs: Vec<EscConfig<S>>,
options: BusOptions, options: BusOptions,
policy: P, policy: P,
) -> Result<EscBusHandle<P>, EscBusError> { ) -> Result<EscBusHandle<P, S>, EscBusError> {
if configs.is_empty() { if configs.is_empty() {
return Err(EscBusError::NoMotors); return Err(EscBusError::NoMotors);
} }
@@ -346,46 +281,48 @@ impl EscBus {
let motors = Arc::new(motors); let motors = Arc::new(motors);
let policy = Arc::new(Mutex::new(policy)); let policy = Arc::new(Mutex::new(policy));
let rx_handle = let rx_handle =
tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone())); tokio::spawn(rx_loop::<S>(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
let tx_handle = tokio::spawn(tx_loop( let tx_handle = tokio::spawn(tx_loop::<S, P>(
outgoing_tx, outgoing_tx,
Arc::clone(&motors), Arc::clone(&motors),
Arc::clone(&policy), Arc::clone(&policy),
options, options,
shutdown_rx, shutdown_rx,
)); ));
Ok(EscBusHandle {
Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] }) motors,
policy,
shutdown_tx,
tasks: vec![rx_handle, tx_handle],
marker: PhantomData,
})
} }
} }
/// `EscBus` 的控制句柄。 /// Control handle for a running bus.
pub struct EscBusHandle<P> { pub struct EscBusHandle<P, S: MotorSpec = motor::Gm3508> {
motors: Arc<Vec<EscMotor>>, motors: Arc<Vec<EscMotor<S>>>,
policy: Arc<Mutex<P>>, policy: Arc<Mutex<P>>,
shutdown_tx: watch::Sender<bool>, shutdown_tx: watch::Sender<bool>,
tasks: Vec<JoinHandle<()>>, tasks: Vec<JoinHandle<()>>,
marker: PhantomData<S>,
} }
impl<P: EffortPolicy> EscBusHandle<P> { impl<P: EffortPolicy, S: CurrentEscDrive> EscBusHandle<P, S> {
/// 按电调 ID 取电机句柄。
#[must_use] #[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) self.motors.iter().find(|m| m.id() == esc_id)
} }
/// 全部电机(按配置顺序)。
#[must_use] #[must_use]
pub fn motors(&self) -> &[EscMotor] { pub fn motors(&self) -> &[EscMotor<S>] {
&self.motors &self.motors
} }
/// Mutate the composed control policy between bus ticks.
pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R { pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R {
update(&mut lock(&self.policy)) update(&mut lock(&self.policy))
} }
/// 停止收发任务并等待退出。
pub async fn shutdown(self) { pub async fn shutdown(self) {
let _ = self.shutdown_tx.send(true); let _ = self.shutdown_tx.send(true);
for task in self.tasks { 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>, mut incoming: mpsc::Receiver<CanFrame>,
motors: Arc<Vec<EscMotor>>, motors: Arc<Vec<EscMotor<S>>>,
mut shutdown: watch::Receiver<bool>, mut shutdown: watch::Receiver<bool>,
) { ) {
loop { loop {
@@ -407,22 +344,20 @@ async fn rx_loop(
None => break, None => break,
}, },
}; };
match decode_feedback(&frame) { match <S::Protocol as MotorProtocol>::decode_feedback(&frame) {
Ok((esc_id, fb)) => { Ok((esc_id, fb)) => {
if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) { 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) => { Err(e) => debug!(error = %e, id = frame.id, "ignore non-feedback frame"),
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>, outgoing: mpsc::Sender<CanFrame>,
motors: Arc<Vec<EscMotor>>, motors: Arc<Vec<EscMotor<S>>>,
policy: Arc<Mutex<P>>, policy: Arc<Mutex<P>>,
options: BusOptions, options: BusOptions,
mut shutdown: watch::Receiver<bool>, mut shutdown: watch::Receiver<bool>,
@@ -453,28 +388,29 @@ async fn tx_loop<P: EffortPolicy>(
} }
} }
let efforts = lock(&policy).step(&PolicyInput { samples, dt }); let efforts = lock(&policy).step(&PolicyInput { samples, dt });
for (frame_id, group_start) in for group_start in [1u8, 5u8] {
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)] let Some(frame_id) = <S::Protocol as MotorProtocol>::command_frame_id(group_start)
{ else {
let group: Vec<&EscMotor> = motors continue;
};
let group: Vec<&EscMotor<S>> = motors
.iter() .iter()
.filter(|m| (group_start..group_start + 4).contains(&m.id())) .filter(|m| (group_start..group_start + 4).contains(&m.id()))
.collect(); .collect();
if group.is_empty() { if group.is_empty() {
continue; continue;
} }
let mut currents = [0i16; 4]; let mut commands = [S::Protocol::command_from_raw(0); 4];
for motor in &group { for motor in &group {
let slot = usize::from(motor.id() - group_start); let slot = usize::from(motor.id() - group_start);
let policy_index = usize::from(motor.id().saturating_sub(1)); let policy_index = usize::from(motor.id().saturating_sub(1));
let physical = efforts.get(policy_index).copied().unwrap_or(0); let physical = efforts.get(policy_index).copied().unwrap_or(0);
let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16; let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16;
currents[slot] = clamp_current( let limited =
motor.shared.cfg.model, directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit);
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"); info!("can link closed, tx loop exiting");
return; return;
} }
@@ -484,7 +420,6 @@ async fn tx_loop<P: EffortPolicy>(
fn tick_every(period: Duration) -> Interval { fn tick_every(period: Duration) -> Interval {
let mut ticker = tokio::time::interval(period); let mut ticker = tokio::time::interval(period);
// 错过节拍后直接跳到当前节拍,避免补发帧风暴。
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
ticker ticker
} }
@@ -492,49 +427,35 @@ fn tick_every(period: Duration) -> Interval {
#[cfg(test)] #[cfg(test)]
mod tests { mod tests {
use super::*; use super::*;
use motor::{Gm3508, M2006};
#[test] #[test]
fn config_validate_rejects_bad_values() { fn typed_defaults_are_sane() {
let mut cfg = EscConfig::gm3508("m", 1, 1.0); let cfg = EscConfig::<Gm3508>::gm3508("front_left", 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);
assert!((cfg.gear_ratio - 19.2).abs() < 0.01); assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
assert_eq!(cfg.current_limit, 12_000); assert_eq!(cfg.current_limit, 12_000);
assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits());
assert!(cfg.validate().is_ok()); assert!(cfg.validate().is_ok());
}
#[test] let cfg = EscConfig::<M2006>::m2006("feeder", 5, 1.0);
fn m2006_defaults_are_sane() {
let cfg = EscConfig::m2006("feeder", 5, 1.0);
assert_eq!(cfg.model, EscModel::C610);
assert!((cfg.gear_ratio - 36.0).abs() < 0.01); assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
assert!(cfg.validate().is_ok()); 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 } delta10-protocol = { workspace = true }
tokio = { workspace = true } tokio = { workspace = true }
thiserror = { workspace = true } thiserror = { workspace = true }
[package.metadata.sentry-chassis]
layer = "device/imu"
@@ -16,3 +16,6 @@ thiserror = { workspace = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } 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 command::*;
pub use gearbox::*; pub use gearbox::*;
pub use heading::*; pub use heading::*;
pub use motor_common::MotorKind;
pub use registry::*; pub use registry::*;
pub use state::*; pub use state::*;
pub use testing::*; pub use testing::*;
pub use traits::*; 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] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "experiment/trace"
@@ -14,3 +14,6 @@ motor-protocol = { workspace = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "model/motor"
@@ -108,6 +108,24 @@ pub mod dc_motor {
self.load_rpm = rpm; self.load_rpm = rpm;
self 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` 便于在同一组参数上重放多条场景。 /// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
@@ -169,6 +187,9 @@ pub mod dc_motor {
/// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`, /// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`,
/// 即历史的一阶模型。 /// 即历史的一阶模型。
pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback { 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 applied = self.apply_delay(f32::from(current_raw));
let net = self.net_torque_raw(applied); let net = self.net_torque_raw(applied);
let target = (net * self.params.gain_rpm_per_raw) let target = (net * self.params.gain_rpm_per_raw)
@@ -184,6 +205,10 @@ pub mod dc_motor {
let ticks = f32::from(ENCODER_TICKS); let ticks = f32::from(ENCODER_TICKS);
self.angle_raw = self.angle_raw =
(self.angle_raw + self.speed_rpm / 60.0 * ticks * dt).rem_euclid(ticks); (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 { EscFeedback {
angle_raw: self.angle_raw as u16, angle_raw: self.angle_raw as u16,
speed_rpm: self.speed_rpm as i16, speed_rpm: self.speed_rpm as i16,
@@ -356,6 +381,36 @@ pub mod dc_motor {
assert_eq!(fb.temperature_c, 30); 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] #[test]
fn reset_returns_to_rest() { fn reset_returns_to_rest() {
let mut plant = PlantMotor::gm3508(); let mut plant = PlantMotor::gm3508();
@@ -371,7 +426,7 @@ pub mod dc_motor {
} }
pub mod gm3508 { 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 dc_motor::{PlantMotor, PlantParams};
pub use gm3508::{bench_params, ideal_params}; 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] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "protocol/imu"
@@ -16,3 +16,6 @@ thiserror = { workspace = true }
[dev-dependencies] [dev-dependencies]
proptest = { workspace = true } proptest = { workspace = true }
approx = { 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 m2006::C610;
pub use m3508::C620; pub use m3508::C620;
pub use turn::*; 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] [dev-dependencies]
tokio = { workspace = true, features = ["test-util"] } tokio = { workspace = true, features = ["test-util"] }
approx = { workspace = true } approx = { workspace = true }
[package.metadata.sentry-chassis]
layer = "simulation/motor"
@@ -706,7 +706,7 @@ pub mod plant {
use motor_protocol::{EscFeedback, clamp_current}; use motor_protocol::{EscFeedback, clamp_current};
use crate::harness::LoopLimits; use crate::harness::LoopLimits;
use motor_model::{PlantMotor, PlantParams}; use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams};
/// 把被测电机的**转速**(rpm)当作输出、**effort raw** 当作输入的被控对象。 /// 把被测电机的**转速**(rpm)当作输出、**effort raw** 当作输入的被控对象。
/// ///
@@ -715,7 +715,7 @@ pub mod plant {
/// 不同的饱和行为。 /// 不同的饱和行为。
#[derive(Debug, Clone)] #[derive(Debug, Clone)]
pub struct SimPlant { pub struct SimPlant {
motor: PlantMotor, motor: DcMotorPlant<Gm3508Model>,
limits: LoopLimits, limits: LoopLimits,
steps: u64, steps: u64,
} }
@@ -724,7 +724,7 @@ pub mod plant {
/// 由参数构造(默认限幅与 [`LoopLimits::gm3508`] 一致)。 /// 由参数构造(默认限幅与 [`LoopLimits::gm3508`] 一致)。
#[must_use] #[must_use]
pub fn new(params: PlantParams) -> Self { 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, self.limits.model,
raw.clamp(-self.limits.current_limit_raw, self.limits.current_limit_raw), raw.clamp(-self.limits.current_limit_raw, self.limits.current_limit_raw),
); );
let feedback = self.motor.step(command, dt); if let Ok(feedback) = MotorPlant::step(&mut self.motor, command, dt) {
self.steps += 1; self.steps += 1;
return Ok(f32::from(feedback.speed_rpm)); return Ok(f32::from(feedback.speed_rpm));
} }
return Ok(self.motor.speed_rpm());
}
Ok(self.motor.speed_rpm()) Ok(self.motor.speed_rpm())
} }
fn reset(&mut self) { fn reset(&mut self) {
self.motor.reset(); MotorPlant::reset(&mut self.motor);
self.steps = 0; self.steps = 0;
} }
@@ -926,7 +928,7 @@ pub mod rig {
use tokio::time::{MissedTickBehavior, interval}; use tokio::time::{MissedTickBehavior, interval};
use tracing::trace; use tracing::trace;
use motor_model::{PlantMotor, PlantParams}; use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams};
/// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。 /// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。
/// ///
@@ -943,8 +945,8 @@ pub mod rig {
tick: Duration, tick: Duration,
) -> JoinHandle<()> { ) -> JoinHandle<()> {
let specs: Vec<(u8, PlantParams)> = specs.into_iter().collect(); let specs: Vec<(u8, PlantParams)> = specs.into_iter().collect();
let mut motors: HashMap<u8, PlantMotor> = let mut motors: HashMap<u8, DcMotorPlant<Gm3508Model>> =
specs.iter().map(|&(id, params)| (id, PlantMotor::new(params))).collect(); specs.iter().map(|&(id, params)| (id, DcMotorPlant::new(params))).collect();
let mut commands: HashMap<u8, i16> = specs.iter().map(|&(id, _)| (id, 0)).collect(); let mut commands: HashMap<u8, i16> = specs.iter().map(|&(id, _)| (id, 0)).collect();
let dt = tick.as_secs_f32(); let dt = tick.as_secs_f32();
let mut ticker = interval(tick); let mut ticker = interval(tick);
@@ -960,7 +962,9 @@ pub mod rig {
_ = ticker.tick() => { _ = ticker.tick() => {
for (&id, motor) in &mut motors { for (&id, motor) in &mut motors {
let cmd = commands.get(&id).copied().unwrap_or(0); 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 if link
.outgoing_tx .outgoing_tx
.send(encode_feedback_frame(id, &feedback)) .send(encode_feedback_frame(id, &feedback))
@@ -29,3 +29,6 @@ motor-model = { workspace = true }
[dev-dependencies] [dev-dependencies]
approx = { workspace = true } approx = { workspace = true }
proptest = { 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 std::path::{Path, PathBuf};
use experiment::{Scenario, SetpointProfile}; use experiment::{Scenario, SetpointProfile};
use motor_model::PlantParams; use motor_model::{DcModelSpec, PlantParams};
use serde::{Deserialize, Serialize}; use serde::{Deserialize, Serialize};
use sim_harness::PlantTolerances; use sim_harness::PlantTolerances;
use thiserror::Error; use thiserror::Error;
@@ -1724,29 +1724,29 @@ pub mod preset {
/// 离线被控对象模型。 /// 离线被控对象模型。
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "kebab-case")] #[serde(rename_all = "kebab-case")]
pub enum PlantSpec { pub enum PlantProfile {
/// 理想一阶模型(无摩擦、无延迟)。 /// 理想一阶模型(无摩擦、无延迟)。
Gm3508, Ideal,
/// 台架量级:静 / 库仑摩擦 + 2 拍延迟。 /// 台架量级:静 / 库仑摩擦 + 2 拍延迟。
Gm3508Bench, Bench,
} }
impl PlantSpec { impl PlantProfile {
/// 文本名。 /// 文本名。
#[must_use] #[must_use]
pub const fn name(self) -> &'static str { pub const fn name(self) -> &'static str {
match self { match self {
Self::Gm3508 => "gm3508", Self::Ideal => "ideal",
Self::Gm3508Bench => "gm3508-bench", Self::Bench => "bench",
} }
} }
/// 展开为被控对象参数。 /// 展开为被控对象参数。
#[must_use] #[must_use]
pub fn params(self) -> PlantParams { pub fn params<M: DcModelSpec>(self) -> PlantParams {
match self { match self {
Self::Gm3508 => PlantParams::gm3508(), Self::Ideal => M::ideal_params(),
Self::Gm3508Bench => PlantParams::gm3508_bench(), Self::Bench => M::bench_params(),
} }
} }
} }
@@ -1921,7 +1921,7 @@ pub mod preset {
/// 离线被控对象与域随机化。 /// 离线被控对象与域随机化。
SimSpec { SimSpec {
/// 名义模型。 /// 名义模型。
plant: PlantSpec, plant: PlantProfile,
/// 集成成员数(1 = 只用名义模型)。 /// 集成成员数(1 = 只用名义模型)。
ensemble: usize, ensemble: usize,
/// 鲁棒权重:0 = 只看均值,1 = 只看最坏成员。 /// 鲁棒权重:0 = 只看均值,1 = 只看最坏成员。
@@ -2040,7 +2040,7 @@ pub mod preset {
threads: Some(0), threads: Some(0),
}, },
sim: SimSpec { sim: SimSpec {
plant: Some(PlantSpec::Gm3508Bench), plant: Some(PlantProfile::Bench),
ensemble: Some(8), ensemble: Some(8),
robust_weight: Some(0.5), robust_weight: Some(0.5),
gain_pct: Some(0.2), gain_pct: Some(0.2),
@@ -2082,8 +2082,8 @@ pub mod preset {
/// ///
/// 缺项([`PresetError::Missing`])或区间 / 阈值 / 预算非法 /// 缺项([`PresetError::Missing`])或区间 / 阈值 / 预算非法
/// ([`PresetError::Invalid`])时返回错误。 /// ([`PresetError::Invalid`])时返回错误。
pub fn to_tuning(&self) -> Result<Tuning, PresetError> { pub fn to_tuning<M: DcModelSpec>(&self) -> Result<Tuning, PresetError> {
let plant_spec = self.sim.plant.unwrap_or(PlantSpec::Gm3508Bench); let plant_spec = self.sim.plant.unwrap_or(PlantProfile::Bench);
Ok(Tuning { Ok(Tuning {
backend: self.backend.unwrap_or(BackendSpec::Sim), backend: self.backend.unwrap_or(BackendSpec::Sim),
space: self.gain_space()?, space: self.gain_space()?,
@@ -2092,7 +2092,7 @@ pub mod preset {
acceptance: self.acceptance()?, acceptance: self.acceptance()?,
search: self.search()?, search: self.search()?,
plant_spec, plant_spec,
plant: plant_spec.params(), plant: plant_spec.params::<M>(),
tolerances: self.tolerances()?, tolerances: self.tolerances()?,
ensemble: self.ensemble()?, ensemble: self.ensemble()?,
robust_weight: self.robust_weight()?, robust_weight: self.robust_weight()?,
@@ -2417,7 +2417,7 @@ pub mod preset {
/// 搜索配置。 /// 搜索配置。
pub search: SearchConfig, pub search: SearchConfig,
/// 名义被控对象(枚举,供显示 / 复现)。 /// 名义被控对象(枚举,供显示 / 复现)。
pub plant_spec: PlantSpec, pub plant_spec: PlantProfile,
/// 名义被控对象(展开后的参数,供仿真重放)。 /// 名义被控对象(展开后的参数,供仿真重放)。
pub plant: PlantParams, pub plant: PlantParams,
/// 域随机化幅度。 /// 域随机化幅度。
@@ -2641,14 +2641,16 @@ top_k = 3
let text = toml::to_string(&PresetFile::builtin()).unwrap(); let text = toml::to_string(&PresetFile::builtin()).unwrap();
let round_trip = PresetFile::parse_str(&text).unwrap(); let round_trip = PresetFile::parse_str(&text).unwrap();
assert_eq!(round_trip, PresetFile::builtin()); 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] #[test]
fn builtin_matches_the_historical_cli_defaults() { fn builtin_matches_the_historical_cli_defaults() {
// 这些数字是 chassis-tune 历史的 CLI 默认值,改动等于改变整定的默认行为, // 这些数字是 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_eq!(tuning.backend, BackendSpec::Sim);
assert!((tuning.space.kp.min - 0.5).abs() < 1e-12); assert!((tuning.space.kp.min - 0.5).abs() < 1e-12);
assert!((tuning.space.kp.max - 30.0).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() { fn preset_overlays_only_the_fields_it_sets() {
let file = PresetFile::parse_str(MINIMAL).unwrap(); let file = PresetFile::parse_str(MINIMAL).unwrap();
let merged = Preset::builtin().overlay(file.select(None).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.samples, 32, "预设必须生效");
assert_eq!(tuning.search.top_k, 3); assert_eq!(tuning.search.top_k, 3);
// 未提及的项沿用内置默认。 // 未提及的项沿用内置默认。
@@ -2686,7 +2688,7 @@ top_k = 3
let mut overlay = Preset::default(); let mut overlay = Preset::default();
overlay.space.kp = Some(RangeSpec::new(1.0, 5.0)); overlay.space.kp = Some(RangeSpec::new(1.0, 5.0));
let merged = Preset::builtin().overlay(&overlay); 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.min - 1.0).abs() < 1e-12);
assert!((tuning.space.kp.max - 5.0).abs() < 1e-12); assert!((tuning.space.kp.max - 5.0).abs() < 1e-12);
assert_eq!(tuning.space.kp.scale, Scale::Log, "刻度必须从内置默认继承"); assert_eq!(tuning.space.kp.scale, Scale::Log, "刻度必须从内置默认继承");
@@ -2759,7 +2761,9 @@ top_k = 3
let build = |body: &str| -> Result<Tuning, PresetError> { let build = |body: &str| -> Result<Tuning, PresetError> {
let text = format!("version = 1\n[preset.a]\n{body}"); let text = format!("version = 1\n[preset.a]\n{body}");
let file = PresetFile::parse_str(&text)?; 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 = let error =
@@ -2794,13 +2798,13 @@ top_k = 3
// 缺 scale 的轴(内置换行默认提供了刻度,这里显式抹掉)。 // 缺 scale 的轴(内置换行默认提供了刻度,这里显式抹掉)。
let mut preset = Preset::builtin(); let mut preset = Preset::builtin();
preset.space.kd = Some(RangeSpec { min: 0.1, max: 1.0, scale: None }); 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}"); assert!(matches!(&error, PresetError::Invalid { field: "kd", .. }), "{error}");
} }
#[test] #[test]
fn missing_keys_are_reported_for_a_bare_preset() { 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}"); assert!(matches!(error, PresetError::Missing { .. }), "{error}");
} }
@@ -2810,16 +2814,16 @@ top_k = 3
preset.scenario.profile = Some(ProfileSpec::Staircase); preset.scenario.profile = Some(ProfileSpec::Staircase);
preset.scenario.stair_levels = None; preset.scenario.stair_levels = None;
assert!(matches!( assert!(matches!(
preset.to_tuning(), preset.to_tuning::<motor_model::Gm3508Model>(),
Err(PresetError::Missing { field: "scenario.stair_levels" }) Err(PresetError::Missing { field: "scenario.stair_levels" })
)); ));
preset.scenario.stair_levels = Some(0); preset.scenario.stair_levels = Some(0);
assert!(matches!( assert!(matches!(
preset.to_tuning(), preset.to_tuning::<motor_model::Gm3508Model>(),
Err(PresetError::Invalid { field: "scenario.stair_levels", .. }) Err(PresetError::Invalid { field: "scenario.stair_levels", .. })
)); ));
preset.scenario.stair_levels = Some(5); preset.scenario.stair_levels = Some(5);
assert!(preset.to_tuning().is_ok()); assert!(preset.to_tuning::<motor_model::Gm3508Model>().is_ok());
} }
#[test] #[test]
@@ -2854,7 +2858,7 @@ top_k = 3
assert_eq!(spec.name(), text); assert_eq!(spec.name(), text);
assert_eq!(parse(text).value, spec); 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!(SolverSpec::NelderMead.name(), "nelder-mead");
assert_eq!(BackendSpec::Bench.name(), "bench"); assert_eq!(BackendSpec::Bench.name(), "bench");
let profile: Wrapper<ProfileSpec> = let profile: Wrapper<ProfileSpec> =
@@ -16,11 +16,10 @@ thiserror = { workspace = true }
tracing = { workspace = true } tracing = { workspace = true }
[dev-dependencies] [dev-dependencies]
esc-bus = { workspace = true }
motor = { workspace = true }
motor-control = { workspace = true }
tokio = { workspace = true } tokio = { workspace = true }
motor-protocol = { workspace = true }
[target.'cfg(target_os = "linux")'.dependencies] [target.'cfg(target_os = "linux")'.dependencies]
socketcan-rs = { workspace = true } socketcan-rs = { workspace = true }
[package.metadata.sentry-chassis]
layer = "transport/can"
@@ -19,3 +19,6 @@ serial = { workspace = true }
[dev-dependencies] [dev-dependencies]
tokio = { workspace = true } 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、限流贴近实际 | | 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 |
| 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 | | 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 |
### 7.4 被控对象模型(`sim-harness::motor_model`) ### 7.4 被控对象模型(`motor-model::MotorPlant`)
逐拍离散(`dt` 秒,电调坐标系:正电流 → 正转速): 逐拍离散(`dt` 秒,电调坐标系:正电流 → 正转速):
@@ -299,7 +299,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp
## 8. 配置参考(chassis.toml) ## 8. 配置参考(chassis.toml)
样例:[`config/chassis.toml.example`](../config/chassis.toml.example)。 样例:[`config/chassis.toml.example`](../config/chassis.toml.example)。
schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程与整定工具共用同一份**—— schema 由 [`chassis-config`](../crates/config/chassis) 定义,**守护进程与整定工具共用同一份**——
这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。 这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。
### 8.1 顶层 ### 8.1 顶层
@@ -308,11 +308,23 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义,**守护进程
| --- | --- | | --- | --- |
| `[can] interface` | 底盘总线 SocketCAN 接口名(如 `can0`) | | `[can] interface` | 底盘总线 SocketCAN 接口名(如 `can0`) |
| `[can.function] interface` | 预留的功能总线(云台 / 拨弹),必须与底盘总线**不同名** | | `[can.function] interface` | 预留的功能总线(云台 / 拨弹),必须与底盘总线**不同名** |
| `[remote]` | 可选 DR16/D-BUS 遥控输入;缺省时保持静止 |
| `[chassis]` | 控制循环与安全超时、速度上限 | | `[chassis]` | 控制循环与安全超时、速度上限 |
| `[mecanum]` | `half_length` / `half_width` / `wheel_radius`(米) | | `[mecanum]` | `half_length` / `half_width` / `wheel_radius`(米) |
| `[[motor]]` × 4 | 顺序固定 FL / FR / RL / RR | | `[[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`) | | `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) | | `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 钳位点 | | `current_limit` | effort 限幅(raw),同时是 PID 钳位点 |
| `kp` / `ki` / `kd` / `feedforward` | 速度环增益(整定的产物写回这里) | | `kp` / `ki` / `kd` / `feedforward` | 速度环增益(整定的产物写回这里) |
### 8.4 校验规则(`Config::validate`) ### 8.5 校验规则(`Config::validate`)
- 电机数必须**恰好 4**;麦轮几何必须为正;四个超时必须 > 0; - 电机数必须**恰好 4**;麦轮几何必须为正;四个超时必须 > 0;
`motor_command_timeout_ms ≥ control_period_ms`;速度上限 > 0;`gear_ratio > 0`; `motor_command_timeout_ms ≥ control_period_ms`;速度上限 > 0;`gear_ratio > 0`;
- 底盘总线**只允许一种协议族**(四轮不能混用 C620 / C610); - 底盘总线**只允许一种协议族**(四轮不能混用 C620 / C610);
- 功能总线接口名不得与底盘总线相同(双 CAN 物理隔离); - 功能总线接口名不得与底盘总线相同(双 CAN 物理隔离);
- `[remote]` 若启用:设备名非空,`timeout_ms` / `ramp_ms` > 0,`deadband ∈ [0, 0.5)`,
且遥控断流超时不得晚于底盘指令超时;
- `deny_unknown_fields`:**拼错的键直接报错**,不会被静默忽略。 - `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% | | `field_updown` | bench | **up-down** 1200 rpm | 实战验收:升 / 降 / 回零双向,阈值收紧到 12% / 3% |
**预设 schema**(`version = 1`,未知键报错,见 **预设 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 ```toml
version = 1 version = 1
@@ -588,7 +602,7 @@ cargo run --release -p spin -- motor -- --interface can0 --dry-run
| 整定 | 指标(合成轨迹解析解)、参数空间(界内 / 确定性 / 对数分布 / 分层覆盖)、成本(硬约束罚值)、搜索(同 seed 逐位复现、优于手调基线)、报告(CSV/TOML/JSON 往返) | 全平台(CI 跑 `just tune-smoke`) | | 整定 | 指标(合成轨迹解析解)、参数空间(界内 / 确定性 / 对数分布 / 分层覆盖)、成本(硬约束罚值)、搜索(同 seed 逐位复现、优于手调基线)、报告(CSV/TOML/JSON 往返) | 全平台(CI 跑 `just tune-smoke`) |
| **预设** | schema 往返、逐字段覆盖、优先级(CLI > 预设 > 内置)、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 | | **预设** | schema 往返、逐字段覆盖、优先级(CLI > 预设 > 内置)、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 |
| vcan 集成 | vcan0 双 socket 收发,真实内核 CAN 栈 | Linux(`#[ignore]`,CI 显式跑) | | 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 | | 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check;armv7 静态构建 `chassisd` | CI |
### 13.2 每个阶段完成后的门禁(`just lint` + 以下) ### 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: test:
cargo test --workspace --all-targets 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: vcan-test:
sudo modprobe vcan sudo modprobe vcan
sudo ip link add dev vcan0 type vcan || true sudo ip link add dev vcan0 type vcan || true
sudo ip link set up vcan0 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: run-mock:
cargo run -p chassisd -- --mock cargo run -p chassisd -- --mock
rc-monitor device="/dev/ttyS2":
cargo run --release -p spin -- rc --device {{device}}
check-structure: check-structure:
test -z "$(find crates/*/src -mindepth 2 -type d -print)" cargo +nightly -Zscript scripts/audit-rules.rs
! rg -n '\bunsafe\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml'
build-armv7: build-armv7:
rustup target add armv7-unknown-linux-musleabihf 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())
}
}