diff --git a/.githooks/pre-commit b/.githooks/pre-commit new file mode 100755 index 0000000..a598194 --- /dev/null +++ b/.githooks/pre-commit @@ -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 diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 91e8a68..881b239 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -16,14 +16,13 @@ jobs: - uses: dtolnay/rust-toolchain@stable with: components: rustfmt, clippy + - uses: dtolnay/rust-toolchain@nightly - uses: Swatinem/rust-cache@v2 + - run: cargo +nightly -Zscript scripts/audit-rules.rs --self-test + - run: cargo +nightly -Zscript scripts/audit-rules.rs - run: cargo fmt --all -- --check - run: cargo clippy --workspace --all-targets -- -D warnings - run: cargo test --workspace --all-targets - - name: enforce flat crate source - run: test -z "$(find crates/*/src -mindepth 2 -type d -print)" - - name: forbid unsafe and no_std markers - run: "! rg '\\bunsafe\\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml'" - name: optional plot feature run: cargo check -p chassis-tune --features plot --all-targets - name: deterministic tuning smoke @@ -42,7 +41,7 @@ jobs: sudo modprobe vcan sudo ip link add dev vcan0 type vcan sudo ip link set up vcan0 - - run: cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1 + - run: cargo test -p integration-tests --test socketcan_vcan -- --ignored --test-threads 1 cross-check: runs-on: ubuntu-latest diff --git a/.gitignore b/.gitignore index ea8c4bf..d8fd8c3 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,4 @@ /target + +__pycache__/ +*.pyc diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md index da09b38..afeebde 100644 --- a/ARCHITECTURE.md +++ b/ARCHITECTURE.md @@ -13,6 +13,7 @@ - **依赖必须无环**：允许依赖同层或更低抽象，但禁止反向控制依赖和隐藏的完整链路打包。 - **std，禁止 unsafe**：workspace 使用 `unsafe_code = "forbid"`；允许使用提供安全 API 的第三方依赖。 - **实时调度交给 systemd**：进程不调用 libc FFI，不实现 `SCHED_FIFO` 或 `mlockall`。 +- **二级语义分层**：crate 名不变，manifest 按 `crates//` 组织；依赖只能由高层指向低层。 - **crate 内保持扁平**：只允许 Cargo 惯例目录 `src`、`tests`；`src` 内不建立模块子目录。 - **安全边界独立于控制律**：总线负责反馈新鲜度、命令租约和零电流；控制器通过策略接口组合。 @@ -26,11 +27,13 @@ 协议 motor-protocol C620/C610、GM6020 编解码、多圈与 ID delta10-protocol DETA10 帧布局与解码 + dr16-protocol DJI DR16 D-BUS 帧布局与解码 驱动 socketcan Linux SocketCAN ↔ CanLink slcan Lawicel 串口 CAN ↔ CanLink delta10 DETA10 IMU 驱动 + dr16 DR16 D-BUS 串口接收驱动 设备与运行时 motor Motor 能力 trait、状态、命令、减速比、注册表、EffortPolicy @@ -53,20 +56,29 @@ chassisd 底盘守护进程二进制 spin chassis / motor 台架二进制 chassis-tune 自动整定二进制与报告/绘图适配 + +验证 + integration-tests 跨层闭环、语义一致性与 vcan 集成测试 ``` -共 21 个 crate。crate 名不使用 `rm-` 等无领域含义的统一前缀。 +共 24 个 crate（含顶层 `verification/integration` 集成测试包）。crate 名不使用 `rm-` 等无领域含义的统一前缀。 ## 3. 允许依赖 +机器可读规则见 [`config/dependency-policy.toml`](config/dependency-policy.toml) 和 +[`docs/dependencies.md`](docs/dependencies.md)。`just audit` 同时检查层 rank、正常/dev 依赖、 +外部库职责和 trait 边界；正常与 dev 依赖都必须单向。 + 主要边如下： ```text motor-protocol -> can delta10-protocol -> can, imu +dr16-protocol -> (无内部依赖) socketcan -> can slcan -> can delta10 -> can, imu, delta10-protocol +dr16 -> dr16-protocol motor -> can, motor-protocol esc-bus -> can, motor, motor-protocol @@ -82,8 +94,8 @@ tuning -> pid, motor-control, motor-model, sim-harness, experiment chassis-config -> esc-bus, motor-control, motor-protocol, mecanum bench -> can, esc-bus, experiment, motor, motor-control, socketcan, slcan -chassisd -> chassis-config, esc-bus, mecanum, motor-control, sim-harness, socketcan -spin -> bench, esc-bus, motor-control, socketcan, slcan +chassisd -> chassis-config, dr16, esc-bus, mecanum, motor-control, sim-harness, socketcan +spin -> bench, dr16, esc-bus, motor-control, socketcan, slcan chassis-tune -> bench, chassis-config, experiment, sim-harness, tuning ``` @@ -136,7 +148,8 @@ tests/*.rs # 可选集成测试 - 四轮 ESC 总线、命令租约、反馈离线保护、外部控制策略。 - PID、速度/位置/串级/前馈、麦轮运动学。 - 电机模型、总线损伤、同步/异步闭环、参数搜索、预设与报告。 -- `chassisd --mock`、`spin chassis|motor`、`chassis-tune`。 +- `chassisd --mock`、`spin chassis|motor|rc`、`chassis-tune`。 +- DR16/D-BUS 遥控输入、S1 使能、断流失能和 500 ms 速度斜坡。 未实现： diff --git a/Cargo.lock b/Cargo.lock index 6577a44..fd25ec1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -205,6 +205,7 @@ dependencies = [ "approx", "esc-bus", "mecanum", + "motor", "motor-control", "motor-protocol", "serde", @@ -256,6 +257,8 @@ dependencies = [ "can", "chassis-config", "clap", + "dr16", + "dr16-protocol", "esc-bus", "mecanum", "motor", @@ -540,6 +543,24 @@ dependencies = [ "libloading", ] +[[package]] +name = "dr16" +version = "0.1.0" +dependencies = [ + "dr16-protocol", + "serial", + "tokio", + "tracing", +] + +[[package]] +name = "dr16-protocol" +version = "0.1.0" +dependencies = [ + "proptest", + "thiserror", +] + [[package]] name = "dwrote" version = "0.11.5" @@ -595,12 +616,8 @@ version = "0.1.0" dependencies = [ "approx", "can", - "experiment", "motor", - "motor-control", - "motor-model", "motor-protocol", - "sim-harness", "thiserror", "tokio", "tracing", @@ -873,6 +890,22 @@ dependencies = [ "web-time", ] +[[package]] +name = "integration-tests" +version = "0.1.0" +dependencies = [ + "can", + "esc-bus", + "experiment", + "motor", + "motor-control", + "motor-model", + "motor-protocol", + "sim-harness", + "socketcan 0.1.0", + "tokio", +] + [[package]] name = "ioctl-rs" version = "0.1.6" @@ -1662,10 +1695,6 @@ name = "socketcan" version = "0.1.0" dependencies = [ "can", - "esc-bus", - "motor", - "motor-control", - "motor-protocol", "socketcan 4.0.0", "thiserror", "tokio", @@ -1699,6 +1728,8 @@ version = "0.1.0" dependencies = [ "bench", "clap", + "dr16", + "dr16-protocol", "esc-bus", "experiment", "mecanum", diff --git a/Cargo.toml b/Cargo.toml index 0e17117..7a68ac0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,27 +1,30 @@ [workspace] resolver = "3" members = [ - "crates/can", - "crates/imu", - "crates/motor-protocol", - "crates/delta10-protocol", - "crates/socketcan", - "crates/slcan", - "crates/delta10", - "crates/motor", - "crates/esc-bus", - "crates/pid", - "crates/motor-control", - "crates/mecanum", - "crates/motor-model", - "crates/experiment", - "crates/sim-harness", - "crates/tuning", - "crates/chassis-config", - "crates/bench", - "crates/chassisd", - "crates/spin", - "crates/chassis-tune", + "crates/contract/can", + "crates/contract/imu", + "crates/protocol/motor", + "crates/protocol/imu", + "crates/transport/can", + "crates/transport/serial", + "crates/device/imu", + "crates/device/motor", + "crates/device/esc", + "crates/control/algorithm", + "crates/control/motor", + "crates/control/chassis", + "crates/protocol/remote", + "crates/device/remote", + "crates/model/motor", + "crates/experiment/trace", + "crates/simulation/motor", + "crates/tooling/tuning", + "crates/config/chassis", + "crates/application/bench", + "crates/application/daemon", + "crates/application/tools", + "crates/application/tuning", + "crates/verification/integration", ] [workspace.package] @@ -71,27 +74,30 @@ anyhow = "1" plotters = { version = "0.3", default-features = false, features = ["svg_backend", "line_series", "ttf"] } # ---- 职责型内部 crate ---- -can = { path = "crates/can" } -imu = { path = "crates/imu" } -motor-protocol = { path = "crates/motor-protocol" } -delta10-protocol = { path = "crates/delta10-protocol" } -socketcan = { path = "crates/socketcan" } -slcan = { path = "crates/slcan" } -delta10 = { path = "crates/delta10" } -motor = { path = "crates/motor" } -esc-bus = { path = "crates/esc-bus" } -pid = { path = "crates/pid" } -motor-control = { path = "crates/motor-control" } -mecanum = { path = "crates/mecanum" } -motor-model = { path = "crates/motor-model" } -experiment = { path = "crates/experiment" } -sim-harness = { path = "crates/sim-harness" } -tuning = { path = "crates/tuning" } -chassis-config = { path = "crates/chassis-config" } -bench = { path = "crates/bench" } -chassisd = { path = "crates/chassisd" } -spin = { path = "crates/spin" } -chassis-tune = { path = "crates/chassis-tune" } +can = { path = "crates/contract/can" } +imu = { path = "crates/contract/imu" } +motor-protocol = { path = "crates/protocol/motor" } +delta10-protocol = { path = "crates/protocol/imu" } +socketcan = { path = "crates/transport/can" } +slcan = { path = "crates/transport/serial" } +delta10 = { path = "crates/device/imu" } +motor = { path = "crates/device/motor" } +esc-bus = { path = "crates/device/esc" } +pid = { path = "crates/control/algorithm" } +motor-control = { path = "crates/control/motor" } +mecanum = { path = "crates/control/chassis" } +dr16-protocol = { path = "crates/protocol/remote" } +dr16 = { path = "crates/device/remote" } +motor-model = { path = "crates/model/motor" } +experiment = { path = "crates/experiment/trace" } +sim-harness = { path = "crates/simulation/motor" } +tuning = { path = "crates/tooling/tuning" } +chassis-config = { path = "crates/config/chassis" } +bench = { path = "crates/application/bench" } +chassisd = { path = "crates/application/daemon" } +spin = { path = "crates/application/tools" } +chassis-tune = { path = "crates/application/tuning" } +integration-tests = { path = "crates/verification/integration" } [workspace.lints.rust] unsafe_code = "forbid" diff --git a/README.md b/README.md index 01ba60c..2d84a9a 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,17 @@ RoboMaster 哨兵机器人麦轮底盘：Linux ARM 主控、4 × GM3508/C620、SocketCAN 或 SLCAN、tokio 异步运行时。 -架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 21 个职责单一 crate 组成，不使用 `rm-` 前缀，crate 内源码保持扁平。 +架构规范见 [`ARCHITECTURE.md`](ARCHITECTURE.md)。workspace 由 24 个职责单一 crate / 测试包组成，不使用 `rm-` 前缀，crate 内源码保持扁平。 + +安装本地规则 hook： + +```sh +just install-hooks +just audit +``` + +依赖层级、外部库白名单和 trait 边界见 [`docs/dependencies.md`](docs/dependencies.md)。 +规则审查器是基于 Nightly `cargo -Zscript` 的单文件 Rust 脚本。 ## 结构 @@ -12,6 +22,7 @@ RoboMaster 哨兵机器人麦轮底盘：Linux ARM 主控、4 × GM3508/C620、S - `motor` / `esc-bus`：设备契约与安全总线运行时。 - `pid` / `motor-control` / `mecanum`：纯控制算法。 - `motor-model` / `experiment` / `sim-harness` / `tuning`：仿真与自动整定。 +- `dr16-protocol` / `dr16`：DJI DR16/D-BUS 协议和串口接收。 - `chassis-config` / `bench` / `chassisd` / `spin` / `chassis-tune`：配置、台架和应用入口。 `esc-bus` 不依赖控制算法。应用把 `motor-control::MotorPolicy` 作为 diff --git a/config/chassis.toml.example b/config/chassis.toml.example index f61ebe8..bc6b35e 100644 --- a/config/chassis.toml.example +++ b/config/chassis.toml.example @@ -4,6 +4,21 @@ [can] interface = "can0" +# DJI DT7 + DR16 遥控接收机。没有遥控时删除整个 [remote] 段，守护进程会保持静止。 +[remote] +kind = "dr16" +device = "/dev/ttyS2" +# 合法 D-BUS 帧断流超时（毫秒）；超时立即失能停车。 +timeout_ms = 100 +# 摇杆满量程死区（0..0.5），死区后使用平方响应曲线。 +deadband = 0.05 +# 从零到满量程的逐轴速度斜坡（毫秒）；S1 失能、断流和超时立即归零。 +ramp_ms = 500 +# 实车方向相反时置 true，不需要改代码。 +invert_vx = false +invert_vy = false +invert_omega = false + [chassis] # 控制循环周期（毫秒）。C620 期望 1kHz 命令。 control_period_ms = 1 diff --git a/config/dependency-policy.toml b/config/dependency-policy.toml new file mode 100644 index 0000000..4d44705 --- /dev/null +++ b/config/dependency-policy.toml @@ -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"] diff --git a/config/tune.toml b/config/tune.toml index 2bd6087..d9dfa2a 100644 --- a/config/tune.toml +++ b/config/tune.toml @@ -19,7 +19,7 @@ # 不属于超参数，本文件**不接受**这些键（写了会直接报错）。它们只来自 # chassis.toml 或显式 CLI——否则预设文件就成了绕过限流的口子。 # -# schema：见 crates/chassis-tune/src/preset.rs（version = 1，未知键报错）。 +# schema：见 crates/application/tuning/src/main.rs（version = 1，未知键报错）。 version = 1 default_preset = "sim_bench_model" @@ -68,7 +68,7 @@ max_evals = 4000 threads = 0 [preset.sim_bench_model.sim] -plant = "gm3508-bench" +plant = "bench" # 域随机化：单点模型调出来的增益对增益/时间常数/延迟高度敏感， # 因此默认在 8 个成员的集成上做鲁棒优化，而不是对着一条理想曲线最优化。 ensemble = 8 @@ -128,7 +128,7 @@ max_evals = 1000 threads = 0 [preset.sim_ideal.sim] -plant = "gm3508" +plant = "ideal" # 理想模型本身就是确定的，单成员即可。 ensemble = 1 robust_weight = 0.0 @@ -193,7 +193,7 @@ threads = 1 [preset.bench_noload.sim] # 真机后端不用这些，但保留"最接近台架的模型"，方便先离线预演再上车。 -plant = "gm3508-bench" +plant = "bench" ensemble = 8 robust_weight = 0.5 gain_pct = 0.2 @@ -254,7 +254,7 @@ max_evals = 80 threads = 1 [preset.bench_loaded.sim] -plant = "gm3508-bench" +plant = "bench" ensemble = 8 robust_weight = 0.5 gain_pct = 0.2 @@ -314,7 +314,7 @@ max_evals = 40 threads = 1 [preset.field_updown.sim] -plant = "gm3508-bench" +plant = "bench" ensemble = 8 robust_weight = 0.5 gain_pct = 0.2 diff --git a/crates/bench/Cargo.toml b/crates/application/bench/Cargo.toml similarity index 91% rename from crates/bench/Cargo.toml rename to crates/application/bench/Cargo.toml index 98cce3d..0742ea5 100644 --- a/crates/bench/Cargo.toml +++ b/crates/application/bench/Cargo.toml @@ -24,3 +24,6 @@ anyhow = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } + +[package.metadata.sentry-chassis] +layer = "application/bench" diff --git a/crates/bench/src/lib.rs b/crates/application/bench/src/lib.rs similarity index 100% rename from crates/bench/src/lib.rs rename to crates/application/bench/src/lib.rs diff --git a/crates/chassisd/Cargo.toml b/crates/application/daemon/Cargo.toml similarity index 84% rename from crates/chassisd/Cargo.toml rename to crates/application/daemon/Cargo.toml index 7806fb4..40d6cf0 100644 --- a/crates/chassisd/Cargo.toml +++ b/crates/application/daemon/Cargo.toml @@ -15,6 +15,8 @@ motor = { workspace = true } esc-bus = { workspace = true } motor-control = { workspace = true } mecanum = { workspace = true } +dr16-protocol = { workspace = true } +dr16 = { workspace = true } chassis-config = { workspace = true } sim-harness = { workspace = true } socketcan = { workspace = true } @@ -26,3 +28,6 @@ tracing-subscriber = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "application/daemon" diff --git a/crates/chassisd/src/main.rs b/crates/application/daemon/src/main.rs similarity index 68% rename from crates/chassisd/src/main.rs rename to crates/application/daemon/src/main.rs index b9f1273..4c11065 100644 --- a/crates/chassisd/src/main.rs +++ b/crates/application/daemon/src/main.rs @@ -166,62 +166,63 @@ pub mod messages { pub use messages::*; pub mod source { - //! 上层速度指令源（twist source）。 + //! 上层底盘指令源。 //! - //! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中 - //! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现 - //! [`TwistSource`] 即可。 + //! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源持续产出 + //! [`RemoteCommand`]。遥控器、自主导航等来源共享同一接口，失能语义不会在 + //! 各来源里重复实现。 use std::future::Future; - use mecanum::Twist; use tokio::sync::{mpsc, watch}; - /// 速度指令源。 - pub trait TwistSource: Send { + use crate::RemoteCommand; + + /// 底盘指令源。 + pub trait CommandSource: Send { /// 取下一条指令；`None` 表示源关闭。 - fn next_twist(&mut self) -> impl Future> + Send; + fn next_command(&mut self) -> impl Future> + Send; } - /// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。 + /// 恒失能占位源：永不产出，控制器超时逻辑兜底停车。 pub struct ZeroSource; - impl TwistSource for ZeroSource { - async fn next_twist(&mut self) -> Option { + impl CommandSource for ZeroSource { + async fn next_command(&mut self) -> Option { std::future::pending().await } } /// 通道指令源（测试 / 简单进程内注入用）。 pub struct ChannelSource { - rx: mpsc::Receiver, + rx: mpsc::Receiver, } impl ChannelSource { /// 由 mpsc 接收端构造。 #[must_use] - pub const fn new(rx: mpsc::Receiver) -> Self { + pub const fn new(rx: mpsc::Receiver) -> Self { Self { rx } } /// 创建一对（发送端, 源）。 #[must_use] - pub fn pair(bound: usize) -> (mpsc::Sender, Self) { + pub fn pair(bound: usize) -> (mpsc::Sender, Self) { let (tx, rx) = mpsc::channel(bound); (tx, Self { rx }) } } - impl TwistSource for ChannelSource { - async fn next_twist(&mut self) -> Option { + impl CommandSource for ChannelSource { + async fn next_command(&mut self) -> Option { self.rx.recv().await } } /// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。 - pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender) { - while let Some(twist) = source.next_twist().await { - if tx.send(twist).is_err() { + pub async fn pump_source(mut source: impl CommandSource, tx: watch::Sender) { + while let Some(command) = source.next_command().await { + if tx.send(command).is_err() { break; } } @@ -229,28 +230,249 @@ pub mod source { #[cfg(test)] mod tests { + use mecanum::Twist; + use super::*; #[tokio::test(start_paused = true)] async fn channel_source_pumps_into_watch() { let (tx, source) = ChannelSource::pair(8); - let (watch_tx, mut watch_rx) = watch::channel(Twist::ZERO); + let (watch_tx, mut watch_rx) = watch::channel(RemoteCommand::DISABLED); let pump = tokio::spawn(pump_source(source, watch_tx)); - tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).await.unwrap(); + let command = RemoteCommand::new(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }); + tx.send(command).await.unwrap(); tokio::time::sleep(std::time::Duration::from_millis(1)).await; - assert_eq!(*watch_rx.borrow_and_update(), Twist { vx: 1.0, vy: 0.0, omega: 0.0 }); + assert_eq!(*watch_rx.borrow_and_update(), command); // 源关闭 → 泵任务退出。 drop(tx); pump.await.unwrap(); - assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值 + assert_eq!(watch_rx.borrow().twist, command.twist); } } } pub use source::*; +pub mod remote { + //! DR16 遥控输入到安全底盘指令的映射。 + //! + //! 左摇杆上下控制前后、左右控制旋转；右摇杆左右控制横移。摇杆先过死区， + //! 再做平方响应曲线。S1 上档是唯一的运动使能开关。 + + use std::future::pending; + use std::time::{Duration, Instant}; + + use chassis_config::RemoteConfig; + use dr16::{SampleStream, StampedSample}; + use dr16_protocol::{Dr16Sample, SwitchPosition}; + use mecanum::Twist; + + use crate::{CommandSource, RemoteCommand}; + + /// DR16 摇杆满量程的协议值。 + const FULL_SCALE: f32 = 660.0; + + /// 遥控输入映射器。 + #[derive(Debug, Clone, Copy)] + pub struct RemoteMapper { + deadband: f32, + max_vx: f32, + max_vy: f32, + max_omega: f32, + invert_vx: bool, + invert_vy: bool, + invert_omega: bool, + } + + impl RemoteMapper { + /// 创建映射器；配置语义由 `chassis-config::Config::validate` 保证。 + #[must_use] + pub const fn new( + config: &RemoteConfig, + max_forward: f32, + max_strafe: f32, + max_spin: f32, + ) -> Self { + Self { + deadband: config.deadband, + max_vx: max_forward, + max_vy: max_strafe, + max_omega: max_spin, + invert_vx: config.invert_vx, + invert_vy: config.invert_vy, + invert_omega: config.invert_omega, + } + } + + /// 将一个合法 DR16 样本映射为底盘意图。 + #[must_use] + pub fn map(&self, sample: &Dr16Sample) -> RemoteCommand { + let vx = self.axis(sample.channel_t(), self.max_vx, self.invert_vx); + // ISO 坐标 y 向左为正，右摇杆向右应产生负 y。 + let vy = -self.axis(sample.channel_a(), self.max_vy, self.invert_vy); + // ISO 坐标 omega 逆时针为正，左摇杆向右应产生顺时针负 omega。 + let omega = -self.axis(sample.channel_r(), self.max_omega, self.invert_omega); + RemoteCommand { + twist: Twist { vx, vy, omega }, + enabled: sample.s1 == SwitchPosition::Up, + } + } + + fn axis(&self, raw: i16, maximum: f32, invert: bool) -> f32 { + let sign = if invert { -1.0 } else { 1.0 }; + sign * self.shaped(raw) * maximum + } + + fn shaped(&self, raw: i16) -> f32 { + let normalized = (f32::from(raw) / FULL_SCALE).clamp(-1.0, 1.0); + let magnitude = normalized.abs(); + if magnitude <= self.deadband { + return 0.0; + } + let scaled = (magnitude - self.deadband) / (1.0 - self.deadband); + normalized.signum() * scaled * scaled + } + } + + /// DR16 样本源：超时和串口关闭都会立即发布失能指令。 + pub struct Dr16CommandSource { + samples: SampleStream, + mapper: RemoteMapper, + timeout: Duration, + last_received_at: Instant, + timed_out: bool, + closed: bool, + } + + impl Dr16CommandSource { + /// 创建 DR16 指令源。 + #[must_use] + pub fn new(samples: SampleStream, mapper: RemoteMapper, timeout: Duration) -> Self { + Self { + samples, + mapper, + timeout, + last_received_at: Instant::now(), + timed_out: false, + closed: false, + } + } + + fn command(&mut self, stamped: StampedSample) -> RemoteCommand { + self.last_received_at = stamped.received_at; + self.timed_out = false; + self.mapper.map(&stamped.sample) + } + } + + impl CommandSource for Dr16CommandSource { + async fn next_command(&mut self) -> Option { + if self.closed { + return pending().await; + } + + if self.timed_out { + return if let Some(stamped) = self.samples.recv().await { + Some(self.command(stamped)) + } else { + self.closed = true; + Some(RemoteCommand::DISABLED) + }; + } + + let elapsed = Instant::now().saturating_duration_since(self.last_received_at); + let remaining = self.timeout.saturating_sub(elapsed); + match tokio::time::timeout(remaining, self.samples.recv()).await { + Ok(Some(stamped)) => Some(self.command(stamped)), + Ok(None) => { + self.closed = true; + Some(RemoteCommand::DISABLED) + } + Err(_) => { + self.timed_out = true; + Some(RemoteCommand::DISABLED) + } + } + } + } + + #[cfg(test)] + mod tests { + use dr16::sample_channel; + use dr16_protocol::ExtendedInput; + + use super::*; + + fn sample(channels: [i16; 4], s1: SwitchPosition) -> Dr16Sample { + Dr16Sample { channels, s1, s2: SwitchPosition::Mid, extended: ExtendedInput::default() } + } + + fn mapper() -> RemoteMapper { + RemoteMapper::new(&RemoteConfig::default(), 3.0, 2.0, 4.0) + } + + #[test] + fn maps_left_stick_to_forward_and_rotation_and_right_stick_to_strafe() { + let command = mapper().map(&sample([660, 0, 660, 660], SwitchPosition::Up)); + assert!(command.enabled); + assert!((command.twist.vx - 3.0).abs() < 1e-6); + assert!((command.twist.vy + 2.0).abs() < 1e-6); + assert!((command.twist.omega + 4.0).abs() < 1e-6); + } + + #[test] + fn applies_deadband_square_curve_and_inversions() { + let config = RemoteConfig { + invert_vx: true, + invert_vy: true, + invert_omega: true, + ..RemoteConfig::default() + }; + let mapper = RemoteMapper::new(&config, 1.0, 1.0, 1.0); + assert!(mapper.map(&sample([0, 0, 33, 0], SwitchPosition::Up)).twist.vx.abs() < 1e-6); + + let command = mapper.map(&sample([660, 0, 660, 660], SwitchPosition::Up)); + assert!((command.twist.vx + 1.0).abs() < 1e-6); + assert!((command.twist.vy - 1.0).abs() < 1e-6); + assert!((command.twist.omega - 1.0).abs() < 1e-6); + } + + #[test] + fn s1_must_be_up_and_bad_switch_is_disabled() { + for position in [SwitchPosition::Down, SwitchPosition::Mid] { + assert!(!mapper().map(&sample([660, 660, 660, 660], position)).enabled); + } + } + + #[tokio::test(start_paused = true)] + async fn timeout_publishes_disabled_once_and_recovers() { + let (sender, samples) = sample_channel(); + let mut source = Dr16CommandSource::new(samples, mapper(), Duration::from_millis(100)); + let _ = sender.send(sample([660, 0, 0, 0], SwitchPosition::Up)); + assert!(source.next_command().await.unwrap().enabled); + + let disabled = source.next_command().await.unwrap(); + assert!(!disabled.enabled); + assert_eq!(disabled.effective_twist(), Twist::ZERO); + + let _ = sender.send(sample([0, 0, 0, 0], SwitchPosition::Up)); + assert!(source.next_command().await.unwrap().enabled); + } + + #[tokio::test(start_paused = true)] + async fn closed_stream_disables_immediately() { + let (sender, samples) = sample_channel(); + drop(sender); + let mut source = Dr16CommandSource::new(samples, mapper(), Duration::from_millis(100)); + let command = source.next_command().await.unwrap(); + assert!(!command.enabled); + assert_eq!(command.effective_twist(), Twist::ZERO); + } + } +} + pub mod telemetry { //! 诊断输出：控制周期抖动统计（ARCHITECTURE.md §7.2）。 //! @@ -569,11 +791,52 @@ pub mod controller { use tokio::time::{Instant, MissedTickBehavior, interval}; use tracing::{info, warn}; + use crate::RemoteCommand; + + /// 逐轴速度斜坡。 + #[derive(Debug, Clone, Copy)] + struct TwistRamp { + current: Twist, + step_vx: f32, + step_vy: f32, + step_omega: f32, + } + + impl TwistRamp { + fn new(limits: (f32, f32, f32), period: Duration, ramp: Duration) -> Self { + let ratio = (period.as_secs_f32() / ramp.as_secs_f32()).clamp(0.0, 1.0); + Self { + current: Twist::ZERO, + step_vx: limits.0 * ratio, + step_vy: limits.1 * ratio, + step_omega: limits.2 * ratio, + } + } + + fn advance(&mut self, target: Twist) -> Twist { + self.current = Twist { + vx: approach(self.current.vx, target.vx, self.step_vx), + vy: approach(self.current.vy, target.vy, self.step_vy), + omega: approach(self.current.omega, target.omega, self.step_omega), + }; + self.current + } + + fn reset(&mut self) { + self.current = Twist::ZERO; + } + } + + fn approach(current: f32, target: f32, maximum_step: f32) -> f32 { + let delta = target - current; + if delta.abs() <= maximum_step { target } else { current + delta.signum() * maximum_step } + } + /// 底盘控制器。 /// /// 职责（1kHz 节拍）： - /// 1. 读取最新 twist 指令，**指令超时则归零**； - /// 2. 速度限幅（保持方向按比例收缩）； + /// 1. 读取最新 [`RemoteCommand`]，超时或失能时立即归零； + /// 2. 速度限幅（保持方向按比例收缩），再逐轴斜坡平滑； /// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用； /// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。 /// @@ -585,6 +848,7 @@ pub mod controller { /// (max_vx, max_vy, max_omega)。 limits: (f32, f32, f32), command_timeout: Duration, + ramp_time: Duration, } impl ChassisController @@ -598,19 +862,25 @@ pub mod controller { wheels: [W; 4], limits: (f32, f32, f32), command_timeout: Duration, + ramp_time: Duration, ) -> Self { - Self { kinematics, wheels, limits, command_timeout } + Self { kinematics, wheels, limits, command_timeout, ramp_time } } /// 启动控制循环任务。 - pub fn spawn(self, commands: watch::Receiver, period: Duration) -> JoinHandle<()> { + pub fn spawn( + self, + commands: watch::Receiver, + period: Duration, + ) -> JoinHandle<()> { tokio::spawn(self.run(commands, period)) } - async fn run(self, mut commands: watch::Receiver, period: Duration) { + async fn run(self, mut commands: watch::Receiver, period: Duration) { let mut ticker = interval(period); ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); - let mut latest = Twist::ZERO; + let mut ramp = TwistRamp::new(self.limits, period, self.ramp_time); + let mut latest = RemoteCommand::DISABLED; let mut last_command_at: Option = None; let mut ticks: u64 = 0; @@ -627,13 +897,21 @@ pub mod controller { // 尚未收到任何指令：保持静止。 None => true, }; - let twist = if timed_out { + let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback()); + let stop = timed_out || !latest.enabled || !all_seen_feedback; + + let twist = if stop { + ramp.reset(); Twist::ZERO } else { - latest.clamped(self.limits.0, self.limits.1, self.limits.2) + let target = latest.effective_twist().clamped( + self.limits.0, + self.limits.1, + self.limits.2, + ); + ramp.advance(target) }; - let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback()); if all_seen_feedback { let wheel_rpms = self.kinematics.wheel_rpms(&twist); for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) { @@ -696,6 +974,46 @@ pub mod controller { m } + #[test] + fn ramp_reaches_full_scale_in_requested_time_and_resets() { + let mut ramp = TwistRamp::new( + (2.0, 2.0, 2.0), + Duration::from_millis(1), + Duration::from_millis(500), + ); + let target = Twist { vx: 2.0, ..Twist::ZERO }; + for _ in 0..250 { + ramp.advance(target); + } + assert!((ramp.current.vx - 1.0).abs() < 1e-5); + for _ in 0..250 { + ramp.advance(target); + } + assert!((ramp.current.vx - target.vx).abs() < 1e-4); + ramp.reset(); + assert_eq!(ramp.current, Twist::ZERO); + } + + #[test] + fn ramp_reverses_through_zero_without_overshoot() { + let mut ramp = TwistRamp::new( + (1.0, 1.0, 1.0), + Duration::from_millis(1), + Duration::from_millis(10), + ); + for _ in 0..10 { + ramp.advance(Twist { vy: 1.0, ..Twist::ZERO }); + } + let mut crossed_zero = false; + for _ in 0..20 { + let output = ramp.advance(Twist { vy: -1.0, ..Twist::ZERO }); + assert!(output.vy <= 1.0 + 1e-6 && output.vy >= -1.0 - 1e-6); + crossed_zero |= output.vy.abs() < 1e-6; + } + assert!(crossed_zero); + assert!((ramp.current.vy + 1.0).abs() < 1e-5); + } + /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。 #[tokio::test(start_paused = true)] async fn controller_drives_wheels_through_capability_traits() { @@ -706,12 +1024,13 @@ pub mod controller { wrap_wheels(mocks.clone(), [GEAR; 4]), (3.0, 3.0, 6.0), Duration::from_millis(200), + Duration::from_millis(1), ); - let (tx, rx) = watch::channel(Twist::ZERO); + let (tx, rx) = watch::channel(RemoteCommand::DISABLED); let handle = ctrl.spawn(rx, Duration::from_millis(1)); let twist = Twist { vx: 1.0, ..Twist::ZERO }; - tx.send_replace(twist); + tx.send_replace(RemoteCommand::new(twist)); tokio::time::sleep(Duration::from_millis(5)).await; let expected = kinematics.wheel_rpms(&twist); @@ -721,7 +1040,7 @@ pub mod controller { let want = expected[i] * GEAR; assert!( (rotor_rpm - want).abs() < 1e-2, - "wheel {i}: rotor_rpm {rotor_rpm} != {want}" + "wheel {i}: {rotor_rpm} != {want}" ); } ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"), @@ -736,9 +1055,7 @@ pub mod controller { async fn disables_wheels_until_all_feedback_seen() { let kinematics = Mecanum::new(0.2, 0.25, 0.0762); let ready = mock_wheel(1); - let missing = MockMotor::new(2); // 无反馈 - let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)]; - let _ = mocks_ready; + let missing = MockMotor::new(2); let wheels = wrap_wheels( [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], [GEAR; 4], @@ -748,11 +1065,12 @@ pub mod controller { wheels, (3.0, 3.0, 6.0), Duration::from_millis(200), + Duration::from_millis(500), ); - let (tx, rx) = watch::channel(Twist::ZERO); + let (tx, rx) = watch::channel(RemoteCommand::DISABLED); let handle = ctrl.spawn(rx, Duration::from_millis(1)); - tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + tx.send_replace(RemoteCommand::new(Twist { vx: 1.0, ..Twist::ZERO })); tokio::time::sleep(Duration::from_millis(5)).await; assert_eq!(ready.last_command(), Command::Disabled); @@ -760,6 +1078,29 @@ pub mod controller { handle.abort(); } + + #[tokio::test(start_paused = true)] + async fn disabled_command_stops_immediately_without_ramping() { + let kinematics = Mecanum::new(0.2, 0.25, 0.0762); + let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)]; + let ctrl = ChassisController::new( + kinematics, + wrap_wheels(mocks.clone(), [GEAR; 4]), + (3.0, 3.0, 6.0), + Duration::from_millis(200), + Duration::from_millis(500), + ); + let (tx, rx) = watch::channel(RemoteCommand::DISABLED); + let handle = ctrl.spawn(rx, Duration::from_millis(1)); + tx.send_replace(RemoteCommand::new(Twist { vx: 3.0, ..Twist::ZERO })); + tokio::time::sleep(Duration::from_millis(200)).await; + tx.send_replace(RemoteCommand { enabled: false, ..RemoteCommand::STOP }); + tokio::time::sleep(Duration::from_millis(2)).await; + for mock in mocks { + assert_eq!(mock.last_command(), Command::Velocity(0.0)); + } + handle.abort(); + } } } @@ -771,11 +1112,11 @@ use std::process::ExitCode; use std::time::Duration; use clap::Parser; -use mecanum::Twist; use tokio::sync::watch; use tracing::{error, info}; use tracing_subscriber::EnvFilter; +use crate::remote::{Dr16CommandSource, RemoteMapper}; use crate::{ChassisController, wrap_wheels}; use can::CanLink; use chassis_config::Config; @@ -802,6 +1143,10 @@ struct Args { /// 只装配配置、链路、总线和控制任务，短跑后安全退出。 #[arg(long)] dry_run: bool, + + /// 覆盖配置文件中的 DR16 串口设备（如 `/dev/ttyS2`）。 + #[arg(long)] + rc_device: Option, } fn main() -> ExitCode { @@ -811,13 +1156,18 @@ fn main() -> ExitCode { let args = Args::parse(); - let config = match load_config(&args) { + let mut config = match load_config(&args) { Ok(config) => config, Err(e) => { error!("{e}"); return ExitCode::FAILURE; } }; + if let Some(device) = args.rc_device.clone() { + let mut remote = config.remote.take().unwrap_or_default(); + remote.device = device; + config.remote = Some(remote); + } let interface = args.interface.clone().unwrap_or_else(|| config.can.interface.clone()); let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() { @@ -856,6 +1206,25 @@ async fn run( dry_run: bool, ) -> Result<(), Box> { let bus_options = config.bus_options(); + let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega); + let remote_source = if mock { + info!("mock 模式不打开 DR16 串口，遥控输入保持失能"); + None + } else { + match config.remote.clone() { + Some(remote_config) => { + let samples = dr16::open(&remote_config.device)?; + let mapper = RemoteMapper::new(&remote_config, limits.0, limits.1, limits.2); + let source = Dr16CommandSource::new( + samples, + mapper, + Duration::from_millis(remote_config.timeout_ms), + ); + Some((source, remote_config.ramp_ms)) + } + None => None, + } + }; // 1) CAN 链路：仿真或真实 SocketCAN。 let link = if mock { @@ -884,10 +1253,8 @@ async fn run( "chassisd 启动" ); - // 3) 控制循环（当前无指令源：watch 保持 ZERO，安全静止； - // 遥控 / 自主导航后续经 chassis_config::source 接入）。 - let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); - let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega); + // 3) 控制循环与遥控指令源。 + let (command_tx, command_rx) = watch::channel(crate::RemoteCommand::DISABLED); let motors: [EscMotor; 4] = bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); let gears: [f32; 4] = config @@ -902,11 +1269,16 @@ async fn run( wrap_wheels(motors, gears), limits, Duration::from_millis(config.chassis.command_timeout_ms), + Duration::from_millis(remote_source.as_ref().map_or(500, |(_, ramp_ms)| *ramp_ms)), ) - .spawn(twist_rx, period); - let _twist_tx = twist_tx; // 保留发送端供后续指令源使用 + .spawn(command_rx, period); + let remote_task = remote_source + .map(|(source, _)| tokio::spawn(crate::source::pump_source(source, command_tx.clone()))); if dry_run { + if let Some(task) = remote_task { + task.abort(); + } tokio::time::sleep(Duration::from_millis(20)).await; for motor in bus.motors() { motor.disable(); @@ -927,6 +1299,9 @@ async fn run( } // 5) 停车：显式停用电机并等若干节拍把 0 电流帧刷入总线。 + if let Some(task) = remote_task { + task.abort(); + } for motor in bus.motors() { motor.disable(); } @@ -953,6 +1328,7 @@ fn open_real_link(interface: &str) -> Result mod daemon_integration { use super::*; use esc_bus::EscBusHandle; + use mecanum::Twist; use motor::MotorFeedback; use sim_harness::spawn_plant; use tokio::time::sleep; @@ -964,7 +1340,7 @@ mod daemon_integration { fn stack( config: &Config, command_timeout_ms: u64, - ) -> (EscBusHandle, watch::Sender) { + ) -> (EscBusHandle, watch::Sender) { let (bus_side, plant_side) = CanLink::pair(512); let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); spawn_plant(plant_side, &esc_ids, config.bus_options().period); @@ -976,7 +1352,7 @@ mod daemon_integration { let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options(), policy).unwrap(); - let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); + let (twist_tx, twist_rx) = watch::channel(RemoteCommand::DISABLED); let motors: [EscMotor; 4] = bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); let gears: [f32; 4] = config @@ -991,6 +1367,7 @@ mod daemon_integration { wrap_wheels(motors, gears), (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega), Duration::from_millis(command_timeout_ms), + Duration::from_millis(2), ); controller.spawn(twist_rx, config.bus_options().period); (bus, twist_tx) @@ -1025,7 +1402,7 @@ mod daemon_integration { let config = Config::mock_default(); let (bus, twist_tx) = stack(&config, 10_000); - twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + twist_tx.send_replace(RemoteCommand::new(Twist { vx: 1.0, ..Twist::ZERO })); let target = RPM_PER_MPS; let ok = wait_until(3000, || { wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target) @@ -1035,7 +1412,7 @@ mod daemon_integration { assert!(ok, "未收敛到 {target} rpm: {rpm:?}"); // 停车：指令归零后各轮减速到停。 - twist_tx.send_replace(Twist::ZERO); + twist_tx.send_replace(RemoteCommand::STOP); let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; let rpm = wheel_rpms(&bus); @@ -1048,7 +1425,7 @@ mod daemon_integration { let config = Config::mock_default(); let (bus, twist_tx) = stack(&config, 10_000); - twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO }); + twist_tx.send_replace(RemoteCommand::new(Twist { omega: 1.0, ..Twist::ZERO })); // a·ω = (0.2 + 0.25) m/s 的轮线速度。 let target = 0.45 * RPM_PER_MPS; let ok = wait_until(3000, || { @@ -1076,7 +1453,7 @@ mod daemon_integration { let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms); // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。 - twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + twist_tx.send_replace(RemoteCommand::new(Twist { vx: 1.0, ..Twist::ZERO })); let moving = wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)) .await; @@ -1096,11 +1473,11 @@ mod daemon_integration { config.chassis.max_vx = 1.0; // 限速 1 m/s let (bus, twist_tx) = stack(&config, 10_000); - twist_tx.send_replace(Twist { + twist_tx.send_replace(RemoteCommand::new(Twist { vx: 3.0, // 超限指令 vy: 2.0, omega: 0.0, - }); + })); // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy： // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。 let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS; diff --git a/crates/spin/Cargo.toml b/crates/application/tools/Cargo.toml similarity index 84% rename from crates/spin/Cargo.toml rename to crates/application/tools/Cargo.toml index 992aca4..6613ec2 100644 --- a/crates/spin/Cargo.toml +++ b/crates/application/tools/Cargo.toml @@ -16,6 +16,8 @@ esc-bus = { workspace = true } experiment = { workspace = true } motor-control = { workspace = true } mecanum = { workspace = true } +dr16-protocol = { workspace = true } +dr16 = { workspace = true } slcan = { workspace = true } socketcan = { workspace = true } tokio = { workspace = true } @@ -25,3 +27,6 @@ tracing-subscriber = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } + +[package.metadata.sentry-chassis] +layer = "application/tools" diff --git a/crates/spin/src/main.rs b/crates/application/tools/src/main.rs similarity index 89% rename from crates/spin/src/main.rs rename to crates/application/tools/src/main.rs index 51572e5..ca78ee4 100644 --- a/crates/spin/src/main.rs +++ b/crates/application/tools/src/main.rs @@ -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) -> ExitCode { + let args = Args::parse_from(std::iter::once(String::from("spin-rc")).chain(parameters)); + let stream = match open(&args.device) { + Ok(stream) => stream, + Err(error) => { + eprintln!("✗ 打开 DR16 串口 {} 失败：{error}", args.device); + return ExitCode::FAILURE; + } + }; + println!("=== DR16/D-BUS 只读监视（{}）===", args.device); + println!("不接触 CAN / 电机；ctrl-c 退出。满量程映射：vx=3.0 vy=3.0 omega=6.0（仅显示）\n"); + + let mut report = tokio::time::interval(Duration::from_millis(args.report_ms.max(1))); + report.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip); + loop { + tokio::select! { + _ = tokio::signal::ctrl_c() => { + println!("\n已退出"); + return ExitCode::SUCCESS; + } + _ = report.tick() => { + let Some(stamped) = stream.latest() else { + println!("等待 DR16 帧……"); + continue; + }; + let sample = stamped.sample; + let age = Instant::now().saturating_duration_since(stamped.received_at); + let vx = shaped(sample.channel_t(), args.deadband) * 3.0; + let vy = -shaped(sample.channel_a(), args.deadband) * 3.0; + let omega = -shaped(sample.channel_r(), args.deadband) * 6.0; + println!( + "age {:>4}ms | A {:>4} E {:>4} T {:>4} R {:>4} | S1 {:?}({}) S2 {:?}({}) | vx {:+.2} vy {:+.2} omega {:+.2}", + age.as_millis(), + sample.channel_a(), + sample.channel_e(), + sample.channel_t(), + sample.channel_r(), + sample.s1, + sample.s1.wire_value(), + sample.s2, + sample.s2.wire_value(), + vx, + vy, + omega, + ); + if sample.s1 != SwitchPosition::Up { + println!(" S1 非上档：底盘使能关闭"); + } + } + } + } + } +} + mod motor_one { //! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。 //! @@ -710,12 +797,13 @@ async fn main() -> std::process::ExitCode { motor_one::main_with_args(args.collect()).await; std::process::ExitCode::SUCCESS } + Some("rc") => rc_monitor::main_with_args(args.collect()).await, Some("--help" | "-h") => { - println!("usage: spin [options]"); + println!("usage: spin [options]"); std::process::ExitCode::SUCCESS } _ => { - eprintln!("usage: spin [options]"); + eprintln!("usage: spin [options]"); std::process::ExitCode::from(2) } } diff --git a/crates/chassis-tune/Cargo.toml b/crates/application/tuning/Cargo.toml similarity index 95% rename from crates/chassis-tune/Cargo.toml rename to crates/application/tuning/Cargo.toml index 3132835..d5ef0e2 100644 --- a/crates/chassis-tune/Cargo.toml +++ b/crates/application/tuning/Cargo.toml @@ -45,3 +45,6 @@ plotters = { workspace = true, optional = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "application/tuning" diff --git a/crates/chassis-tune/src/main.rs b/crates/application/tuning/src/main.rs similarity index 98% rename from crates/chassis-tune/src/main.rs rename to crates/application/tuning/src/main.rs index 5102e70..ab28382 100644 --- a/crates/chassis-tune/src/main.rs +++ b/crates/application/tuning/src/main.rs @@ -122,7 +122,7 @@ pub mod report { #[cfg(feature = "plot")] if let Some(trace) = &ctx.best_trace { let svg_path = ctx.out_dir.join("best.svg"); - crate::tune::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?; + crate::plot::write_step_svg(&svg_path, trace, &ctx.scenario)?; written.push(svg_path); } @@ -932,14 +932,15 @@ use esc_bus::EscConfig; use experiment::SetpointProfile; use indicatif::ProgressBar; use motor_control::VelocityGains; +use motor_model::Gm3508Model; use motor_protocol::EscModel; use serde::Serialize; use sim_harness::{LoopLimits, PlantEnsemble}; use tuning::{ AcceptanceSpec, BackendSpec, BenchEnvelope, BenchSpec, CostSpec, DEFAULT_PRESET_PATH, - Evaluator, Objective, PlantSpec, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, ScaleSpec, - ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict, parse_axis, - sweep, + Evaluator, Objective, PlantProfile, Preset, PresetFile, ProfileSpec, RangeSpec, Ranked, + ScaleSpec, ScenarioSpec, Score, SearchSpec, SimSpec, SolverSpec, SpaceSpec, Tuning, Verdict, + parse_axis, sweep, }; /// 子命令。 @@ -1121,7 +1122,7 @@ struct CommonArgs { /// 仿真后端参数（覆盖预设）。 #[derive(Debug, Clone, ClapArgs)] struct SimArgs { - /// 被控对象模型（内置默认 `gm3508-bench`） + /// 被控对象模型（内置默认 `bench`） #[arg(long, value_enum)] plant: Option, /// 集成成员数（域随机化；1 = 只用名义模型；内置默认 8） @@ -1268,16 +1269,16 @@ impl From for SolverSpec { #[derive(Debug, Clone, Copy, PartialEq, Eq, ValueEnum)] enum PlantArg { /// 理想一阶模型（无摩擦、无延迟） - Gm3508, + Ideal, /// 台架量级：静 / 库仑摩擦 + 2 拍延迟 - Gm3508Bench, + Bench, } -impl From for PlantSpec { +impl From for PlantProfile { fn from(value: PlantArg) -> Self { match value { - PlantArg::Gm3508 => Self::Gm3508, - PlantArg::Gm3508Bench => Self::Gm3508Bench, + PlantArg::Ideal => Self::Ideal, + PlantArg::Bench => Self::Bench, } } } @@ -1411,7 +1412,9 @@ impl CommonArgs { let source = if prefix.is_empty() { "内置默认".to_owned() } else { format!("{prefix}{name}") }; - let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{source}' 失败"))?; + let tuning = merged + .to_tuning::() + .with_context(|| format!("解算预设 '{source}' 失败"))?; let envelope = merged .bench_envelope() .with_context(|| format!("解算预设 '{source}' 的安全包络失败"))?; @@ -1922,7 +1925,8 @@ fn presets_cmd(args: &PresetsArgs) -> Result { println!("共 {} 个预设", file.preset.len()); for (name, preset) in &file.preset { let merged = Preset::builtin().overlay(preset); - let tuning = merged.to_tuning().with_context(|| format!("解算预设 '{name}' 失败"))?; + let tuning = + merged.to_tuning::().with_context(|| format!("解算预设 '{name}' 失败"))?; let envelope = merged.bench_envelope().with_context(|| format!("解算预设 '{name}' 的安全包络失败"))?; let mark = @@ -2369,7 +2373,6 @@ fn open_bench( let mut esc = EscConfig::gm3508(hardware.name.clone(), hardware.esc_id, hardware.direction); esc.gear_ratio = hardware.gear_ratio; esc.current_limit = limits.current_limit_raw; - esc.model = limits.model; let envelope = SafetyEnvelope::new() .with_overspeed(envelope_spec.max_rpm, Duration::from_millis(60)) @@ -2452,7 +2455,14 @@ feedforward = 0.0 /// 仓库里随代码一起评审的预设文件。 fn repo_preset_path() -> PathBuf { - Path::new(env!("CARGO_MANIFEST_DIR")).join("../../config/tune.toml") + let mut directory = Path::new(env!("CARGO_MANIFEST_DIR")); + loop { + let candidate = directory.join("config/tune.toml"); + if candidate.exists() { + return candidate; + } + directory = directory.parent().expect("workspace root must exist"); + } } fn args_with(extra: &[&str]) -> CommonArgs { @@ -2573,7 +2583,7 @@ feedforward = 0.0 assert_eq!(tuning.search.seed, 20_260_101); assert!((tuning.scenario.target_rpm - 1500.0).abs() < 1e-6); assert!((tuning.acceptance.max_overshoot_pct - 20.0).abs() < 1e-6); - assert_eq!(tuning.plant_spec, PlantSpec::Gm3508Bench); + assert_eq!(tuning.plant_spec, PlantProfile::Bench); assert_eq!(settings.envelope.validate_runs, 3); } @@ -2722,7 +2732,7 @@ samples = 99 for (name, preset) in &file.preset { let merged = Preset::builtin().overlay(preset); let tuning = merged - .to_tuning() + .to_tuning::() .unwrap_or_else(|error| panic!("预设 '{name}' 无法解算：{error}")); merged .bench_envelope() diff --git a/crates/chassis-config/Cargo.toml b/crates/config/chassis/Cargo.toml similarity index 85% rename from crates/chassis-config/Cargo.toml rename to crates/config/chassis/Cargo.toml index 43c1162..bdbc14a 100644 --- a/crates/chassis-config/Cargo.toml +++ b/crates/config/chassis/Cargo.toml @@ -10,6 +10,7 @@ license.workspace = true workspace = true [dependencies] +motor = { workspace = true } motor-control = { workspace = true } mecanum = { workspace = true } esc-bus = { workspace = true } @@ -20,3 +21,6 @@ thiserror = { workspace = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "config/chassis" diff --git a/crates/chassis-config/src/lib.rs b/crates/config/chassis/src/lib.rs similarity index 84% rename from crates/chassis-config/src/lib.rs rename to crates/config/chassis/src/lib.rs index 0f49b63..40139df 100644 --- a/crates/chassis-config/src/lib.rs +++ b/crates/config/chassis/src/lib.rs @@ -12,6 +12,7 @@ use std::time::Duration; use esc_bus::{BusOptions, EscConfig}; use mecanum::Mecanum; +use motor::Gm3508; use motor_control::VelocityGains; use motor_protocol::EscModel; use serde::{Deserialize, Serialize}; @@ -37,6 +38,9 @@ pub struct Config { /// 底盘控制参数。 #[serde(default)] pub chassis: ChassisConfig, + /// 可选遥控输入；缺省时守护进程保持安全静止。 + #[serde(default)] + pub remote: Option, /// 麦轮几何。 pub mecanum: MecanumConfig, /// 四电机配置，顺序固定：前左、前右、后左、后右。 @@ -128,6 +132,69 @@ impl Default for ChassisConfig { } } +/// 遥控输入类型。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize, Serialize)] +#[serde(rename_all = "lowercase")] +pub enum RemoteKind { + /// DJI DR16 接收机的 D-BUS 串口。 + Dr16, +} + +/// 遥控输入配置。 +#[derive(Debug, Clone, PartialEq, Deserialize, Serialize)] +#[serde(deny_unknown_fields)] +pub struct RemoteConfig { + /// 接收机协议。 + pub kind: RemoteKind, + /// T153 UART 设备节点。 + pub device: String, + /// 合法遥控帧断流超时（毫秒）。 + #[serde(default = "default_remote_timeout")] + pub timeout_ms: u64, + /// 摇杆死区，按满量程比例计。 + #[serde(default = "default_remote_deadband")] + pub deadband: f32, + /// 从零到满量程的速度斜坡时间（毫秒）。 + #[serde(default = "default_remote_ramp")] + pub ramp_ms: u64, + /// 反转前后方向。 + #[serde(default)] + pub invert_vx: bool, + /// 反转左右方向。 + #[serde(default)] + pub invert_vy: bool, + /// 反转旋转方向。 + #[serde(default)] + pub invert_omega: bool, +} + +impl Default for RemoteConfig { + fn default() -> Self { + Self { + kind: RemoteKind::Dr16, + device: "/dev/ttyS2".into(), + timeout_ms: default_remote_timeout(), + deadband: default_remote_deadband(), + ramp_ms: default_remote_ramp(), + invert_vx: false, + invert_vy: false, + invert_omega: false, + } + } +} + +fn default_remote_timeout() -> u64 { + 100 +} + +fn default_remote_deadband() -> f32 { + 0.05 +} + +fn default_remote_ramp() -> u64 { + 500 +} + /// 麦轮几何（米）。 #[derive(Debug, Clone, Copy, PartialEq, Deserialize, Serialize)] #[serde(deny_unknown_fields)] @@ -341,6 +408,23 @@ impl Config { if !(c.max_vx > 0.0 && c.max_vy > 0.0 && c.max_omega > 0.0) { return Err(ConfigError::Invalid("speed limits must be > 0".into())); } + if let Some(remote) = &self.remote { + if remote.device.trim().is_empty() { + return Err(ConfigError::Invalid("remote.device must not be empty".into())); + } + if remote.timeout_ms == 0 || remote.ramp_ms == 0 { + return Err(ConfigError::Invalid("remote timeout/ramp must be > 0 ms".into())); + } + if !remote.deadband.is_finite() || !(0.0..0.5).contains(&remote.deadband) { + return Err(ConfigError::Invalid("remote.deadband must be in [0, 0.5)".into())); + } + if remote.timeout_ms > c.command_timeout_ms { + return Err(ConfigError::Invalid(format!( + "remote.timeout_ms ({}) must be <= command_timeout_ms ({})", + remote.timeout_ms, c.command_timeout_ms + ))); + } + } for motor in &self.motors { let gear_ratio_valid = motor.gear_ratio > 0.0; if !gear_ratio_valid { @@ -390,6 +474,7 @@ impl Config { Self { can: CanConfig { interface: "vcan0".into(), function: None }, chassis: ChassisConfig::default(), + remote: None, mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 }, motors: vec![ motor("front_left", 1, 1.0), @@ -402,16 +487,14 @@ impl Config { /// 转换为 `EscBus` 配置（顺序 FL、FR、RL、RR）。 #[must_use] - pub fn to_esc_configs(&self) -> Vec { + pub fn to_esc_configs(&self) -> Vec> { self.motors .iter() - .map(|m| EscConfig { - name: m.name.clone(), - esc_id: m.esc_id, - model: m.model.into(), - gear_ratio: m.gear_ratio, - direction: m.direction, - current_limit: m.current_limit, + .map(|m| { + let mut cfg = EscConfig::::gm3508(m.name.clone(), m.esc_id, m.direction); + cfg.gear_ratio = m.gear_ratio; + cfg.current_limit = m.current_limit; + cfg }) .collect() } @@ -441,6 +524,16 @@ mod tests { [can] interface = "can0" +[remote] +kind = "dr16" +device = "/dev/ttyS2" +timeout_ms = 100 +deadband = 0.05 +ramp_ms = 500 +invert_vx = false +invert_vy = false +invert_omega = false + [chassis] control_period_ms = 1 command_timeout_ms = 200 @@ -497,6 +590,10 @@ feedforward = 9.0 fn parses_example() { let cfg = Config::from_toml(EXAMPLE).unwrap(); assert_eq!(cfg.can.interface, "can0"); + let remote = cfg.remote.as_ref().unwrap(); + assert_eq!(remote.kind, RemoteKind::Dr16); + assert_eq!(remote.device, "/dev/ttyS2"); + assert_eq!(remote.timeout_ms, 100); assert_eq!(cfg.motors.len(), 4); assert_eq!(cfg.motors[0].name, "front_left"); assert_eq!(cfg.motors[3].esc_id, 4); @@ -536,6 +633,7 @@ gear_ratio = 19.2 ) .unwrap(); assert_eq!(cfg.chassis, ChassisConfig::default()); + assert!(cfg.remote.is_none()); assert_eq!(cfg.motors[0].direction.to_bits(), 1.0f32.to_bits()); assert_eq!(cfg.motors[0].current_limit, 12_000); assert_eq!(cfg.motors[0].model, EscModelSerde::C620); @@ -611,6 +709,7 @@ gear_ratio = 19.2 let reparsed = Config::from_toml(&text).unwrap(); assert_eq!(reparsed, original); // 落盘文本必须仍然是合法配置（守护进程直接读它）。 + assert!(text.contains("[remote]"), "{text}"); assert!(text.contains("[[motor]]"), "{text}"); assert!(text.contains("interface = \"can0\""), "{text}"); } @@ -620,9 +719,11 @@ gear_ratio = 19.2 let mut original = Config::mock_default(); original.can.function = Some(FunctionBusConfig { interface: "can1".into() }); original.chassis.motor_command_timeout_ms = 37; + original.remote = Some(RemoteConfig { invert_omega: true, ..RemoteConfig::default() }); let reparsed = Config::from_toml(&original.to_toml_string().unwrap()).unwrap(); assert_eq!(reparsed, original); assert_eq!(reparsed.chassis.motor_command_timeout_ms, 37); + assert!(reparsed.remote.unwrap().invert_omega); assert_eq!(reparsed.can.function.unwrap().interface, "can1"); } @@ -666,6 +767,19 @@ gear_ratio = 19.2 )); } + #[test] + fn remote_validation_rejects_unsafe_values() { + let mut cfg = Config::mock_default(); + cfg.remote = Some(RemoteConfig { timeout_ms: 0, ..RemoteConfig::default() }); + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + + cfg.remote = Some(RemoteConfig { deadband: 0.5, ..RemoteConfig::default() }); + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + + cfg.remote = Some(RemoteConfig { timeout_ms: 201, ..RemoteConfig::default() }); + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + } + #[test] fn save_and_load_file_round_trip() { let dir = std::env::temp_dir().join("chassis-config-roundtrip"); diff --git a/crates/can/Cargo.toml b/crates/contract/can/Cargo.toml similarity index 86% rename from crates/can/Cargo.toml rename to crates/contract/can/Cargo.toml index 39338fd..711371b 100644 --- a/crates/can/Cargo.toml +++ b/crates/contract/can/Cargo.toml @@ -17,3 +17,6 @@ tokio = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } + +[package.metadata.sentry-chassis] +layer = "contract/can" diff --git a/crates/can/src/lib.rs b/crates/contract/can/src/lib.rs similarity index 100% rename from crates/can/src/lib.rs rename to crates/contract/can/src/lib.rs diff --git a/crates/imu/Cargo.toml b/crates/contract/imu/Cargo.toml similarity index 77% rename from crates/imu/Cargo.toml rename to crates/contract/imu/Cargo.toml index 96e2469..1c80db9 100644 --- a/crates/imu/Cargo.toml +++ b/crates/contract/imu/Cargo.toml @@ -11,3 +11,6 @@ workspace = true [dependencies] + +[package.metadata.sentry-chassis] +layer = "contract/imu" diff --git a/crates/imu/src/lib.rs b/crates/contract/imu/src/lib.rs similarity index 100% rename from crates/imu/src/lib.rs rename to crates/contract/imu/src/lib.rs diff --git a/crates/pid/Cargo.toml b/crates/control/algorithm/Cargo.toml similarity index 80% rename from crates/pid/Cargo.toml rename to crates/control/algorithm/Cargo.toml index 5be517d..bc6245f 100644 --- a/crates/pid/Cargo.toml +++ b/crates/control/algorithm/Cargo.toml @@ -14,3 +14,6 @@ workspace = true [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "control/algorithm" diff --git a/crates/pid/src/lib.rs b/crates/control/algorithm/src/lib.rs similarity index 100% rename from crates/pid/src/lib.rs rename to crates/control/algorithm/src/lib.rs diff --git a/crates/mecanum/Cargo.toml b/crates/control/chassis/Cargo.toml similarity index 83% rename from crates/mecanum/Cargo.toml rename to crates/control/chassis/Cargo.toml index ee44335..831a8fd 100644 --- a/crates/mecanum/Cargo.toml +++ b/crates/control/chassis/Cargo.toml @@ -15,3 +15,6 @@ workspace = true [dev-dependencies] proptest = { workspace = true } approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "control/chassis" diff --git a/crates/mecanum/src/lib.rs b/crates/control/chassis/src/lib.rs similarity index 100% rename from crates/mecanum/src/lib.rs rename to crates/control/chassis/src/lib.rs diff --git a/crates/motor-control/Cargo.toml b/crates/control/motor/Cargo.toml similarity index 85% rename from crates/motor-control/Cargo.toml rename to crates/control/motor/Cargo.toml index 8023e1d..e8c46d5 100644 --- a/crates/motor-control/Cargo.toml +++ b/crates/control/motor/Cargo.toml @@ -15,3 +15,6 @@ motor = { workspace = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "control/motor" diff --git a/crates/motor-control/src/lib.rs b/crates/control/motor/src/lib.rs similarity index 100% rename from crates/motor-control/src/lib.rs rename to crates/control/motor/src/lib.rs diff --git a/crates/esc-bus/Cargo.toml b/crates/device/esc/Cargo.toml similarity index 79% rename from crates/esc-bus/Cargo.toml rename to crates/device/esc/Cargo.toml index 6529726..8ccfd34 100644 --- a/crates/esc-bus/Cargo.toml +++ b/crates/device/esc/Cargo.toml @@ -18,9 +18,8 @@ thiserror = { workspace = true } tracing = { workspace = true } [dev-dependencies] -sim-harness = { workspace = true } -motor-control = { workspace = true } -motor-model = { workspace = true } -experiment = { workspace = true } tokio = { workspace = true, features = ["test-util"] } approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "device/esc" diff --git a/crates/esc-bus/src/lib.rs b/crates/device/esc/src/lib.rs similarity index 54% rename from crates/esc-bus/src/lib.rs rename to crates/device/esc/src/lib.rs index 1110a2b..1095768 100644 --- a/crates/esc-bus/src/lib.rs +++ b/crates/device/esc/src/lib.rs @@ -1,132 +1,96 @@ #![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] -//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。 -//! -//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务： -//! -//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）； -//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发 -//! effort），按 `0x200`/`0x1FF` 分组组帧发送。 -//! -//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。 -//! 命令模式切换时同样复位控制器，避免积分冲击。 +//! Generic asynchronous ESC frame pump, state, leases, and safety. +use std::marker::PhantomData; use std::sync::{Arc, Mutex, MutexGuard}; use std::time::Duration; use can::{CanFrame, CanLink}; -use motor_protocol::common::{ - EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame, +use motor::{ + Command, CurrentEscDrive, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorSpec, + MotorState, PolicyInput, PolicySample, PositionDrive, VelocityDrive, }; -use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker}; +use motor_protocol::{EffortScale, FeedbackFields, MotorProtocol, TurnTracker}; use thiserror::Error; use tokio::sync::{mpsc, watch}; use tokio::task::JoinHandle; use tokio::time::{Instant, Interval, MissedTickBehavior}; use tracing::{debug, info, warn}; -use motor::{ - Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput, - PolicySample, PositionDrive, VelocityDrive, -}; - -/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌， -/// 状态本身仍一致，驱动机不应因此停摆）。 fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) } -/// 单台电机的配置。 +/// Configuration for one typed motor. #[derive(Debug, Clone)] -pub struct EscConfig { - /// 便于日志辨认的名字（如 `front_left`）。 +pub struct EscConfig { pub name: String, - /// 电调 ID（1–8，即 CAN 拨码）。 pub esc_id: u8, - /// 电调型号。 - pub model: EscModel, - /// 减速比（电机转子 : 输出轴）。 - /// - /// 驱动层**不使用**此字段；由上层 `crate::OutputShaft` 应用。 - /// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。 pub gear_ratio: f32, - /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转， - /// 上层永远以"输出轴正转 = 前进方向"工作。 pub direction: f32, - /// effort 限幅（raw，`1..=型号满量程`）。 pub current_limit: i16, + marker: PhantomData, } -impl EscConfig { - /// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值，需按整车整定。 +impl EscConfig { #[must_use] - pub fn gm3508(name: impl Into, esc_id: u8, direction: f32) -> Self { + pub fn new(name: impl Into, esc_id: u8, direction: f32) -> Self { Self { name: name.into(), esc_id, - model: EscModel::C620, - // 3591 / 187 ≈ 19.203 - gear_ratio: 3591.0 / 187.0, + gear_ratio: S::GEAR_RATIO, direction, - current_limit: 12_000, - } - } - - /// M2006 + C610（拨弹轮）的常用配置。 - /// - /// 减速比按 P36 起步（36.0），接硬件前需核对；电流标度见 [`EscModel::C610`]。 - #[must_use] - pub fn m2006(name: impl Into, esc_id: u8, direction: f32) -> Self { - Self { - name: name.into(), - esc_id, - model: EscModel::C610, - // M2006 P36 减速比（接入前核对） - gear_ratio: 36.0, - direction, - current_limit: 8_000, + current_limit: S::scale().max_raw() / 4 * 3, + marker: PhantomData, } } fn validate(&self) -> Result<(), String> { - if command_frame_id(self.esc_id).is_none() { + if ::command_frame_id(self.esc_id).is_none() { return Err(format!("esc_id {} out of range 1..=8", self.esc_id)); } if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) { return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio)); } - // 逐位比较：direction 只允许精确的 ±1.0。 let dir_bits = self.direction.to_bits(); if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() { return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction)); } - if !(1..=self.model.max_current_raw()).contains(&self.current_limit) { + if !(1..=S::scale().max_raw()).contains(&self.current_limit) { return Err(format!( - "current_limit {} out of range 1..={} for {:?}", + "current_limit {} out of range 1..={}", self.current_limit, - self.model.max_current_raw(), - self.model + S::scale().max_raw() )); } Ok(()) } } -/// 总线运行参数。 +impl EscConfig { + #[must_use] + pub fn gm3508(name: impl Into, esc_id: u8, direction: f32) -> Self { + let mut cfg = Self::new(name, esc_id, direction); + cfg.current_limit = 12_000; + cfg + } +} + +impl EscConfig { + #[must_use] + pub fn m2006(name: impl Into, esc_id: u8, direction: f32) -> Self { + let mut cfg = Self::new(name, esc_id, direction); + cfg.current_limit = 8_000; + cfg + } +} + +/// Bus timing and lease options. #[derive(Debug, Clone)] pub struct BusOptions { - /// 控制节拍（C620 期望 1kHz 命令）。 pub period: Duration, - /// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。 pub feedback_timeout: Duration, - /// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。 - /// - /// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` / - /// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发 - /// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可）， - /// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。 - /// - /// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。 pub command_timeout: Duration, } @@ -135,45 +99,34 @@ impl Default for BusOptions { Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100), - // 1kHz 下约 50 拍未刷新即失效。 command_timeout: Duration::from_millis(50), } } } -/// 启动错误。 +/// Startup error. #[derive(Debug, Clone, PartialEq, Eq, Error)] pub enum EscBusError { - /// 两台电机配置了同一电调 ID。 #[error("duplicate ESC id {0}")] DuplicateEscId(u8), - /// 电机配置非法（原因见 `reason`）。 #[error("invalid config for motor {name}: {reason}")] - InvalidConfig { - /// 电机名。 - name: String, - /// 非法原因。 - reason: String, - }, - /// 一台电机都没有配置。 + InvalidConfig { name: String, reason: String }, #[error("no motors configured")] NoMotors, } -/// 单电机共享状态（驱动任务与 [`EscMotor`] 句柄之间）。 -struct MotorShared { - cfg: EscConfig, +struct MotorShared { + cfg: EscConfig, state: Mutex, cmd: Mutex, - /// 命令写入时刻（看门狗用）。 cmd_at: Mutex>, turn: Mutex, last_feedback: Mutex>, offline_warned: std::sync::atomic::AtomicBool, } -impl MotorShared { - fn new(cfg: EscConfig) -> Self { +impl MotorShared { + fn new(cfg: EscConfig) -> Self { Self { cfg, state: Mutex::new(MotorState::default()), @@ -190,24 +143,22 @@ impl MotorShared { *lock(&self.cmd_at) = Some(Instant::now()); } - /// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。 - fn apply_feedback(&self, fb: EscFeedback, now: Instant) { - let esc_rev = lock(&self.turn).update(fb.angle_raw); + fn apply_feedback(&self, fb: &F, now: Instant) { + let esc_rev = lock(&self.turn).update(fb.angle_raw()); let dir = self.cfg.direction; { let mut st = lock(&self.state); - st.rotor_angle_raw = fb.angle_raw; + st.rotor_angle_raw = fb.angle_raw(); st.rotor_revolutions = dir * esc_rev; - st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm); - st.effort_raw = (dir * f32::from(fb.current_raw)) as i16; - st.temperature_c = fb.temperature_c; + st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm()); + st.effort_raw = (dir * f32::from(fb.current_raw())) as i16; + st.temperature_c = fb.temperature_c(); st.feedback_count = st.feedback_count.saturating_add(1); } *lock(&self.last_feedback) = Some(now); self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed); } - /// Produce one policy sample while enforcing feedback and command leases. fn sample( &self, feedback_timeout: Duration, @@ -219,7 +170,7 @@ impl MotorShared { Some(t) if now.saturating_duration_since(t) <= feedback_timeout ); if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) { - warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current"); + warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero output"); } let fresh = matches!( *lock(&self.cmd_at), @@ -230,103 +181,87 @@ impl MotorShared { } } -/// 电机的公共句柄：可克隆，跨任务共享。 -/// -/// # 命令租约 -/// -/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] / -/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。 -/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出）， -/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。 -#[derive(Clone)] -pub struct EscMotor { - shared: Arc, +/// Typed motor handle. +pub struct EscMotor { + shared: Arc>, } -impl EscMotor { - /// 电机名。 +impl Clone for EscMotor { + fn clone(&self) -> Self { + Self { shared: Arc::clone(&self.shared) } + } +} + +impl EscMotor { + #[must_use] + pub fn config(&self) -> &EscConfig { + &self.shared.cfg + } #[must_use] pub fn name(&self) -> &str { &self.shared.cfg.name } - - /// 电机配置。 - #[must_use] - pub fn config(&self) -> &EscConfig { - &self.shared.cfg - } - - /// 当前命令。 #[must_use] pub fn command(&self) -> Command { *lock(&self.shared.cmd) } - - /// 最近一帧反馈的时刻（诊断用）。 #[must_use] pub fn last_feedback(&self) -> Option { *lock(&self.shared.last_feedback) } } -impl MotorFeedback for EscMotor { +impl MotorFeedback for EscMotor { fn id(&self) -> u8 { self.shared.cfg.esc_id } - fn state(&self) -> MotorState { *lock(&self.shared.state) } - - fn feedback_age(&self) -> Option { - let t = *lock(&self.shared.last_feedback); - Some(Instant::now().saturating_duration_since(t?)) + fn feedback_age(&self) -> Option { + Some(Instant::now().saturating_duration_since(*lock(&self.shared.last_feedback).as_ref()?)) } } -impl VelocityDrive for EscMotor { - /// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。 +impl VelocityDrive for EscMotor { fn set_velocity(&self, rotor_rpm: f32) { self.shared.store_command(Command::Velocity(rotor_rpm)); } } -impl EffortDrive for EscMotor { - /// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。 +impl EffortDrive for EscMotor { fn set_effort_raw(&self, raw: i16) { self.shared.store_command(Command::EffortRaw(raw)); } } -impl PositionDrive for EscMotor { - /// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。 +impl PositionDrive for EscMotor { fn set_position(&self, rotor_rev: f32) { self.shared.store_command(Command::Position(rotor_rev)); } } -impl Disable for EscMotor { - /// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。 +impl Disable for EscMotor { fn disable(&self) { self.shared.store_command(Command::Disabled); } } -/// 运行中的电调总线。 -pub struct EscBus; +/// Generic current-frame ESC bus. +pub struct EscBus(PhantomData); -impl EscBus { - /// 启动收发任务，返回句柄。 +impl EscBus { + /// Start receive and transmit tasks. /// /// # Errors /// - /// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。 + /// Returns an error when configuration is empty, duplicated, or invalid. pub fn spawn( link: CanLink, - configs: Vec, + configs: Vec>, options: BusOptions, policy: P, - ) -> Result, EscBusError> { + ) -> Result, EscBusError> { if configs.is_empty() { return Err(EscBusError::NoMotors); } @@ -346,46 +281,48 @@ impl EscBus { let motors = Arc::new(motors); let policy = Arc::new(Mutex::new(policy)); let rx_handle = - tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone())); - let tx_handle = tokio::spawn(tx_loop( + tokio::spawn(rx_loop::(incoming_rx, Arc::clone(&motors), shutdown_rx.clone())); + let tx_handle = tokio::spawn(tx_loop::( outgoing_tx, Arc::clone(&motors), Arc::clone(&policy), options, shutdown_rx, )); - - Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] }) + Ok(EscBusHandle { + motors, + policy, + shutdown_tx, + tasks: vec![rx_handle, tx_handle], + marker: PhantomData, + }) } } -/// `EscBus` 的控制句柄。 -pub struct EscBusHandle

{ - motors: Arc>, +/// Control handle for a running bus. +pub struct EscBusHandle { + motors: Arc>>, policy: Arc>, shutdown_tx: watch::Sender, tasks: Vec>, + marker: PhantomData, } -impl EscBusHandle

{ - /// 按电调 ID 取电机句柄。 +impl EscBusHandle { #[must_use] - pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> { + pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> { self.motors.iter().find(|m| m.id() == esc_id) } - /// 全部电机（按配置顺序）。 #[must_use] - pub fn motors(&self) -> &[EscMotor] { + pub fn motors(&self) -> &[EscMotor] { &self.motors } - /// Mutate the composed control policy between bus ticks. pub fn with_policy(&self, update: impl FnOnce(&mut P) -> R) -> R { update(&mut lock(&self.policy)) } - /// 停止收发任务并等待退出。 pub async fn shutdown(self) { let _ = self.shutdown_tx.send(true); for task in self.tasks { @@ -394,9 +331,9 @@ impl EscBusHandle

{ } } -async fn rx_loop( +async fn rx_loop( mut incoming: mpsc::Receiver, - motors: Arc>, + motors: Arc>>, mut shutdown: watch::Receiver, ) { loop { @@ -407,22 +344,20 @@ async fn rx_loop( None => break, }, }; - match decode_feedback(&frame) { + match ::decode_feedback(&frame) { Ok((esc_id, fb)) => { if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) { - motor.shared.apply_feedback(fb, Instant::now()); + motor.shared.apply_feedback(&fb, Instant::now()); } } - Err(e) => { - debug!(error = %e, id = frame.id, "ignore non-feedback frame"); - } + Err(e) => debug!(error = %e, id = frame.id, "ignore non-feedback frame"), } } } -async fn tx_loop( +async fn tx_loop( outgoing: mpsc::Sender, - motors: Arc>, + motors: Arc>>, policy: Arc>, options: BusOptions, mut shutdown: watch::Receiver, @@ -453,28 +388,29 @@ async fn tx_loop( } } let efforts = lock(&policy).step(&PolicyInput { samples, dt }); - for (frame_id, group_start) in - [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)] - { - let group: Vec<&EscMotor> = motors + for group_start in [1u8, 5u8] { + let Some(frame_id) = ::command_frame_id(group_start) + else { + continue; + }; + let group: Vec<&EscMotor> = motors .iter() .filter(|m| (group_start..group_start + 4).contains(&m.id())) .collect(); if group.is_empty() { continue; } - let mut currents = [0i16; 4]; + let mut commands = [S::Protocol::command_from_raw(0); 4]; for motor in &group { let slot = usize::from(motor.id() - group_start); let policy_index = usize::from(motor.id().saturating_sub(1)); let physical = efforts.get(policy_index).copied().unwrap_or(0); let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16; - currents[slot] = clamp_current( - motor.shared.cfg.model, - directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit), - ); + let limited = + directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit); + commands[slot] = S::Protocol::command_from_raw(S::scale().clamp_raw(limited)); } - if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() { + if outgoing.send(S::Protocol::encode_group(frame_id, commands)).await.is_err() { info!("can link closed, tx loop exiting"); return; } @@ -484,7 +420,6 @@ async fn tx_loop( fn tick_every(period: Duration) -> Interval { let mut ticker = tokio::time::interval(period); - // 错过节拍后直接跳到当前节拍，避免补发帧风暴。 ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); ticker } @@ -492,49 +427,35 @@ fn tick_every(period: Duration) -> Interval { #[cfg(test)] mod tests { use super::*; + use motor::{Gm3508, M2006}; #[test] - fn config_validate_rejects_bad_values() { - let mut cfg = EscConfig::gm3508("m", 1, 1.0); - - cfg.esc_id = 0; - assert!(cfg.validate().is_err()); - cfg.esc_id = 9; - assert!(cfg.validate().is_err()); - cfg.esc_id = 5; - - cfg.gear_ratio = 0.0; - assert!(cfg.validate().is_err()); - cfg.gear_ratio = 19.2; - - cfg.direction = 0.5; - assert!(cfg.validate().is_err()); - cfg.direction = -1.0; - - cfg.current_limit = 0; - assert!(cfg.validate().is_err()); - cfg.current_limit = 16385; - assert!(cfg.validate().is_err()); - cfg.current_limit = 12_000; - - assert!(cfg.validate().is_ok()); - } - - #[test] - fn gm3508_defaults_are_sane() { - let cfg = EscConfig::gm3508("front_left", 1, -1.0); - assert_eq!(cfg.model, EscModel::C620); + fn typed_defaults_are_sane() { + let cfg = EscConfig::::gm3508("front_left", 1, -1.0); assert!((cfg.gear_ratio - 19.2).abs() < 0.01); assert_eq!(cfg.current_limit, 12_000); - assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits()); assert!(cfg.validate().is_ok()); - } - #[test] - fn m2006_defaults_are_sane() { - let cfg = EscConfig::m2006("feeder", 5, 1.0); - assert_eq!(cfg.model, EscModel::C610); + let cfg = EscConfig::::m2006("feeder", 5, 1.0); assert!((cfg.gear_ratio - 36.0).abs() < 0.01); assert!(cfg.validate().is_ok()); } + + #[test] + fn config_validate_rejects_bad_values() { + let mut cfg = EscConfig::::gm3508("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()); + } } diff --git a/crates/delta10/Cargo.toml b/crates/device/imu/Cargo.toml similarity index 86% rename from crates/delta10/Cargo.toml rename to crates/device/imu/Cargo.toml index fd8a90a..e7f68ff 100644 --- a/crates/delta10/Cargo.toml +++ b/crates/device/imu/Cargo.toml @@ -15,3 +15,6 @@ imu = { workspace = true } delta10-protocol = { workspace = true } tokio = { workspace = true } thiserror = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "device/imu" diff --git a/crates/delta10/src/lib.rs b/crates/device/imu/src/lib.rs similarity index 100% rename from crates/delta10/src/lib.rs rename to crates/device/imu/src/lib.rs diff --git a/crates/motor/Cargo.toml b/crates/device/motor/Cargo.toml similarity index 87% rename from crates/motor/Cargo.toml rename to crates/device/motor/Cargo.toml index a9e2a6f..5d94b66 100644 --- a/crates/motor/Cargo.toml +++ b/crates/device/motor/Cargo.toml @@ -16,3 +16,6 @@ thiserror = { workspace = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "device/motor" diff --git a/crates/motor/src/lib.rs b/crates/device/motor/src/lib.rs similarity index 81% rename from crates/motor/src/lib.rs rename to crates/device/motor/src/lib.rs index 539acac..e76870a 100644 --- a/crates/motor/src/lib.rs +++ b/crates/device/motor/src/lib.rs @@ -792,173 +792,121 @@ pub mod testing { } } -pub mod motor_common { - //! 电机种类：把"型号"变成可在设备层与配置层之间传递的事实。 - //! - //! 设备层不关心电调怎么接、齿轮多大，但必须知道**协议族**：电流环在电调内部， - //! 而不同协议族的控制字含义不同（电流 vs 电压）。[`MotorKind`] 就是这张表。 - - use motor_protocol::EscModel; - - /// 支持的电机型号。 - #[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] - pub enum MotorKind { - /// GM3508 + C620（底盘驱动轮）。 - Gm3508, - /// M2006 + C610（拨弹 / 小负载）。 - M2006, - /// GM6020（云台直驱，**电压**控制）。 - Gm6020, - } - - impl MotorKind { - /// 配套电调型号。 - #[must_use] - pub const fn esc_model(self) -> EscModel { - match self { - // GM6020 的反馈标度与 C620 相同，但控制字是电压。 - Self::Gm3508 | Self::Gm6020 => EscModel::C620, - Self::M2006 => EscModel::C610, - } - } - - /// 控制量是否是**电压**（而非电流）。 - /// - /// 总线驱动据此选择组帧方式：C620/C610 走 `encode_current_frame`， - /// GM6020 走 `motor_protocol::motor::gm6020::encode_voltage_frame`。 - #[must_use] - pub const fn encodes_voltage(self) -> bool { - matches!(self, Self::Gm6020) - } - - /// 该型号是否与其它型号**共用**电流控制帧（C620/C610 共用 `0x200`/`0x1FF`）。 - #[must_use] - pub const fn shares_current_frames(self) -> bool { - !self.encodes_voltage() - } - - /// 默认减速比（转子 : 输出轴）。接硬件前必须按实车核对。 - #[must_use] - pub const fn default_gear_ratio(self) -> f32 { - match self { - Self::Gm3508 => 3591.0 / 187.0, - // M2006 常见配置 36:1；与 ARCHITECTURE.md §8.2 的 motors.toml 一致。 - Self::M2006 => 36.0, - // 云台直驱。 - Self::Gm6020 => 1.0, - } - } - - /// 型号名（配置与日志用）。 - #[must_use] - pub const fn name(self) -> &'static str { - match self { - Self::Gm3508 => "GM3508", - Self::M2006 => "M2006", - Self::Gm6020 => "GM6020", - } - } - } - - #[cfg(test)] - mod tests { - use super::*; - - #[test] - fn only_gm6020_encodes_voltage() { - assert!(MotorKind::Gm6020.encodes_voltage()); - assert!(!MotorKind::Gm3508.encodes_voltage()); - assert!(!MotorKind::M2006.encodes_voltage()); - assert!(MotorKind::Gm3508.shares_current_frames()); - assert!(!MotorKind::Gm6020.shares_current_frames()); - } - - #[test] - fn esc_models_match_the_scale_documentation() { - assert_eq!(MotorKind::Gm3508.esc_model(), EscModel::C620); - assert_eq!(MotorKind::Gm6020.esc_model(), EscModel::C620); - assert_eq!(MotorKind::M2006.esc_model(), EscModel::C610); - assert_eq!(MotorKind::M2006.esc_model().max_current_raw(), 10_000); - } - - #[test] - fn default_gear_ratios_are_sane() { - assert!((MotorKind::Gm3508.default_gear_ratio() - 19.203).abs() < 5e-3); - assert!((MotorKind::Gm6020.default_gear_ratio() - 1.0).abs() < 1e-6); - assert!(MotorKind::M2006.default_gear_ratio() > 0.0); - assert_eq!(MotorKind::Gm6020.name(), "GM6020"); - } - } -} - -pub mod motor_gm6020 { - //! GM6020 设备（云台直驱，**电压**控制）。 - //! - //! # 当前状态（务必读） - //! - //! 协议层的电压帧编解码已经可用（[`motor_protocol::gm6020`]），但**总线驱动 - //! `EscBus` 目前只实现 C620/C610 的电流路径**：它把 `set_effort_raw` 直接组 - //! 成电流帧。把 GM6020 接上云台还需要总线驱动增加"电压帧"分支 - //! （[`MotorKind::encodes_voltage`] 就是给那条分支准备的判据），这属于云台 - //! 部分的工作（ARCHITECTURE.md §5.7 `gimbal` 任务）。 - //! - //! 在此之前，本模块提供的是**型号事实**（协议族、减速比、ID 映射）， - //! 不含"已经能驱动 GM6020"的暗示。 - - use crate::MotorKind; - - /// 该型号的 [`MotorKind`]。 - pub const KIND: MotorKind = MotorKind::Gm6020; - - /// 云台直驱的减速比。 - pub const GEAR_RATIO: f32 = 1.0; - - /// 该型号的控制量是电压而非电流。 - pub const ENCODES_VOLTAGE: bool = true; - - /// 总线驱动是否已支持该型号的驱动路径。 - /// - /// 现在返回 `false`：电压帧分支随云台工作落地。用它做**运行期**判定， - /// 而不是让配置静默接受一个驱动不了的型号。 - #[must_use] - pub const fn drive_path_available() -> bool { - false - } - - #[cfg(test)] - mod tests { - use super::*; - use motor_protocol::gm6020::{COMMAND_FRAME_ID_PRIMARY, feedback_frame_id}; - - #[test] - fn kind_facts_match_the_protocol_module() { - assert_eq!(KIND, MotorKind::Gm6020); - assert!(KIND.encodes_voltage()); - assert!((KIND.default_gear_ratio() - GEAR_RATIO).abs() < 1e-6); - // 两个常量同源：`ENCODES_VOLTAGE` 就是 `KIND.encodes_voltage()` 的镜像。 - assert_eq!(ENCODES_VOLTAGE, KIND.encodes_voltage()); - } - - #[test] - fn gm6020_uses_its_own_frame_ids() { - assert_eq!(COMMAND_FRAME_ID_PRIMARY, 0x1FF); - assert_eq!(feedback_frame_id(1), Some(0x205)); - // 与 C620 不同：C620 的 ID 1 控制帧是 0x200。 - assert_ne!(COMMAND_FRAME_ID_PRIMARY, motor_protocol::COMMAND_FRAME_ID_PRIMARY); - } - - #[test] - fn drive_path_is_honestly_reported_as_unavailable() { - assert!(!drive_path_available(), "云台电压路径落地后本断言才应改为 true"); - } - } -} - pub use command::*; pub use gearbox::*; pub use heading::*; -pub use motor_common::MotorKind; pub use registry::*; pub use state::*; pub use testing::*; pub use traits::*; + +pub mod spec { + //! Generic motor identity and mechanical characteristics. + + use motor_protocol::{CurrentC610, CurrentC620, CurrentEsc, Gm6020Voltage, MotorProtocol}; + + /// Control quantity represented by a motor's wire command. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum ControlMode { + /// Current command. + Current, + /// Voltage command. + Voltage, + } + + /// Mechanical and protocol identity for one motor model. + pub trait MotorSpec: Send + Sync + 'static { + type Protocol: MotorProtocol; + const NAME: &'static str; + const GEAR_RATIO: f32; + const ENCODER_TICKS: u16; + const CONTROL_MODE: ControlMode; + + fn scale() -> ::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; + 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; + 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() -> ::Scale { + S::scale() + } + + /// Compile-time assertion helper for current-path models. + pub fn assert_current_drive() {} + + /// Compile-time assertion helper for voltage-path models. + pub fn assert_voltage_drive() {} +} + +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::(); + spec::assert_current_drive::(); + spec::assert_voltage_drive::(); + 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); + } +} diff --git a/crates/device/remote/Cargo.toml b/crates/device/remote/Cargo.toml new file mode 100644 index 0000000..7835b65 --- /dev/null +++ b/crates/device/remote/Cargo.toml @@ -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" diff --git a/crates/device/remote/src/lib.rs b/crates/device/remote/src/lib.rs new file mode 100644 index 0000000..5d4da35 --- /dev/null +++ b/crates/device/remote/src/lib.rs @@ -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>, +} + +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>, +} + +impl SampleStream { + /// 等待下一个新样本；读取线程退出且没有样本时返回 `None`。 + pub async fn recv(&mut self) -> Option { + 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 { + *self.rx.borrow() + } +} + +/// 打开 DR16 串口并开始接收。 +/// +/// # Errors +/// +/// 设备不存在、权限不足、串口不支持 100000 baud 或配置 8E1 失败。 +pub fn open(device: &str) -> io::Result { + open_with(device, Dr16Options::default()) +} + +/// 使用指定读取参数打开 DR16 串口。 +/// +/// # Errors +/// +/// 同 [`open`]。 +pub fn open_with(device: &str, options: Dr16Options) -> io::Result { + 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(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(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>, + index: usize, + } + + impl Read for ChunkReader { + fn read(&mut self, output: &mut [u8]) -> io::Result { + 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))); + } +} diff --git a/crates/experiment/Cargo.toml b/crates/experiment/trace/Cargo.toml similarity index 82% rename from crates/experiment/Cargo.toml rename to crates/experiment/trace/Cargo.toml index 4e1eecc..42cfe21 100644 --- a/crates/experiment/Cargo.toml +++ b/crates/experiment/trace/Cargo.toml @@ -14,3 +14,6 @@ workspace = true [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "experiment/trace" diff --git a/crates/experiment/src/lib.rs b/crates/experiment/trace/src/lib.rs similarity index 100% rename from crates/experiment/src/lib.rs rename to crates/experiment/trace/src/lib.rs diff --git a/crates/motor-model/Cargo.toml b/crates/model/motor/Cargo.toml similarity index 84% rename from crates/motor-model/Cargo.toml rename to crates/model/motor/Cargo.toml index 2ff3297..e9e089b 100644 --- a/crates/motor-model/Cargo.toml +++ b/crates/model/motor/Cargo.toml @@ -14,3 +14,6 @@ motor-protocol = { workspace = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "model/motor" diff --git a/crates/motor-model/src/lib.rs b/crates/model/motor/src/lib.rs similarity index 58% rename from crates/motor-model/src/lib.rs rename to crates/model/motor/src/lib.rs index 06fad59..45c5b2a 100644 --- a/crates/motor-model/src/lib.rs +++ b/crates/model/motor/src/lib.rs @@ -108,6 +108,24 @@ pub mod dc_motor { self.load_rpm = rpm; self } + + /// Whether all parameters are finite and physically admissible. + #[must_use] + pub fn is_valid(&self) -> bool { + self.gain_rpm_per_raw.is_finite() + && self.gain_rpm_per_raw >= 0.0 + && self.tau_s.is_finite() + && self.tau_s >= 0.0 + && self.max_rpm.is_finite() + && self.max_rpm >= 0.0 + && self.static_raw.is_finite() + && self.static_raw >= 0.0 + && self.coulomb_raw.is_finite() + && self.coulomb_raw >= 0.0 + && self.viscous_raw_per_rpm.is_finite() + && self.viscous_raw_per_rpm >= 0.0 + && self.load_rpm.is_finite() + } } /// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。 @@ -169,6 +187,9 @@ pub mod dc_motor { /// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`， /// 即历史的一阶模型。 pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback { + if !dt.is_finite() || dt <= 0.0 { + return self.feedback(current_raw); + } let applied = self.apply_delay(f32::from(current_raw)); let net = self.net_torque_raw(applied); let target = (net * self.params.gain_rpm_per_raw) @@ -184,6 +205,10 @@ pub mod dc_motor { let ticks = f32::from(ENCODER_TICKS); self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * ticks * dt).rem_euclid(ticks); + self.feedback(applied as i16) + } + + fn feedback(&self, current_raw: i16) -> EscFeedback { EscFeedback { angle_raw: self.angle_raw as u16, speed_rpm: self.speed_rpm as i16, @@ -356,6 +381,36 @@ pub mod dc_motor { assert_eq!(fb.temperature_c, 30); } + #[test] + fn delayed_feedback_reports_the_effective_command() { + let params = PlantParams::gm3508().with_delay_ticks(2); + let mut plant = PlantMotor::new(params); + assert_eq!(plant.step(1000, DT).current_raw, 0); + assert_eq!(plant.step(2000, DT).current_raw, 0); + assert_eq!(plant.step(3000, DT).current_raw, 1000); + } + + #[test] + fn invalid_dt_is_a_strict_no_op() { + let mut plant = PlantMotor::gm3508(); + plant.step(5000, DT); + let before_speed = plant.speed_rpm(); + let before_angle = plant.angle_raw(); + for dt in [0.0, -1.0, f32::NAN, f32::INFINITY] { + plant.step(10_000, dt); + assert_eq!(plant.speed_rpm().to_bits(), before_speed.to_bits()); + assert_eq!(plant.angle_raw().to_bits(), before_angle.to_bits()); + } + } + + #[test] + fn parameter_validation_rejects_non_finite_values() { + let mut params = PlantParams::gm3508(); + assert!(params.is_valid()); + params.tau_s = f32::NAN; + assert!(!params.is_valid()); + } + #[test] fn reset_returns_to_rest() { let mut plant = PlantMotor::gm3508(); @@ -371,7 +426,7 @@ pub mod dc_motor { } pub mod gm3508 { - //! GM3508 的被控对象参数（ARCHITECTURE.md §5.6 / §8.2）。 + //! GM3508 的被控对象参数（docs/chassis.md §7.4）。 //! //! 这里只放**有实测依据**的两组参数： //! @@ -424,3 +479,319 @@ pub mod gm3508 { pub use dc_motor::{PlantMotor, PlantParams}; pub use gm3508::{bench_params, ideal_params}; + +pub mod generic { + //! Generic motor dynamic-model seam. + + use core::marker::PhantomData; + + use motor_protocol::{EscFeedback, FeedbackFields, MotorProtocol}; + + use crate::{PlantMotor, PlantParams}; + + /// Error returned by a dynamic model step. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub enum ModelError { + /// The requested time step is not finite and positive. + InvalidDt, + /// A model parameter is outside its valid domain. + InvalidParams, + } + + impl core::fmt::Display for ModelError { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + match self { + Self::InvalidDt => write!(f, "dt must be finite and positive"), + Self::InvalidParams => write!(f, "model parameters are invalid"), + } + } + } + + impl core::error::Error for ModelError {} + + /// Dynamic model contract for one protocol family. + pub trait MotorPlant { + type Protocol: MotorProtocol; + type Feedback: FeedbackFields + Copy + Send + Sync; + type Error; + + fn step( + &mut self, + command: ::Command, + dt: f32, + ) -> Result; + + fn reset(&mut self); + } + + /// Parameter provider for a DC current-controlled model. + pub trait DcModelSpec: Send + Sync + 'static { + type Protocol: MotorProtocol; + fn ideal_params() -> PlantParams; + fn bench_params() -> PlantParams; + } + + /// DC motor plant generic over the motor model profile. + #[derive(Debug, Clone)] + pub struct DcMotorPlant { + motor: PlantMotor, + marker: PhantomData, + } + + impl DcMotorPlant { + #[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 MotorPlant for DcMotorPlant { + type Protocol = S::Protocol; + type Feedback = EscFeedback; + type Error = ModelError; + + fn step( + &mut self, + command: ::Command, + dt: f32, + ) -> Result { + 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; + fn ideal_params() -> PlantParams { + PlantParams::gm3508() + } + fn bench_params() -> PlantParams { + PlantParams::gm3508_bench() + } + } + + impl DcModelSpec for M2006Model { + type Protocol = motor_protocol::CurrentEsc; + 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::::ideal(); + assert!(plant.step(1000, 0.001).is_ok()); + assert!(plant.speed_rpm().is_finite()); + + let mut plant = DcMotorPlant::::bench(); + assert!(plant.step(1000, 0.001).is_ok()); + } + + #[test] + fn typed_models_reject_invalid_steps_and_parameters() { + let mut plant = DcMotorPlant::::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::::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; + fn params() -> VoltagePlantParams; + } + + /// Direct-drive voltage plant. It deliberately does not reuse DC current friction. + #[derive(Debug, Clone)] + pub struct VoltageMotorPlant { + params: VoltagePlantParams, + speed_rpm: f32, + angle_raw: f32, + delay_line: VecDeque, + marker: PhantomData, + } + + impl VoltageMotorPlant { + #[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 MotorPlant for VoltageMotorPlant { + type Protocol = Gm6020Voltage; + type Feedback = Gm6020Feedback; + type Error = ModelError; + + fn step(&mut self, command: Gm6020Command, dt: f32) -> Result { + 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::::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) + ); + } +} diff --git a/crates/delta10-protocol/Cargo.toml b/crates/protocol/imu/Cargo.toml similarity index 86% rename from crates/delta10-protocol/Cargo.toml rename to crates/protocol/imu/Cargo.toml index f63cafc..bbdcbaf 100644 --- a/crates/delta10-protocol/Cargo.toml +++ b/crates/protocol/imu/Cargo.toml @@ -16,3 +16,6 @@ thiserror = { workspace = true } [dev-dependencies] approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "protocol/imu" diff --git a/crates/delta10-protocol/src/lib.rs b/crates/protocol/imu/src/lib.rs similarity index 100% rename from crates/delta10-protocol/src/lib.rs rename to crates/protocol/imu/src/lib.rs diff --git a/crates/motor-protocol/Cargo.toml b/crates/protocol/motor/Cargo.toml similarity index 86% rename from crates/motor-protocol/Cargo.toml rename to crates/protocol/motor/Cargo.toml index 1e76583..7078f14 100644 --- a/crates/motor-protocol/Cargo.toml +++ b/crates/protocol/motor/Cargo.toml @@ -16,3 +16,6 @@ thiserror = { workspace = true } [dev-dependencies] proptest = { workspace = true } approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "protocol/motor" diff --git a/crates/motor-protocol/src/lib.rs b/crates/protocol/motor/src/lib.rs similarity index 80% rename from crates/motor-protocol/src/lib.rs rename to crates/protocol/motor/src/lib.rs index d008b8a..265e311 100644 --- a/crates/motor-protocol/src/lib.rs +++ b/crates/protocol/motor/src/lib.rs @@ -909,3 +909,276 @@ pub use id::*; pub use m2006::C610; pub use m3508::C620; pub use turn::*; + +pub mod generic { + //! Generic wire-protocol contracts. + //! + //! The concrete codec modules remain the source of wire layout. This module + //! only gives upper layers a type-safe seam for current/voltage families. + + use core::marker::PhantomData; + + use crate::common::{self, DecodeError, EscFeedback}; + use crate::frame::CanFrame; + use crate::gm6020::{self, Gm6020Command, Gm6020Feedback}; + + /// Protocol decoding error used by the generic codec seam. + pub type ProtocolError = DecodeError; + + /// Protocol-independent view of a wire feedback frame. + pub trait FeedbackFields { + fn angle_raw(&self) -> u16; + fn speed_rpm(&self) -> i16; + fn current_raw(&self) -> i16; + fn temperature_c(&self) -> i8; + } + + impl FeedbackFields for EscFeedback { + fn angle_raw(&self) -> u16 { + self.angle_raw + } + fn speed_rpm(&self) -> i16 { + self.speed_rpm + } + fn current_raw(&self) -> i16 { + self.current_raw + } + fn temperature_c(&self) -> i8 { + self.temperature_c + } + } + + impl FeedbackFields for Gm6020Feedback { + fn angle_raw(&self) -> u16 { + self.angle_raw + } + fn speed_rpm(&self) -> i16 { + self.speed_rpm + } + fn current_raw(&self) -> i16 { + self.current_raw + } + fn temperature_c(&self) -> i8 { + self.temperature_c + } + } + + /// Raw effort scaling and saturation for one controller family. + pub trait EffortScale: Copy + Send + Sync + 'static { + fn max_raw(self) -> i16; + fn max_amp(self) -> f32; + fn clamp_raw(self, raw: i16) -> i16; + fn raw_from_amp(self, amp: f32) -> i16; + fn amp_from_raw(self, raw: i16) -> f32; + } + + /// C620 current scale: ±16384 raw ↔ ±20 A. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct CurrentC620; + + /// C610 current scale: ±10000 raw ↔ ±10 A. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct CurrentC610; + + /// GM6020 voltage scale: ±30000 raw. + #[derive(Debug, Clone, Copy, PartialEq, Eq)] + pub struct Voltage30000; + + impl EffortScale for CurrentC620 { + fn max_raw(self) -> i16 { + 16_384 + } + fn max_amp(self) -> f32 { + 20.0 + } + fn clamp_raw(self, raw: i16) -> i16 { + common::clamp_current(common::EscModel::C620, raw) + } + fn raw_from_amp(self, amp: f32) -> i16 { + common::EscModel::C620.raw_from_amp(amp) + } + fn amp_from_raw(self, raw: i16) -> f32 { + common::EscModel::C620.amp_from_raw(raw) + } + } + + impl EffortScale for CurrentC610 { + fn max_raw(self) -> i16 { + 10_000 + } + fn max_amp(self) -> f32 { + 10.0 + } + fn clamp_raw(self, raw: i16) -> i16 { + common::clamp_current(common::EscModel::C610, raw) + } + fn raw_from_amp(self, amp: f32) -> i16 { + common::EscModel::C610.raw_from_amp(amp) + } + fn amp_from_raw(self, raw: i16) -> f32 { + common::EscModel::C610.amp_from_raw(raw) + } + } + + impl EffortScale for Voltage30000 { + fn max_raw(self) -> i16 { + gm6020::VOLTAGE_MAX_RAW + } + fn max_amp(self) -> f32 { + f32::from(gm6020::VOLTAGE_MAX_RAW) + } + fn clamp_raw(self, raw: i16) -> i16 { + gm6020::clamp_voltage(raw) + } + fn raw_from_amp(self, amp: f32) -> i16 { + let raw = amp.clamp(-f32::from(self.max_raw()), f32::from(self.max_raw())); + self.clamp_raw(raw as i16) + } + fn amp_from_raw(self, raw: i16) -> f32 { + f32::from(self.clamp_raw(raw)) + } + } + + /// Wire protocol contract for one motor family. + pub trait MotorProtocol: Send + Sync + 'static { + type Command: Copy + Send + Sync; + type Feedback: FeedbackFields + Copy + Send + Sync; + type Scale: EffortScale; + + fn command_frame_id(id: u8) -> Option; + fn feedback_frame_id(id: u8) -> Option; + 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(PhantomData); + + impl MotorProtocol for CurrentEsc { + type Command = i16; + type Feedback = EscFeedback; + type Scale = S; + + fn command_frame_id(id: u8) -> Option { + common::command_frame_id(id) + } + fn feedback_frame_id(id: u8) -> Option { + 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 { + gm6020::command_frame_id(id) + } + fn feedback_frame_id(id: u8) -> Option { + 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) = + 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 = ::command_from_raw(1234); + assert_eq!(command.voltage_raw, 1234); + assert_eq!( + ::command_frame_id(1), + Some(gm6020::COMMAND_FRAME_ID_PRIMARY) + ); + } +} diff --git a/crates/protocol/remote/Cargo.toml b/crates/protocol/remote/Cargo.toml new file mode 100644 index 0000000..545ee48 --- /dev/null +++ b/crates/protocol/remote/Cargo.toml @@ -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" diff --git a/crates/protocol/remote/src/lib.rs b/crates/protocol/remote/src/lib.rs new file mode 100644 index 0000000..dd22b08 --- /dev/null +++ b/crates/protocol/remote/src/lib.rs @@ -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 for SwitchPosition { + type Error = DecodeError; + + fn try_from(value: u8) -> Result { + 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 { + 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, + 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 { + 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 = 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]); + } +} diff --git a/crates/sim-harness/Cargo.toml b/crates/simulation/motor/Cargo.toml similarity index 91% rename from crates/sim-harness/Cargo.toml rename to crates/simulation/motor/Cargo.toml index 7a5ea32..97475bc 100644 --- a/crates/sim-harness/Cargo.toml +++ b/crates/simulation/motor/Cargo.toml @@ -24,3 +24,6 @@ rand = { workspace = true } [dev-dependencies] tokio = { workspace = true, features = ["test-util"] } approx = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "simulation/motor" diff --git a/crates/sim-harness/src/lib.rs b/crates/simulation/motor/src/lib.rs similarity index 97% rename from crates/sim-harness/src/lib.rs rename to crates/simulation/motor/src/lib.rs index 6736864..9ff5c27 100644 --- a/crates/sim-harness/src/lib.rs +++ b/crates/simulation/motor/src/lib.rs @@ -706,7 +706,7 @@ pub mod plant { use motor_protocol::{EscFeedback, clamp_current}; use crate::harness::LoopLimits; - use motor_model::{PlantMotor, PlantParams}; + use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams}; /// 把被测电机的**转速**（rpm）当作输出、**effort raw** 当作输入的被控对象。 /// @@ -715,7 +715,7 @@ pub mod plant { /// 不同的饱和行为。 #[derive(Debug, Clone)] pub struct SimPlant { - motor: PlantMotor, + motor: DcMotorPlant, limits: LoopLimits, steps: u64, } @@ -724,7 +724,7 @@ pub mod plant { /// 由参数构造（默认限幅与 [`LoopLimits::gm3508`] 一致）。 #[must_use] pub fn new(params: PlantParams) -> Self { - Self { motor: PlantMotor::new(params), limits: LoopLimits::gm3508(), steps: 0 } + Self { motor: DcMotorPlant::new(params), limits: LoopLimits::gm3508(), steps: 0 } } /// 换限幅与电调型号。 @@ -789,15 +789,17 @@ pub mod plant { self.limits.model, raw.clamp(-self.limits.current_limit_raw, self.limits.current_limit_raw), ); - let feedback = self.motor.step(command, dt); - self.steps += 1; - return Ok(f32::from(feedback.speed_rpm)); + if let Ok(feedback) = MotorPlant::step(&mut self.motor, command, dt) { + self.steps += 1; + return Ok(f32::from(feedback.speed_rpm)); + } + return Ok(self.motor.speed_rpm()); } Ok(self.motor.speed_rpm()) } fn reset(&mut self) { - self.motor.reset(); + MotorPlant::reset(&mut self.motor); self.steps = 0; } @@ -926,7 +928,7 @@ pub mod rig { use tokio::time::{MissedTickBehavior, interval}; use tracing::trace; - use motor_model::{PlantMotor, PlantParams}; + use motor_model::{DcMotorPlant, Gm3508Model, MotorPlant, PlantParams}; /// 用理想 `GM3508` 模型启动指定电调 ID 的假电调组。 /// @@ -943,8 +945,8 @@ pub mod rig { tick: Duration, ) -> JoinHandle<()> { let specs: Vec<(u8, PlantParams)> = specs.into_iter().collect(); - let mut motors: HashMap = - specs.iter().map(|&(id, params)| (id, PlantMotor::new(params))).collect(); + let mut motors: HashMap> = + specs.iter().map(|&(id, params)| (id, DcMotorPlant::new(params))).collect(); let mut commands: HashMap = specs.iter().map(|&(id, _)| (id, 0)).collect(); let dt = tick.as_secs_f32(); let mut ticker = interval(tick); @@ -960,7 +962,9 @@ pub mod rig { _ = ticker.tick() => { for (&id, motor) in &mut motors { let cmd = commands.get(&id).copied().unwrap_or(0); - let feedback = motor.step(cmd, dt); + let Ok(feedback) = MotorPlant::step(motor, cmd, dt) else { + continue; + }; if link .outgoing_tx .send(encode_feedback_frame(id, &feedback)) diff --git a/crates/tuning/Cargo.toml b/crates/tooling/tuning/Cargo.toml similarity index 93% rename from crates/tuning/Cargo.toml rename to crates/tooling/tuning/Cargo.toml index ad1e8b1..cebb496 100644 --- a/crates/tuning/Cargo.toml +++ b/crates/tooling/tuning/Cargo.toml @@ -29,3 +29,6 @@ motor-model = { workspace = true } [dev-dependencies] approx = { workspace = true } proptest = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "tooling/tuning" diff --git a/crates/tuning/src/lib.rs b/crates/tooling/tuning/src/lib.rs similarity index 98% rename from crates/tuning/src/lib.rs rename to crates/tooling/tuning/src/lib.rs index 96e458b..17bba76 100644 --- a/crates/tuning/src/lib.rs +++ b/crates/tooling/tuning/src/lib.rs @@ -1591,7 +1591,7 @@ pub mod preset { use std::path::{Path, PathBuf}; use experiment::{Scenario, SetpointProfile}; - use motor_model::PlantParams; + use motor_model::{DcModelSpec, PlantParams}; use serde::{Deserialize, Serialize}; use sim_harness::PlantTolerances; use thiserror::Error; @@ -1724,29 +1724,29 @@ pub mod preset { /// 离线被控对象模型。 #[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)] #[serde(rename_all = "kebab-case")] - pub enum PlantSpec { + pub enum PlantProfile { /// 理想一阶模型（无摩擦、无延迟）。 - Gm3508, + Ideal, /// 台架量级：静 / 库仑摩擦 + 2 拍延迟。 - Gm3508Bench, + Bench, } - impl PlantSpec { + impl PlantProfile { /// 文本名。 #[must_use] pub const fn name(self) -> &'static str { match self { - Self::Gm3508 => "gm3508", - Self::Gm3508Bench => "gm3508-bench", + Self::Ideal => "ideal", + Self::Bench => "bench", } } /// 展开为被控对象参数。 #[must_use] - pub fn params(self) -> PlantParams { + pub fn params(self) -> PlantParams { match self { - Self::Gm3508 => PlantParams::gm3508(), - Self::Gm3508Bench => PlantParams::gm3508_bench(), + Self::Ideal => M::ideal_params(), + Self::Bench => M::bench_params(), } } } @@ -1921,7 +1921,7 @@ pub mod preset { /// 离线被控对象与域随机化。 SimSpec { /// 名义模型。 - plant: PlantSpec, + plant: PlantProfile, /// 集成成员数（1 = 只用名义模型）。 ensemble: usize, /// 鲁棒权重：0 = 只看均值，1 = 只看最坏成员。 @@ -2040,7 +2040,7 @@ pub mod preset { threads: Some(0), }, sim: SimSpec { - plant: Some(PlantSpec::Gm3508Bench), + plant: Some(PlantProfile::Bench), ensemble: Some(8), robust_weight: Some(0.5), gain_pct: Some(0.2), @@ -2082,8 +2082,8 @@ pub mod preset { /// /// 缺项（[`PresetError::Missing`]）或区间 / 阈值 / 预算非法 /// （[`PresetError::Invalid`]）时返回错误。 - pub fn to_tuning(&self) -> Result { - let plant_spec = self.sim.plant.unwrap_or(PlantSpec::Gm3508Bench); + pub fn to_tuning(&self) -> Result { + let plant_spec = self.sim.plant.unwrap_or(PlantProfile::Bench); Ok(Tuning { backend: self.backend.unwrap_or(BackendSpec::Sim), space: self.gain_space()?, @@ -2092,7 +2092,7 @@ pub mod preset { acceptance: self.acceptance()?, search: self.search()?, plant_spec, - plant: plant_spec.params(), + plant: plant_spec.params::(), tolerances: self.tolerances()?, ensemble: self.ensemble()?, robust_weight: self.robust_weight()?, @@ -2417,7 +2417,7 @@ pub mod preset { /// 搜索配置。 pub search: SearchConfig, /// 名义被控对象（枚举，供显示 / 复现）。 - pub plant_spec: PlantSpec, + pub plant_spec: PlantProfile, /// 名义被控对象（展开后的参数，供仿真重放）。 pub plant: PlantParams, /// 域随机化幅度。 @@ -2641,14 +2641,16 @@ top_k = 3 let text = toml::to_string(&PresetFile::builtin()).unwrap(); let round_trip = PresetFile::parse_str(&text).unwrap(); assert_eq!(round_trip, PresetFile::builtin()); - assert!(round_trip.select(None).unwrap().to_tuning().is_ok()); + assert!( + round_trip.select(None).unwrap().to_tuning::().is_ok() + ); } #[test] fn builtin_matches_the_historical_cli_defaults() { // 这些数字是 chassis-tune 历史的 CLI 默认值，改动等于改变整定的默认行为， // 必须是有意为之。 - let tuning = Preset::builtin().to_tuning().unwrap(); + let tuning = Preset::builtin().to_tuning::().unwrap(); assert_eq!(tuning.backend, BackendSpec::Sim); assert!((tuning.space.kp.min - 0.5).abs() < 1e-12); assert!((tuning.space.kp.max - 30.0).abs() < 1e-12); @@ -2671,7 +2673,7 @@ top_k = 3 fn preset_overlays_only_the_fields_it_sets() { let file = PresetFile::parse_str(MINIMAL).unwrap(); let merged = Preset::builtin().overlay(file.select(None).unwrap()); - let tuning = merged.to_tuning().unwrap(); + let tuning = merged.to_tuning::().unwrap(); assert_eq!(tuning.search.samples, 32, "预设必须生效"); assert_eq!(tuning.search.top_k, 3); // 未提及的项沿用内置默认。 @@ -2686,7 +2688,7 @@ top_k = 3 let mut overlay = Preset::default(); overlay.space.kp = Some(RangeSpec::new(1.0, 5.0)); let merged = Preset::builtin().overlay(&overlay); - let tuning = merged.to_tuning().unwrap(); + let tuning = merged.to_tuning::().unwrap(); assert!((tuning.space.kp.min - 1.0).abs() < 1e-12); assert!((tuning.space.kp.max - 5.0).abs() < 1e-12); assert_eq!(tuning.space.kp.scale, Scale::Log, "刻度必须从内置默认继承"); @@ -2759,7 +2761,9 @@ top_k = 3 let build = |body: &str| -> Result { let text = format!("version = 1\n[preset.a]\n{body}"); let file = PresetFile::parse_str(&text)?; - Preset::builtin().overlay(file.select(None)?).to_tuning() + Preset::builtin() + .overlay(file.select(None)?) + .to_tuning::() }; let error = @@ -2794,13 +2798,13 @@ top_k = 3 // 缺 scale 的轴（内置换行默认提供了刻度，这里显式抹掉）。 let mut preset = Preset::builtin(); preset.space.kd = Some(RangeSpec { min: 0.1, max: 1.0, scale: None }); - let error = preset.to_tuning().unwrap_err(); + let error = preset.to_tuning::().unwrap_err(); assert!(matches!(&error, PresetError::Invalid { field: "kd", .. }), "{error}"); } #[test] fn missing_keys_are_reported_for_a_bare_preset() { - let error = Preset::default().to_tuning().unwrap_err(); + let error = Preset::default().to_tuning::().unwrap_err(); assert!(matches!(error, PresetError::Missing { .. }), "{error}"); } @@ -2810,16 +2814,16 @@ top_k = 3 preset.scenario.profile = Some(ProfileSpec::Staircase); preset.scenario.stair_levels = None; assert!(matches!( - preset.to_tuning(), + preset.to_tuning::(), Err(PresetError::Missing { field: "scenario.stair_levels" }) )); preset.scenario.stair_levels = Some(0); assert!(matches!( - preset.to_tuning(), + preset.to_tuning::(), Err(PresetError::Invalid { field: "scenario.stair_levels", .. }) )); preset.scenario.stair_levels = Some(5); - assert!(preset.to_tuning().is_ok()); + assert!(preset.to_tuning::().is_ok()); } #[test] @@ -2854,7 +2858,7 @@ top_k = 3 assert_eq!(spec.name(), text); assert_eq!(parse(text).value, spec); } - assert_eq!(PlantSpec::Gm3508Bench.name(), "gm3508-bench"); + assert_eq!(PlantProfile::Bench.name(), "bench"); assert_eq!(SolverSpec::NelderMead.name(), "nelder-mead"); assert_eq!(BackendSpec::Bench.name(), "bench"); let profile: Wrapper = diff --git a/crates/socketcan/Cargo.toml b/crates/transport/can/Cargo.toml similarity index 77% rename from crates/socketcan/Cargo.toml rename to crates/transport/can/Cargo.toml index d2a40ff..39f92f1 100644 --- a/crates/socketcan/Cargo.toml +++ b/crates/transport/can/Cargo.toml @@ -16,11 +16,10 @@ thiserror = { workspace = true } tracing = { workspace = true } [dev-dependencies] -esc-bus = { workspace = true } -motor = { workspace = true } -motor-control = { workspace = true } tokio = { workspace = true } -motor-protocol = { workspace = true } [target.'cfg(target_os = "linux")'.dependencies] socketcan-rs = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "transport/can" diff --git a/crates/socketcan/src/lib.rs b/crates/transport/can/src/lib.rs similarity index 100% rename from crates/socketcan/src/lib.rs rename to crates/transport/can/src/lib.rs diff --git a/crates/slcan/Cargo.toml b/crates/transport/serial/Cargo.toml similarity index 87% rename from crates/slcan/Cargo.toml rename to crates/transport/serial/Cargo.toml index 246f7d7..9703b9d 100644 --- a/crates/slcan/Cargo.toml +++ b/crates/transport/serial/Cargo.toml @@ -19,3 +19,6 @@ serial = { workspace = true } [dev-dependencies] tokio = { workspace = true } + +[package.metadata.sentry-chassis] +layer = "transport/serial" diff --git a/crates/slcan/src/lib.rs b/crates/transport/serial/src/lib.rs similarity index 100% rename from crates/slcan/src/lib.rs rename to crates/transport/serial/src/lib.rs diff --git a/crates/verification/integration/Cargo.toml b/crates/verification/integration/Cargo.toml new file mode 100644 index 0000000..fe8cd7f --- /dev/null +++ b/crates/verification/integration/Cargo.toml @@ -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" diff --git a/crates/verification/integration/src/lib.rs b/crates/verification/integration/src/lib.rs new file mode 100644 index 0000000..95dc449 --- /dev/null +++ b/crates/verification/integration/src/lib.rs @@ -0,0 +1 @@ +//! Cross-layer integration test package. diff --git a/crates/esc-bus/tests/closed_loop.rs b/crates/verification/integration/tests/esc_closed_loop.rs similarity index 100% rename from crates/esc-bus/tests/closed_loop.rs rename to crates/verification/integration/tests/esc_closed_loop.rs diff --git a/crates/esc-bus/tests/harness_parity.rs b/crates/verification/integration/tests/esc_harness_parity.rs similarity index 100% rename from crates/esc-bus/tests/harness_parity.rs rename to crates/verification/integration/tests/esc_harness_parity.rs diff --git a/crates/socketcan/tests/vcan.rs b/crates/verification/integration/tests/socketcan_vcan.rs similarity index 100% rename from crates/socketcan/tests/vcan.rs rename to crates/verification/integration/tests/socketcan_vcan.rs diff --git a/docs/chassis.md b/docs/chassis.md index 24fda1a..e62cf97 100644 --- a/docs/chassis.md +++ b/docs/chassis.md @@ -280,7 +280,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp | 一起动就飞 | 前馈过大 or 限流远大于实际所需 | ff → 0、限流贴近实际 | | 稳态有静差 | 摩擦未被积分吃掉 | 检查 `ki` 是否被抗饱和长期冻结 | -### 7.4 被控对象模型（`sim-harness::motor_model`） +### 7.4 被控对象模型（`motor-model::MotorPlant`） 逐拍离散（`dt` 秒，电调坐标系：正电流 → 正转速）： @@ -299,7 +299,7 @@ effort_raw = clamp( PID(target_rpm, measured_rpm, dt) + feedforward × target_rp ## 8. 配置参考（chassis.toml） 样例：[`config/chassis.toml.example`](../config/chassis.toml.example)。 -schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程与整定工具共用同一份**—— +schema 由 [`chassis-config`](../crates/config/chassis) 定义，**守护进程与整定工具共用同一份**—— 这样"整定工具用的限流"和"守护进程用的限流"不可能悄悄不一致。 ### 8.1 顶层 @@ -308,11 +308,23 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程 | --- | --- | | `[can] interface` | 底盘总线 SocketCAN 接口名（如 `can0`） | | `[can.function] interface` | 预留的功能总线（云台 / 拨弹），必须与底盘总线**不同名** | +| `[remote]` | 可选 DR16/D-BUS 遥控输入；缺省时保持静止 | | `[chassis]` | 控制循环与安全超时、速度上限 | | `[mecanum]` | `half_length` / `half_width` / `wheel_radius`（米） | | `[[motor]]` × 4 | 顺序固定 FL / FR / RL / RR | -### 8.2 `[chassis]`（缺省值） +### 8.2 `[remote]`（可选） + +| 键 | 默认 | 含义 | +| --- | --- | --- | +| `kind` | 必填 | 当前支持 `dr16` | +| `device` | `/dev/ttyS2` | D-BUS UART 设备 | +| `timeout_ms` | 100 | 合法帧断流后立即失能 | +| `deadband` | 0.05 | 摇杆满量程死区，之后使用平方曲线 | +| `ramp_ms` | 500 | 零到满量程逐轴斜坡；失能/断流立即归零 | +| `invert_vx/vy/omega` | false | 实车方向校正 | + +### 8.3 `[chassis]`（缺省值） | 键 | 默认 | 含义 | | --- | --- | --- | @@ -322,7 +334,7 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程 | `motor_command_timeout_ms` | 50 | 驱动层命令租约（必须 ≥ `control_period_ms`） | | `max_vx` / `max_vy` / `max_omega` | 3.0 / 3.0 / 6.0 | 速度限幅（m/s、m/s、rad/s） | -### 8.3 `[[motor]]` +### 8.4 `[[motor]]` | 键 | 含义 | | --- | --- | @@ -334,12 +346,14 @@ schema 由 [`chassis-config`](../crates/chassis-config) 定义，**守护进程 | `current_limit` | effort 限幅（raw），同时是 PID 钳位点 | | `kp` / `ki` / `kd` / `feedforward` | 速度环增益（整定的产物写回这里） | -### 8.4 校验规则（`Config::validate`） +### 8.5 校验规则（`Config::validate`） - 电机数必须**恰好 4**；麦轮几何必须为正；四个超时必须 > 0； `motor_command_timeout_ms ≥ control_period_ms`；速度上限 > 0；`gear_ratio > 0`； - 底盘总线**只允许一种协议族**（四轮不能混用 C620 / C610）； - 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）； +- `[remote]` 若启用：设备名非空，`timeout_ms` / `ramp_ms` > 0，`deadband ∈ [0, 0.5)`， + 且遥控断流超时不得晚于底盘指令超时； - `deny_unknown_fields`：**拼错的键直接报错**，不会被静默忽略。 --- @@ -441,7 +455,7 @@ chassis-tune sweep --preset config/tune.toml --samples 128 # CLI 覆盖预 | `field_updown` | bench | **up-down** 1200 rpm | 实战验收：升 / 降 / 回零双向，阈值收紧到 12% / 3% | **预设 schema**（`version = 1`，未知键报错，见 -[`crates/tuning/src/lib.rs`](../crates/tuning/src/lib.rs)）： +[`crates/tooling/tuning/src/lib.rs`](../crates/tooling/tuning/src/lib.rs)）： ```toml version = 1 @@ -588,7 +602,7 @@ cargo run --release -p spin -- motor -- --interface can0 --dry-run | 整定 | 指标（合成轨迹解析解）、参数空间（界内 / 确定性 / 对数分布 / 分层覆盖）、成本（硬约束罚值）、搜索（同 seed 逐位复现、优于手调基线）、报告（CSV/TOML/JSON 往返） | 全平台（CI 跑 `just tune-smoke`） | | **预设** | schema 往返、逐字段覆盖、优先级（CLI > 预设 > 内置）、未知键 / 越界 / 缺失报错、**`config/tune.toml` 全部预设可解算**、命名与后端一致、安全参数不可注入 | 全平台 | | vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） | -| 实车台架 | `spin chassis` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） | +| 实车台架 | `spin chassis` 四轮定速：链路/反馈、方向、转速稳态、过速保护、安全停机；`spin rc` 只读监视 DR16；`chassis-tune --backend bench` 逐次试验 + 重复性复验 | armv7 + slcan/SocketCAN（人工） | | 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check；armv7 静态构建 `chassisd` | CI | ### 13.2 每个阶段完成后的门禁（`just lint` + 以下） diff --git a/docs/dependencies.md b/docs/dependencies.md new file mode 100644 index 0000000..2535d84 --- /dev/null +++ b/docs/dependencies.md @@ -0,0 +1,50 @@ +# 依赖分层与 Trait 边界 + +`config/dependency-policy.toml` 是依赖规则的唯一机器可读来源。crate 名称保持不变，manifest 按 `crates//` 组织，并在 `[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`、`EscMotor`、`EscBus` 通过 `MotorSpec::Protocol` 组合协议能力；电流路径要求 `CurrentEscDrive`，GM6020 使用独立的 `VoltageEscDrive` 路径。 +- `model/motor`：`MotorPlant`、`DcMotorPlant`，只处理动态模型和反馈量化。 + +`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` 执行同一规则。 diff --git a/docs/dt16/DT7&DR16_RC_System_User_Manual_v2.00_cn.pdf b/docs/dt16/DT7&DR16_RC_System_User_Manual_v2.00_cn.pdf new file mode 100644 index 0000000..7bd2b01 Binary files /dev/null and b/docs/dt16/DT7&DR16_RC_System_User_Manual_v2.00_cn.pdf differ diff --git a/docs/dt16/README.md b/docs/dt16/README.md new file mode 100644 index 0000000..fed7706 --- /dev/null +++ b/docs/dt16/README.md @@ -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) diff --git a/justfile b/justfile index 66c638e..8343b99 100644 --- a/justfile +++ b/justfile @@ -11,18 +11,36 @@ lint: test: cargo test --workspace --all-targets +audit: + cargo +nightly -Zscript scripts/audit-rules.rs + +audit-test: + cargo +nightly -Zscript scripts/audit-rules.rs --self-test + +install-hooks: + git config --local core.hooksPath .githooks + chmod +x .githooks/pre-commit + +check: + just fmt + just lint + just test + just audit + vcan-test: sudo modprobe vcan sudo ip link add dev vcan0 type vcan || true sudo ip link set up vcan0 - cargo test -p socketcan@0.1.0 --test vcan -- --ignored --test-threads 1 + cargo test -p integration-tests --test socketcan_vcan -- --ignored --test-threads 1 run-mock: cargo run -p chassisd -- --mock +rc-monitor device="/dev/ttyS2": + cargo run --release -p spin -- rc --device {{device}} + check-structure: - test -z "$(find crates/*/src -mindepth 2 -type d -print)" - ! rg -n '\bunsafe\b|libc|no_std' crates Cargo.toml -g '*.rs' -g '*.toml' + cargo +nightly -Zscript scripts/audit-rules.rs build-armv7: rustup target add armv7-unknown-linux-musleabihf diff --git a/scripts/audit-rules.rs b/scripts/audit-rules.rs new file mode 100755 index 0000000..e92d45c --- /dev/null +++ b/scripts/audit-rules.rs @@ -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; + +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, + #[serde(skip_serializing_if = "Option::is_none")] + path: Option, +} + +impl Issue { + fn new(code: impl Into, message: impl Into) -> Self { + Self { + code: code.into(), + message: message.into(), + package: None, + path: None, + } + } + + fn package(mut self, package: impl Into) -> Self { + self.package = Some(package.into()); + self + } + + fn path(mut self, path: impl AsRef) -> 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, +} + +#[derive(Debug, Default)] +struct Graph { + normal: BTreeMap>, + dev: BTreeMap>, +} + +fn root_dir() -> Result { + 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 { + let text = fs::read_to_string(path)?; + Ok(text.parse::()?) +} + +fn run(command: &mut Command) -> Result { + 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 { + 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 { + 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 { + 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, 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) -> BTreeMap { + packages + .values() + .map(|package| (package.package_dir.clone(), package.name.clone())) + .collect() +} + +fn dependency_target( + dependency: &Value, + by_path: &BTreeMap, +) -> Option { + let path = dependency.get("path")?.as_str()?; + by_path.get(&PathBuf::from(path)).cloned() +} + +fn build_graph( + packages: &BTreeMap, +) -> 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) -> 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 = 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, + allowed: &BTreeSet, +) -> Vec { + let mut issues = actual + .difference(allowed) + .map(|target| { + Issue::new( + "dependency-edge", + format!("{kind} dependency {source} -> {target} is not declared"), + ) + .package(source) + }) + .collect::>(); + + 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>) -> Vec> { + #[derive(Clone, Copy, PartialEq)] + enum Color { + White, + Gray, + Black, + } + + fn visit( + node: &str, + edges: &BTreeMap>, + colors: &mut BTreeMap, + trail: &mut Vec, + cycles: &mut Vec>, + ) { + 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 { + 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 { + 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, +) -> Result, 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, +) -> Result, 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::>(); + 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, 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, BTreeSet) { + 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, 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::>(); + let actual = packages.keys().cloned().collect::>(); + + 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::, 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::, 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, 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, AnyError> { + let staged: BTreeSet = git_lines( + root, + &["diff", "--cached", "--name-only", "--diff-filter=ACMR"], + )? + .into_iter() + .collect(); + let unstaged: BTreeSet = 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()) + } +}