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