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