From 7d87987e41e92c2d2af66397e013370450d0f6e9 Mon Sep 17 00:00:00 2001 From: Horldsence Date: Tue, 22 Sep 2026 20:05:16 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=88=9D=E5=A7=8B=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .cargo/config.toml | 10 + .github/workflows/ci.yml | 81 ++ .gitignore | 1 + .mimocode/plans/1790059920942-kind-squid.md | 384 ++++++ ...ss_5bb01b23-baf7-4632-be6e-f8a5bf9ab890.md | 75 + .zcodeignore | 33 + Cargo.lock | 1226 +++++++++++++++++ Cargo.toml | 65 + LICENSE | 21 + README.md | 61 + clippy.toml | 4 + config/chassis.toml.example | 67 + crates/can-link/Cargo.toml | 13 + crates/can-link/src/lib.rs | 75 + crates/chassisd/Cargo.toml | 28 + crates/chassisd/src/config.rs | 504 +++++++ crates/chassisd/src/controller.rs | 203 +++ crates/chassisd/src/lib.rs | 15 + crates/chassisd/src/main.rs | 161 +++ crates/chassisd/src/source.rs | 81 ++ crates/chassisd/tests/daemon.rs | 169 +++ crates/dji-esc-socketcan/Cargo.toml | 25 + crates/dji-esc-socketcan/examples/spin.rs | 110 ++ crates/dji-esc-socketcan/src/bridge.rs | 96 ++ crates/dji-esc-socketcan/src/lib.rs | 11 + crates/dji-esc-socketcan/tests/vcan.rs | 132 ++ crates/dji-esc/Cargo.toml | 20 + crates/dji-esc/src/bus.rs | 512 +++++++ crates/dji-esc/src/codec.rs | 249 ++++ crates/dji-esc/src/frame.rs | 13 + crates/dji-esc/src/lib.rs | 29 + crates/dji-esc/src/sim.rs | 116 ++ crates/dji-esc/src/turn.rs | 134 ++ crates/dji-esc/tests/closed_loop.rs | 275 ++++ crates/mecanum/Cargo.toml | 16 + crates/mecanum/proptest-regressions/lib.txt | 7 + crates/mecanum/src/lib.rs | 284 ++++ crates/motor-core/Cargo.toml | 12 + crates/motor-core/src/command.rs | 41 + crates/motor-core/src/gearbox.rs | 173 +++ crates/motor-core/src/heading.rs | 21 + crates/motor-core/src/lib.rs | 93 ++ crates/motor-core/src/pid.rs | 264 ++++ crates/motor-core/src/position.rs | 103 ++ crates/motor-core/src/state.rs | 29 + crates/motor-core/src/testing.rs | 116 ++ crates/motor-core/src/traits.rs | 47 + crates/motor-core/src/velocity.rs | 87 ++ deploy/chassisd.service | 19 + docs/architecture.md | 77 ++ justfile | 48 + rust-toolchain.toml | 3 + rustfmt.toml | 2 + ubuntu.md | 5 + 54 files changed, 6446 insertions(+) create mode 100644 .cargo/config.toml create mode 100644 .github/workflows/ci.yml create mode 100644 .gitignore create mode 100644 .mimocode/plans/1790059920942-kind-squid.md create mode 100644 .zcode/plans/plan-sess_5bb01b23-baf7-4632-be6e-f8a5bf9ab890.md create mode 100644 .zcodeignore create mode 100644 Cargo.lock create mode 100644 Cargo.toml create mode 100644 LICENSE create mode 100644 README.md create mode 100644 clippy.toml create mode 100644 config/chassis.toml.example create mode 100644 crates/can-link/Cargo.toml create mode 100644 crates/can-link/src/lib.rs create mode 100644 crates/chassisd/Cargo.toml create mode 100644 crates/chassisd/src/config.rs create mode 100644 crates/chassisd/src/controller.rs create mode 100644 crates/chassisd/src/lib.rs create mode 100644 crates/chassisd/src/main.rs create mode 100644 crates/chassisd/src/source.rs create mode 100644 crates/chassisd/tests/daemon.rs create mode 100644 crates/dji-esc-socketcan/Cargo.toml create mode 100644 crates/dji-esc-socketcan/examples/spin.rs create mode 100644 crates/dji-esc-socketcan/src/bridge.rs create mode 100644 crates/dji-esc-socketcan/src/lib.rs create mode 100644 crates/dji-esc-socketcan/tests/vcan.rs create mode 100644 crates/dji-esc/Cargo.toml create mode 100644 crates/dji-esc/src/bus.rs create mode 100644 crates/dji-esc/src/codec.rs create mode 100644 crates/dji-esc/src/frame.rs create mode 100644 crates/dji-esc/src/lib.rs create mode 100644 crates/dji-esc/src/sim.rs create mode 100644 crates/dji-esc/src/turn.rs create mode 100644 crates/dji-esc/tests/closed_loop.rs create mode 100644 crates/mecanum/Cargo.toml create mode 100644 crates/mecanum/proptest-regressions/lib.txt create mode 100644 crates/mecanum/src/lib.rs create mode 100644 crates/motor-core/Cargo.toml create mode 100644 crates/motor-core/src/command.rs create mode 100644 crates/motor-core/src/gearbox.rs create mode 100644 crates/motor-core/src/heading.rs create mode 100644 crates/motor-core/src/lib.rs create mode 100644 crates/motor-core/src/pid.rs create mode 100644 crates/motor-core/src/position.rs create mode 100644 crates/motor-core/src/state.rs create mode 100644 crates/motor-core/src/testing.rs create mode 100644 crates/motor-core/src/traits.rs create mode 100644 crates/motor-core/src/velocity.rs create mode 100644 deploy/chassisd.service create mode 100644 docs/architecture.md create mode 100644 justfile create mode 100644 rust-toolchain.toml create mode 100644 rustfmt.toml create mode 100644 ubuntu.md diff --git a/.cargo/config.toml b/.cargo/config.toml new file mode 100644 index 0000000..c3c6324 --- /dev/null +++ b/.cargo/config.toml @@ -0,0 +1,10 @@ +# 跨平台交叉编译配置。 +# +# armv7（目标板）使用 musl 静态链接：目标产物不依赖板端 libc 版本， +# 且 rust-std 自带全部 crt 对象，配合 Rust 自带的 rust-lld 即可在 +# macOS / Linux 宿主机上直接交叉编译，无需安装外部交叉工具链。 +# +# rustup target add armv7-unknown-linux-musleabihf +# cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf +[target.armv7-unknown-linux-musleabihf] +linker = "rust-lld" diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..4688c81 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,81 @@ +name: CI + +on: + push: + branches: [main] + pull_request: + +env: + CARGO_TERM_COLOR: always + +jobs: + lint-and-test: + name: fmt / clippy / test / doc + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: dtolnay/rust-toolchain@stable + with: + components: rustfmt, clippy + - uses: Swatinem/rust-cache@v2 + - name: rustfmt + run: cargo fmt --all -- --check + - name: clippy + run: cargo clippy --workspace --all-targets -- -D warnings + - name: test + run: cargo test --workspace + - name: doc + env: + RUSTDOCFLAGS: -D warnings + run: cargo doc --workspace --no-deps + + vcan-integration: + name: vcan0 integration (real kernel CAN stack) + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: dtolnay/rust-toolchain@stable + - uses: Swatinem/rust-cache@v2 + - name: set up vcan0 + run: | + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan + sudo ip link set up vcan0 + - name: run vcan tests + run: cargo test -p dji-esc-socketcan -- --ignored --test-threads 1 + + cross-aarch64: + name: cross check aarch64 + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: dtolnay/rust-toolchain@stable + with: + targets: aarch64-unknown-linux-gnu + - uses: Swatinem/rust-cache@v2 + - run: cargo check --workspace --target aarch64-unknown-linux-gnu + + build-armv7: + name: build armv7 static (board target) + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: dtolnay/rust-toolchain@stable + with: + targets: armv7-unknown-linux-musleabihf + - uses: Swatinem/rust-cache@v2 + - run: cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf + + coverage: + name: coverage (llvm-cov) + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: dtolnay/rust-toolchain@stable + - uses: Swatinem/rust-cache@v2 + - uses: taiki-e/install-action@cargo-llvm-cov + - run: cargo llvm-cov --workspace --lcov --output-path lcov.info + - uses: actions/upload-artifact@v4 + with: + name: coverage-lcov + path: lcov.info diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ea8c4bf --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/target diff --git a/.mimocode/plans/1790059920942-kind-squid.md b/.mimocode/plans/1790059920942-kind-squid.md new file mode 100644 index 0000000..0404a4a --- /dev/null +++ b/.mimocode/plans/1790059920942-kind-squid.md @@ -0,0 +1,384 @@ +# 审计 + 电机抽象层改造 + 实车台架单轮测试 + +## 目标（用户已确认） + +1. **审计**现有抽象层级与抽象功能是否正确。 +2. **改造抽象层**（本次就做），为后续派生铺路： + - 云台 = **GM6020**（位置环，绝对/多圈角度） + - 拨弹轮 = **M2006 + C610**（速度环 + 定量拨弹位置环） +3. **小车轮子测试** = 实车台架单轮验证（现有 4× GM3508 轮子）。 +4. **硬约束**：合适的 Rust 抽象层与 crate，**明确单一职责**。 +5. **架构决策**（本轮确认）： + - **双 CAN 总线**：底盘一条 CAN，云台/拨弹另一条 CAN（协议隔离，不用避让 ID）。 + - **必须预留 IMU 融合口**：云台绝对角需要 `HeadingSource` 类 seam。 + - **wheel-bench 分阶段自动跑**。 + +--- + +## 一、审计结论 + +### 1.1 分层现状（正确的部分，保留） + +``` +chassisd ──► mecanum ──► motor-core::Motor ◄── dji-esc (EscBus) ──► CanLink ──► dji-esc-socketcan +``` + +| 做对的事 | 证据 | +|---|---| +| 协议与 I/O 解耦 | `CanLink` = tokio mpsc 对；`motor-core`/`dji-esc` 零真实 I/O | +| 命令 / 反馈分离 | `Command` enum vs `MotorState` struct | +| 方向修正对称 | `EscConfig::direction=±1`，读写双向翻转（`bus.rs:163-203`） | +| 纯算法可测 | `Pid`（抗饱和、D-on-measurement、滤波）、`VelocityController` | +| 多圈累计 | `TurnTracker` 8192 编码器回绕（`turn.rs`） | +| 安全三件套 | 上电门控 / 反馈离线 0 电流 / 命令超时归零 | +| 仿真闭环 | `sim::spawn_plant` + `closed_loop.rs` + `--mock` 全平台可跑 | +| 运动学纯数学 | `mecanum` 正逆解 + proptest | +| 组合根 | `chassisd` 装配 + `TwistSource` seam | + +**结论：轮子（C620 电流环 + 主机速度环）路径是通的。** + +### 1.2 单一职责违规 / 抽象缺口（必须改） + +| # | 问题 | 位置 | SRP 违规 | +|---|---|---|---| +| G1 | `Motor` 无位置 API | `motor-core/src/lib.rs:51-79` | 能力面不足 | +| G2 | `MotorState.wheel_speed_rpm` | `lib.rs:32`, `bus.rs:171` | **底盘/减速比泄漏进协议无关层** | +| G3 | 无 `PositionController` | `motor-core` | 云台/拨弹阻塞 | +| G4 | 主机速度环写死在 `EscBus::compute_command` | `bus.rs:181-203` | **控制律与协议组帧/安全耦合** | +| G5 | `EscModel` 无 GM6020 | `codec.rs:23-30` | 电压协议族缺失 | +| G6 | `tx_loop` 硬编码 0x200/0x1FF 电流槽 | `bus.rs:369-388` | 协议族绑定过死 | +| G7 | `ChassisController` 依赖 `EscMotor` + 自己乘 gear | `controller.rs:25,83` | 应用层依赖协议 crate | +| G8 | `Command` 切模式不复位 PID | `bus.rs:192-199` | 拨弹速度↔位置积分冲击 | +| G9 | 无 `MockMotor` | — | 上层无法脱离 EscBus 测 | +| G10 | 配置写死恰好 4 电机 | `config.rs:181` | 无法扩设备段 | +| G11 | `CanFrame`/`CanLink` 在 `dji-esc::frame` | `frame.rs` | **传输被单个协议族占有** | +| G12 | 无航向/绝对角抽象 | — | 云台场地绝对角需 IMU 融合（用户要求预留） | + +### 1.3 CAN ID 问题 —— 由「双 CAN」架构决策消解 + +`0x1FF` / `0x205` 在 C620 与 GM6020 两族中含义不同（电流 vs 电压）。**用户选择双 CAN 总线**： + +| 总线 | 设备 | 协议 | ID | 命令帧 | +|---|---|---|---|---| +| **can0 底盘** | FL/FR/RL/RR 轮 | C620+GM3508 | 1–4 | `0x200` | +| **can1 功能** | 拨弹轮 | C610+M2006 | 1–4（本总线内） | `0x200` | +| **can1 功能** | 云台 | GM6020 | 1–4（本总线内） | `0x1FF` | + +- 每条 CAN 一个独立 `CanLink` + 一个协议族 bus；**物理隔离**后 ID 空间独立，无需避让。 +- 仍建议配置层按「每总线一协议族」校验，避免同总线混接时踩坑。 +- 组合根按接口名装配：`chassisd --interface can0 --gimbal-interface can1`（或配置 `[can]`/`[can.function]`）。 + +--- + +## 二、目标设计（单一职责） + +### 2.1 目标 crate / 模块树 + +``` +crates/ + motor-core/ # 协议无关电机域：能力 trait、状态、命令寄存器、控制算法、mock（零依赖、无 tokio） + src/ + lib.rs # 导出 + 单位/方向约定 + state.rs # MotorState（转子轴物理量，无 gear_ratio） + command.rs # Command + CommandMode + traits.rs # MotorFeedback / EffortDrive / VelocityDrive / PositionDrive / Disable + heading.rs # HeadingSource（IMU/绝对角融合 seam，新增） + pid.rs # Pid（保持） + velocity.rs # VelocityController（输出泛化为 effort_raw） + position.rs # PositionController（新增：Direct | Cascade） + gearbox.rs # OutputShaft：输出轴单位适配（新增） + testing.rs # MockMotor / MockHeading（新增） + can-link/ # CAN 传输：CanFrame + CanLink（新增；无协议知识） + dji-esc/ # C620/C610 电流协议族：编解码 + 总线调度/安全 + sim + dji-gm6020/ # GM6020 电压协议族（本次只留 seam 文档，实现=后续） + dji-esc-socketcan/ # SocketCAN 桥（只依赖 can-link；支持多接口） + examples/ + spin.rs # 保留：最小抽象演示 + wheel-bench.rs # 新增：实车台架单轮验证（分阶段自动） + mecanum/ # 纯运动学（不动） + chassisd/ # 组合根：配置 / 1kHz 底盘循环 / 安全 / --mock +docs/ + architecture.md # 更新分层图（双 CAN） + motor-abstraction.md # trait 契约与单位（新增） + bench-single-wheel.md# 台架流程（新增） +``` + +**裁剪原则**：只新增 `can-link` + `dji-gm6020` seam + `heading.rs` 模块。不拆 `motor-controllers`、暂不建 `gimbal`/`feeder` crate。 + +### 2.2 motor-core：能力拆分（小 trait） + +```rust +// traits.rs —— 能力面，可组合，静态分发 +pub trait MotorFeedback: Send + Sync { + fn id(&self) -> u8; + fn state(&self) -> MotorState; +} +pub trait EffortDrive: Send + Sync { + /// 开环 raw（ESC=电流，GM6020=电压）。 + fn set_effort_raw(&self, raw: i16); +} +pub trait VelocityDrive: Send + Sync { + /// 目标转子转速（rpm，物理方向）。 + fn set_velocity(&self, rotor_rpm: f32); +} +pub trait PositionDrive: Send + Sync { + /// 目标转子多圈位置（圈，物理方向）。 + fn set_position(&self, rotor_rev: f32); +} +pub trait Disable: Send + Sync { + fn disable(&self); +} + +/// 便利别名（blanket impl，零成本）。 +pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {} +impl Motor for T {} +``` + +**为什么不是胖 `Motor` trait**：能力是硬件事实。消费者只声明用到的能力： +- 底盘 → `VelocityDrive + MotorFeedback` +- 云台 → `PositionDrive + EffortDrive`（+ 上层 `HeadingSource`） +- 拨弹 → `VelocityDrive + PositionDrive` +- 台架 → `VelocityDrive + MotorFeedback + Disable` + +**`EffortDrive` 统一命名**：两族都是 i16 开环命令，仅物理量不同（A vs V）；协议 crate 文档化单位。 + +### 2.3 IMU / 绝对角融合 seam（用户要求） + +```rust +// heading.rs —— 不属于电机，属于「航向观测」 +/// 绝对航向观测源（IMU / 磁力计 / 视觉等）。 +pub trait HeadingSource: Send { + /// 机体航向角（rad，连续；定义与 chassis ISO 约定一致）。 + fn heading_rad(&self) -> impl Future + Send; +} + +/// 云台目标角的参考系。 +pub enum HeadingFrame { + /// 电机转子多圈（上电相对）。 + RotorMultiTurn, + /// 场地绝对角（经 HeadingSource 对齐）。 + AbsoluteField, +} +``` + +- **motor 层只保证 `rotor_revolutions` 连续**；场地绝对角由未来 `gimbal` crate 融合 IMU（`HeadingSource`）与电机多圈。 +- `PositionDrive::set_position` 仍是转子圈；绝对角 → 转子圈的映射在云台控制器，不在 motor-core。 +- `MockHeading` 放 `testing.rs`。 + +### 2.4 `Command` / `MotorState` + +```rust +pub enum Command { + Disabled, + EffortRaw(i16), // 原 CurrentRaw + Velocity(f32), // rotor rpm + Position(f32), // rotor revolutions（圈，连续） +} +pub enum CommandMode { Disabled, EffortRaw, Velocity, Position } + +pub struct MotorState { + pub rotor_angle_raw: u16, + pub rotor_revolutions: f32, + pub rotor_speed_rpm: f32, + pub effort_raw: i16, // 原 torque_current_raw + pub temperature_c: i8, + pub feedback_count: u64, +} +// 删除 wheel_speed_rpm（G2） +``` + +**模式切换复位**（G8）：`compute_effort` 在 `cmd.mode()` 变化时 reset 控制器。 + +### 2.5 motor-core：控制算法 + +| 类型 | 模式 | 输入 → 输出 | +|---|---|---| +| `Pid` | — | setpoint, meas, dt → f32 | +| `VelocityController` | — | target_rpm, meas_rpm, dt → effort_raw | +| `PositionController` **新增** | `Direct(Pid)` | pos_rev, meas_rev, dt → effort_raw（云台电压） | +| | `Cascade{pos, vel}` | 位置→限速→速度环→effort_raw（拨弹定量） | + +控制律纯函数放 motor-core；**执行节拍/安全/方向/限幅**留在各协议族 bus。 + +### 2.6 gearbox：`OutputShaft` + +```rust +/// 输出轴单位 ↔ 转子单位。gear_ratio = 转子 : 输出轴。 +pub struct OutputShaft { inner: T, gear_ratio: f32 } +impl VelocityDrive for OutputShaft { + fn set_velocity(&self, output_rpm: f32) { + self.inner.set_velocity(output_rpm * self.gear_ratio); + } +} +// Position 同理；Feedback/Effort/Disable 透传 +``` + +### 2.7 can-link（新 crate） + +`CanFrame`、`CanLink`、`CanLink::pair`。**不含 DJI ID 常量**。`motor-core` 保持无 tokio。 + +### 2.8 dji-esc（电流协议族） + +- `EscConfig::m2006(name, esc_id, direction)`（C610、36:1 待核）。 +- 算法改调 motor-core；`compute_effort` 加 `Position` 分支 + 模式 reset。 +- 电流槽分组保持（协议事实）。 + +### 2.9 dji-gm6020（设计 seam，不实机） + +- 实现 5 个能力 trait。 +- 命令 `0x1FF`(ID1–4)/`0x2FF`(ID5–7) 电压槽；反馈 `0x204+id`；`TurnTracker` 复用。 +- 云台位置环：`PositionController` → voltage raw。 +- 绝对角：未来 `gimbal` crate 用 `HeadingSource` + `HeadingFrame::AbsoluteField` 对齐到 `rotor_revolutions` 基准后再 `set_position`。 + +### 2.10 拨弹 M2006/C610 派生路径（设计，不实现） + +- 复用 `dji-esc` + `EscConfig::m2006`；定量 = `Position` + Cascade；连续 = `Velocity`。 +- FSM 在上层，不在 motor-core。 + +### 2.11 chassisd（组合根） + +```rust +pub struct ChassisController { + kinematics: Mecanum, + wheels: [W; 4], + limits: (f32, f32, f32), + command_timeout: Duration, +} +impl ChassisController { ... } +``` + +- **不 import `dji_esc`**。 +- 配置：底盘 `[can]` + 四轮；预留 `[can.function]` / `[gimbal]` / `[feeder]` seam 注释（双 CAN 接口名）。 +- 不写云台/拨弹逻辑。 + +--- + +## 三、实车台架单轮验证（小车轮子测试） + +### 3.1 工具：`wheel-bench`（分阶段自动跑） + +```sh +cargo run --release -p dji-esc-socketcan --example wheel-bench -- \ + --interface can0 --esc-id 1 --direction 1 --gear-ratio 19.203 \ + --current-limit 4000 --rpm 300 --secs 3 +``` + +一条命令顺序执行 stage 0–6，输出 CSV + 结论；ctrl-c 任意点安全停机。 + +| Stage | 动作 | 观察 | 通过标准 | +|---|---|---|---| +| 0 Link | `disable()` | 反馈帧率 ~1kHz、温度 | 2s 内有反馈 | +| 1 Direction | `set_effort_raw(±limit/4)` 点动 | `rotor_speed_rpm` 符号 | 与 `direction`/前进约定一致 | +| 2 Vel step | `set_velocity(+target)` | 超调、稳态误差 | 1s 后 \|err\|<5% | +| 3 gear_ratio | 对比转子 rpm 与手测轮端 | 比值 | ≈19.203 ±3% | +| 4 Reverse | `set_velocity(-target)` | 对称性 | 对称 | +| 5 Disable | `disable()` | rpm→0 | 停转 | +| 6 Offline（可选） | 运行中断电调 | 离线强制 0 电流 | 无重启冲击 | + +安全内建：默认低限流、总时限、退出路径 `disable()+50ms` 刷帧、无反馈拒启动、单电机、打印「架空禁止落地」。保留 `spin.rs`。 + +### 3.2 流程（`docs/bench-single-wheel.md`） + +1. **架空车轮**，急停可及；`ip link set can0 up type can bitrate 1000000 restart-ms 100`。 +2. 首次 `--current-limit 3000~5000`。 +3. 一次只驱一台；顺序 FL→FR→RL→RR。 +4. 跑完自动 stage；方向错**改配置 `direction`**。 +5. 记录 `direction` 终值、实测 gear_ratio、PID 备注。 +6. 填表归档后才考虑落地四轮联动（不在本次）。 + +**通过标准**：反馈无连续丢帧；方向一致；无持续振荡、稳态 <5%；减速比偏差 <3%；`disable`/离线停转、电流 0。 + +--- + +## 四、迁移步骤（每步可编译、测试绿） + +| 步 | 内容 | +|---|---| +| M0 | 基线 `cargo test --workspace && just lint` | +| M1 | 新建 `can-link`；`dji-esc::frame` re-export 兼容 | +| M2 | `dji-esc-socketcan` 改依赖 `can-link`；收缩 `frame.rs` | +| M3 | motor-core 拆 `state/command/traits/heading`；**一次破坏性重命名** `CurrentRaw→EffortRaw`、`torque_current_raw→effort_raw`；删 `wheel_speed_rpm` | +| M4 | 能力 trait 落地；`EscMotor` 实现；`Motor` blanket 别名 | +| M5 | `OutputShaft`；`ChassisController` 泛型化；daemon 测试 gear 换算 | +| M6 | `Command::Position` + `CommandMode` 模式 reset；`PositionController` + 单测；`dji-esc` Position 分支 | +| M7 | `MockMotor`/`MockHeading`；改写 1 个 chassis 单测证明 seam | +| M8 | `EscConfig::m2006()`；双 CAN 配置 seam + 每总线单协议族校验 | +| M9 | `wheel-bench`（分阶段自动）+ `docs/bench-single-wheel.md` | +| M10 | 文档：`architecture.md`（双 CAN 图）、`motor-abstraction.md`、README | +| M11（后续） | `dji-gm6020` codec+bus+sim | +| M12（后续） | `gimbal` crate：`HeadingSource` 融合 + 云台控制 | + +--- + +## 五、关键文件 + +**新建** + +- `crates/can-link/{Cargo.toml,src/lib.rs}` +- `crates/motor-core/src/{state,command,traits,heading,position,gearbox,testing}.rs` +- `crates/dji-esc-socketcan/examples/wheel-bench.rs` +- `docs/{motor-abstraction,bench-single-wheel}.md` + +**修改** + +- `Cargo.toml`（workspace） +- `crates/motor-core/src/{lib,velocity}.rs` +- `crates/dji-esc/src/{lib,bus,codec,sim}.rs`、`tests/closed_loop.rs` +- `crates/dji-esc-socketcan/src/bridge.rs`、`examples/spin.rs`、`Cargo.toml` +- `crates/chassisd/src/{controller,config,main}.rs`、`tests/daemon.rs` +- `config/chassis.toml.example`、`docs/architecture.md`、`README.md`、`justfile` + +**删除 / 迁出** + +- `crates/dji-esc/src/frame.rs`（类型迁 can-link） +- `MotorState.wheel_speed_rpm`、`Command::CurrentRaw`、`torque_current_raw` + +--- + +## 六、验证 + +```sh +cargo test --workspace +just lint +cargo run -p chassisd -- --mock +just vcan-test +just build-armv7 +cargo build --release -p dji-esc-socketcan --example wheel-bench +cargo build --release -p dji-esc-socketcan --example spin +# 实车台架见 §3 +``` + +**Definition of Done** + +1. `cargo test --workspace` + `just lint` 绿。 +2. `chassisd --mock` 行为不变。 +3. `rg 'wheel_speed_rpm|CurrentRaw|torque_current_raw'` 为空。 +4. `rg 'dji_esc' crates/chassisd/src/controller.rs` 为空。 +5. `motor-core` 无运行时依赖。 +6. `wheel-bench` + 台架文档就位；实车四轮台架表归档。 +7. `HeadingSource` seam + 双 CAN 配置 seam 就位；**`chassisd` 无云台/拨弹逻辑**。 + +--- + +## 七、风险 + +| 风险 | 缓解 | +|---|---| +| 双 CAN 板端接口（can1）是否可用 | 配置可单 CAN 起步（只填 `[can]`）；功能段可选 | +| GM6020 电压/反馈标度未核手册 | 常量隔离在 `dji-gm6020::codec`；上电前台架核对 | +| C610/M2006 36:1？±10000↔±10A | `m2006()` 标注接入前核对 | +| `OutputShaft` 单位混淆 | 参数名 `output_rpm`/`output_rev` + 文档 | +| 删 `wheel_speed_rpm` 破坏日志/测试 | M3/M5 同步改 | +| 模式切换 reset 惊吓 | 文档 + `debug!` | +| `HeadingSource` 的异步形态是否合适 | 先定 trait 契约；M12 可按 IMU 驱动改 `async fn` / watch | +| 同总线误接混协议 | 配置校验「每总线一协议族」+ 文档警告 | + +--- + +## 八、不在本次范围 + +- 云台 / 拨弹控制逻辑与整定 +- GM6020 实机驱动完整实现（M11） +- IMU 驱动与航向融合实现（M12，仅 seam） +- 四轮落地联动测试 +- 遥控器 / 导航 TwistSource diff --git a/.zcode/plans/plan-sess_5bb01b23-baf7-4632-be6e-f8a5bf9ab890.md b/.zcode/plans/plan-sess_5bb01b23-baf7-4632-be6e-f8a5bf9ab890.md new file mode 100644 index 0000000..ce47476 --- /dev/null +++ b/.zcode/plans/plan-sess_5bb01b23-baf7-4632-be6e-f8a5bf9ab890.md @@ -0,0 +1,75 @@ +# 麦轮底盘仓库基础搭建方案（Linux ARM 主控 + 4×GM3508/C620） + +## 已确认决策 +- **主控**：Linux ARM 板，CAN 经 **SocketCAN (can0)** 接入（USB转CAN / CAN HAT） +- **并发模型**：tokio 异步 +- **架构**：5 crate 分层，通用职能命名（不含仓库名 sentry-chassis，无 rm- 前缀；五个名字已在 crates.io 确认全部未占用） + +## 关键协议事实（编码实现依据，来自 C620 官方手册调研） +- 控制：ID **0x200** 携带电调 1–4 的电流命令，**0x1FF** 携带 5–8；每电机 i16 **大端**，±16384 ↔ ±20A（C620） +- 反馈：ID **0x200+电调ID**（0x201–0x208），8 字节 = 机械角(0..8191, u16) + 转速 rpm(i16) + 转矩电流(i16) + 温度℃(i8) +- C620 **只闭电流环**，速度环/位置环须在主控侧用 PID 实现；GM3508 减速比 3591/187 ≈ 19.2 + +## 仓库结构（Cargo workspace，虚拟清单，删除现有 src/main.rs） +``` +sentry-chassis/ +├── Cargo.toml # [workspace] resolver="3" + workspace.package/dependencies/lints +├── rust-toolchain.toml # stable + rustfmt/clippy 组件 +├── rustfmt.toml / clippy.toml +├── justfile # lint / test / vcan-test / run-mock +├── .github/workflows/ci.yml +├── README.md / LICENSE(MIT) / docs/architecture.md / deploy/chassisd.service +└── crates/ + ├── motor-core/ # 电机抽象层（纯逻辑，无 IO） + ├── dji-esc/ # C620/C610 协议层（帧编解码 + 异步驱动，纯逻辑可全平台测） + ├── dji-esc-socketcan/ # SocketCAN 后端（Linux only，非 Linux 编译为空壳） + ├── mecanum/ # 麦轮正逆运动学（纯数学） + └── chassisd/ # 守护进程二进制（组装一切，支持 --mock 无硬件运行） +``` + +## 各 crate 设计 + +### 1. `motor-core` — 抽象与控制（依赖：仅 tokio/thiserror） +- `MotorState`：转子原始角、连续累计角(多圈)、转子 rpm、电流、温度、减速比换算的轮端速度 +- `Motor` trait：`id() / state() / set_velocity(rpm) / set_current_raw(i16) / disable()`，句柄可克隆、内部共享状态，供 chassisd 与测试用 mock 泛化 +- `Pid`（f32，显式 dt）：输出限幅、积分抗饱和(clamping)、测量值微分 + 一阶低通滤波 +- `VelocityController`：速度环封装（由驱动层按反馈节拍调用） + +### 2. `dji-esc` — DJI 电调协议（依赖 motor-core + tokio） +- 自带最小 `CanFrame{ id, data:[u8;8] }` 与异步 `CanPort` trait（send/recv）——不依赖 socketcan，测试用 channel 实现 +- 纯函数编解码：`encode_current_frame`（0x200/0x1FF 分组、大端、饱和 ±16384）、`decode_feedback`（0x201–0x208），C620/C610 双型号量程 +- `TurnTracker`：0..8191 → 连续多圈角（正反向回绕处理） +- `EscBus`：spawn 收发任务；rx 分发反馈到各 `C620Motor` 句柄（含方向翻转、速度 PID tick），tx 每 1kHz 汇装两帧控制报文；`EscConfig{ esc_id, 型号, 减速比, 方向, 电流限幅 }` +- 反馈超时检测（离线 → 电流清零） + +### 3. `dji-esc-socketcan` — SocketCAN 后端（依赖 dji-esc + socketcan v4 "tokio" feature） +- `SocketCanPort::open("can0")` 实现 `CanPort`；socketcan Frame ↔ CanFrame 转换 +- crate 主体 `#[cfg(target_os="linux")]`，macOS 上编译为空壳 → 全 workspace 可在 mac 开发机直接 build/test + +### 4. `mecanum` — 麦轮运动学（纯数学，零依赖） +- `Mecanum{ lx, ly, wheel_radius }`，ISO 约定（x前 y左 ω逆正，45°辊子） +- `inverse(Twist) -> [4] 轮角速度`（FL/FR/RL/RR）、`forward([4]) -> Twist`（伪逆）、rpm↔rad/s 换算 + +### 5. `chassisd` — 守护进程（依赖以上全部 + clap/serde/toml/tracing） +- CLI：`--interface can0 --config chassis.toml --mock` +- `--mock`：进程内 CanPort + 一阶电机模型，**无硬件/macOS 可跑通全链路** +- 1kHz 控制循环：Twist → mecanum 逆解 → 4 路 `set_velocity`；`TwistSource` trait（当前 Zero/Mock 实现，遥控器后续接入） +- 安全：命令超时清零、反馈离线清零、上电等首帧反馈后才使能；tracing 日志、ctrl-c 优雅退出、systemd 部署样例 + +## 底层库配置（workspace.dependencies 统一管理） +tokio(rt-multi-thread,macros,sync,time) / socketcan v4+tokio（仅 Linux target 依赖；若 4.0 有问题回退 3.6.2）/ thiserror / tracing + tracing-subscriber / serde+toml（配置） / clap（bin） / proptest+approx（dev）。`[workspace.lints]`：clippy pedantic 子集 + 库 crate 禁 unwrap/expect。 + +## 充分测试策略 +1. **单元（全平台）**：编解码官方字节级样例与边界值（±16384 饱和、负值、全部 ID 路由）、多圈回绕序列、PID（阶跃/限幅/抗饱和恢复/微分滤波）、运动学（纯平移/纯旋转/对角线/零点 + proptest 往返一致性）、配置解析 +2. **闭环仿真（全平台）**：mock CanPort + 一阶电机模型（电流→rpm 增益与限速），4 电机速度环收敛、群帧组装、离线安全、`--mock` 守护进程全链路（tokio::time::pause 控时） +3. **vcan 集成（Linux 专属，#[ignore] + CI 显式跑）**：vcan0 上双 socket 收发，真实内核 CAN 栈回环验证 +4. **CI（GitHub Actions）**：fmt --check / clippy -D warnings / test / doc / vcan job（modprobe vcan）/ **aarch64-unknown-linux-gnu 交叉 check**（目标板架构）/ llvm-cov 覆盖率 + +## 实施步骤 +1. 工作区脚手架：根 Cargo.toml（虚拟清单）、toolchain/lint/fmt 配置、删 src/main.rs、README、docs、justfile、CI、LICENSE +2. `motor-core`：类型 + Motor trait + PID + 单测 +3. `dji-esc`：CanFrame/CanPort、编解码、TurnTracker、EscBus 异步驱动、闭环仿真测试 +4. `dji-esc-socketcan`：CanPort 实现 + vcan 集成测试 +5. `mecanum`：正逆解 + proptest +6. `chassisd`：配置/CLI/mock 模式/控制循环/安全逻辑 + 测试、deploy 样例 +7. 本机验证：cargo fmt / clippy / test 全绿（macOS 上 Linux 专属测试自动跳过） \ No newline at end of file diff --git a/.zcodeignore b/.zcodeignore new file mode 100644 index 0000000..a331a6a --- /dev/null +++ b/.zcodeignore @@ -0,0 +1,33 @@ +/target + +# ===== ↑ 以上同步自 .gitignore（「从 .gitignore 同步」只重写以上部分）===== +.git/ +.hg/ +.svn/ +node_modules/ +bower_components/ +jspm_packages/ +__pycache__/ +site-packages/ +venv/ +coverage/ +htmlcov/ +lcov-report/ +cmakefiles/ +cmake-build-*/ +bazel-*/ +pods/ +deriveddata/ +storybook-static/ +playwright-report/ +test-results/ +allure-results/ +allure-report/ +cdk.out/ +*.egg-info/ +*.dist-info/ +eggs/ +pip-wheel-metadata/ +wheels/ +# ----- ↑ 以上为 ZCode 默认排除规则（自定义规则请写在本行下方，不会被同步/恢复改动）----- +# 自定义规则写在下方（本行提示可删除） diff --git a/Cargo.lock b/Cargo.lock new file mode 100644 index 0000000..24a9a6f --- /dev/null +++ b/Cargo.lock @@ -0,0 +1,1226 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "aho-corasick" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c982642fa9e8606056828ee9a8505737230110bb1099153c79efe865c59d12ba" +dependencies = [ + "memchr", +] + +[[package]] +name = "anstream" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "824a212faf96e9acacdbd09febd34438f8f711fb84e09a8916013cd7815ca28d" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "940b3a0ca603d1eade50a4846a2afffd5ef57a9feac2c0e2ec2e14f9ead76000" + +[[package]] +name = "anstyle-parse" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "52ce7f38b242319f7cabaa6813055467063ecdc9d355bbb4ce0c68908cd8130e" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys", +] + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "autocfg" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" + +[[package]] +name = "bit-set" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "08807e080ed7f9d5433fa9b275196cfc35414f66a0c79d864dc51a0d825231a3" +dependencies = [ + "bit-vec", +] + +[[package]] +name = "bit-vec" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5e764a1d40d510daf35e07be9eb06e75770908c27d411ee6c92109c9840eaaf7" + +[[package]] +name = "bitflags" +version = "2.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ded4057c258ba199e2d26386d3af3780957ecaee6c4ef4041c6b4b8b97c0b06" + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "can-link" +version = "0.1.0" +dependencies = [ + "tokio", +] + +[[package]] +name = "cfg-if" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7648175b45a9a48536d676f68d918270699102aa8dab5496df06904c914600" + +[[package]] +name = "cfg_aliases" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f079e83a288787bcd14a6aea84cee5c87a67c5a3e660c30f557a3d24761b3527" + +[[package]] +name = "chassisd" +version = "0.1.0" +dependencies = [ + "approx", + "clap", + "dji-esc", + "dji-esc-socketcan", + "mecanum", + "motor-core", + "proptest", + "serde", + "thiserror", + "tokio", + "toml", + "tracing", + "tracing-subscriber", +] + +[[package]] +name = "clap" +version = "4.6.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa8876b300ab35ba921adea3dfd70157a46249b33f95c9084ae5709785478946" +dependencies = [ + "clap_builder", + "clap_derive", +] + +[[package]] +name = "clap_builder" +version = "4.6.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0797fb7aeb1406c84efac526901f7ec3ead2124f946b494e72879d4b54704d" +dependencies = [ + "anstream", + "anstyle", + "clap_lex", + "strsim", +] + +[[package]] +name = "clap_derive" +version = "4.6.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9c751b79415d4e559e3d1fcf128e09e720eb673a06d26cf6f392d37d75b66e0" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "clap_lex" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1c133bc6a41be0d194c306b5506d15e6feeea7b1d6604bd3f8310dfb2ca96486" + +[[package]] +name = "colorchoice" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d07550c9036bf2ae0c684c4297d503f838287c83c53686d05370d0e139ae570" + +[[package]] +name = "darling" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc7f46116c46ff9ab3eb1597a45688b6715c6e628b5c133e288e709a29bcb4ee" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d00b9596d185e565c2207a0b01f8bd1a135483d02d9b7b0a54b11da8d53412e" +dependencies = [ + "fnv", + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.119", +] + +[[package]] +name = "darling_macro" +version = "0.20.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc34b93ccb385b40dc71c6fceac4b2ad23662c7eeb248cf10d529b7e055b6ead" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "derive_builder" +version = "0.20.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "507dfb09ea8b7fa618fcf76e953f4f5e192547945816d5358edffe39f6f94947" +dependencies = [ + "derive_builder_macro", +] + +[[package]] +name = "derive_builder_core" +version = "0.20.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d5bcf7b024d6835cfb3d473887cd966994907effbe9227e8c8219824d06c4e8" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "derive_builder_macro" +version = "0.20.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ab63b0e2bf4d5928aff72e83a7dace85d7bba5fe12dcc3c5a572d78caffd3f3c" +dependencies = [ + "derive_builder_core", + "syn 2.0.119", +] + +[[package]] +name = "dji-esc" +version = "0.1.0" +dependencies = [ + "can-link", + "motor-core", + "thiserror", + "tokio", + "tracing", +] + +[[package]] +name = "dji-esc-socketcan" +version = "0.1.0" +dependencies = [ + "can-link", + "dji-esc", + "motor-core", + "socketcan", + "tokio", + "tracing", +] + +[[package]] +name = "either" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "252afb9ae5eaa683babdc6a068b3f5726eb19e05070c731f9b2a23a7c3e8ed34" + +[[package]] +name = "embedded-can" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e9d2e857f87ac832df68fa498d18ddc679175cf3d2e4aa893988e5601baf9438" +dependencies = [ + "nb", +] + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys", +] + +[[package]] +name = "fastrand" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da7c62ceae207dd37ea5b845da6a0696c799f85e97da1ab5b7910be3c1c80223" + +[[package]] +name = "fnv" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f9eec918d3f24069decb9af1554cad7c880e2da24a9afd88aca000531ab82c1" + +[[package]] +name = "futures" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a31d2a3fbaaeb2af2368bbdd904aa8e812d3c04a1ee10d3171f52d556e5d0a3" +dependencies = [ + "futures-channel", + "futures-core", + "futures-executor", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1f9e3d69d39e4862ffed03ed071a76f9a13ba1d9109d355b0f0aa6b15e393c4" +dependencies = [ + "futures-core", + "futures-sink", +] + +[[package]] +name = "futures-core" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92d699e522242e69e3003b94ecc1f960f3a5e015aa7c5d7486e65ad01dd94f5e" + +[[package]] +name = "futures-executor" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "031b47cf1a3c6cc8bc2fc76cd437f521619387907d469316e7c0bc278f1f5432" +dependencies = [ + "futures-core", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-io" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53c0fa8157de1303bfffdaa1cc2a673bfffb60102f76b0ef4441659124373fed" + +[[package]] +name = "futures-macro" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fb9654ba8355388abeb8dcb4fc62f511300867002afc858860463bdd9fe0c44" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "futures-sink" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1944426bf7d03f1d14f708785e4b33efd750b36d48a157b836b3efc15ede8e1d" + +[[package]] +name = "futures-task" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd417de3d1d015fc3bfd2b1ea46dfc7bab72ef86f1cc7cc9c78e728b34a6d1fd" + +[[package]] +name = "futures-util" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d50a92467f8ba5dd6e3ee5d4bd04d73ab2e4e1c44474a0674821dfce14b79bc" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-macro", + "futures-sink", + "futures-task", + "memchr", + "pin-project-lite", + "slab", +] + +[[package]] +name = "getrandom" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi 5.3.0", + "wasip2", +] + +[[package]] +name = "getrandom" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "300e883d756b2e4ec94e02791f39b04b522276138852cfc41d9fb7e904106099" +dependencies = [ + "cfg-if", + "libc", + "r-efi 6.0.0", +] + +[[package]] +name = "getset" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6cf442baaabe4213ce7d1239afc26c039180b6456da2cededa316ae2c8a77a77" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "hashbrown" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" + +[[package]] +name = "heck" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2304e00983f87ffb38b55b444b5e3b60a884b5d30c0fca7d82fe33449bbe55ea" + +[[package]] +name = "hex" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70" + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "indexmap" +version = "2.14.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc4e190f5d26ca7051642629da2c52fc03bde85a03197c99408dcd291734c855" +dependencies = [ + "equivalent", + "hashbrown", +] + +[[package]] +name = "is_terminal_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "libc" +version = "0.2.189" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3eaf3ede3fee6db1a4c2ee091bf8a8b4dccdc6d17f656fb07896ee72867612f2" + +[[package]] +name = "linux-raw-sys" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a66949e030da00e8c7d4434b251670a91556f4144941d37452769c25d58a53" + +[[package]] +name = "lock_api" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "224399e74b87b5f3557511d98dff8b14089b3dadafcab6bb93eab67d3aace965" +dependencies = [ + "scopeguard", +] + +[[package]] +name = "log" +version = "0.4.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9f8bd3e56ce4dfc153cf470fffbfa98c7620958b312ca5c3a4b8d5181fd13c6" + +[[package]] +name = "matchers" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9" +dependencies = [ + "regex-automata", +] + +[[package]] +name = "mecanum" +version = "0.1.0" +dependencies = [ + "approx", + "proptest", +] + +[[package]] +name = "memchr" +version = "2.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf8baf1c55e62ffcace7a9f06f4bd9cd3f0c4beb022d3b367256b91b87513d98" + +[[package]] +name = "memoffset" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "488016bfae457b036d996092f6cb448677611ce4449e970ceaf42695203f218a" +dependencies = [ + "autocfg", +] + +[[package]] +name = "mio" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4b18443e9c262bfe8fa82f51666e2642c53393f7e5c27b3e1aeab922cff5b9d8" +dependencies = [ + "libc", + "wasi", + "windows-sys", +] + +[[package]] +name = "motor-core" +version = "0.1.0" + +[[package]] +name = "nb" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" + +[[package]] +name = "neli" +version = "0.7.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22f9786d56d972959e1408b6a93be6af13b9c1392036c5c1fafa08a1b0c6ee87" +dependencies = [ + "bitflags", + "byteorder", + "derive_builder", + "getset", + "libc", + "log", + "neli-proc-macros", + "parking_lot", +] + +[[package]] +name = "neli-proc-macros" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05d8d08c6e98f20a62417478ebf7be8e1425ec9acecc6f63e22da633f6b71609" +dependencies = [ + "either", + "proc-macro2", + "quote", + "serde", + "syn 2.0.119", +] + +[[package]] +name = "nix" +version = "0.31.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf20d2fde8ff38632c426f1165ed7436270b44f199fc55284c38276f9db47c3d" +dependencies = [ + "bitflags", + "cfg-if", + "cfg_aliases", + "libc", + "memoffset", +] + +[[package]] +name = "nu-ansi-term" +version = "0.50.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +dependencies = [ + "windows-sys", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "once_cell_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" + +[[package]] +name = "parking_lot" +version = "0.12.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93857453250e3077bd71ff98b6a65ea6621a19bb0f559a85248955ac12c45a1a" +dependencies = [ + "lock_api", + "parking_lot_core", +] + +[[package]] +name = "parking_lot_core" +version = "0.9.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2621685985a2ebf1c516881c026032ac7deafcda1a2c9b7850dc81e3dfcb64c1" +dependencies = [ + "cfg-if", + "libc", + "redox_syscall", + "smallvec", + "windows-link", +] + +[[package]] +name = "pin-project-lite" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" + +[[package]] +name = "ppv-lite86" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85eae3c4ed2f50dcfe72643da4befc30deadb458a9b590d720cde2f2b1e97da9" +dependencies = [ + "zerocopy", +] + +[[package]] +name = "proc-macro2" +version = "1.0.107" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "985e7ec9bb745e6ce6535b544d84d6cd6f7ad8bd711c398938ae983b91a766d9" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "proptest" +version = "1.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4b45fcc2344c680f5025fe57779faef368840d0bd1f42f216291f0dc4ace4744" +dependencies = [ + "bit-set", + "bit-vec", + "bitflags", + "num-traits", + "rand", + "rand_chacha", + "rand_xorshift", + "regex-syntax", + "rusty-fork", + "tempfile", + "unarray", +] + +[[package]] +name = "quick-error" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" + +[[package]] +name = "quote" +version = "1.0.47" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fbf4db142a473a8d80c26bbf18454ed458bf8d26c8219c331daecfdbd079001" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r-efi" +version = "5.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" + +[[package]] +name = "r-efi" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8dcc9c7d52a811697d2151c701e0d08956f92b0e24136cf4cf27b57a6a0d9bf" + +[[package]] +name = "rand" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9ef1d0d795eb7d84685bca4f72f3649f064e6641543d3a8c415898726a57b41" +dependencies = [ + "rand_chacha", + "rand_core", +] + +[[package]] +name = "rand_chacha" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d3022b5f1df60f26e1ffddd6c66e8aa15de382ae63b3a0c1bfc0e4d3e3f325cb" +dependencies = [ + "ppv-lite86", + "rand_core", +] + +[[package]] +name = "rand_core" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "76afc826de14238e6e8c374ddcc1fa19e374fd8dd986b0d2af0d02377261d83c" +dependencies = [ + "getrandom 0.3.4", +] + +[[package]] +name = "rand_xorshift" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "513962919efc330f829edb2535844d1b912b0fbe2ca165d613e4e8788bb05a5a" +dependencies = [ + "rand_core", +] + +[[package]] +name = "redox_syscall" +version = "0.5.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed2bf2547551a7053d6fdfafda3f938979645c44812fbfcda098faae3f1a362d" +dependencies = [ + "bitflags", +] + +[[package]] +name = "regex-automata" +version = "0.4.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ad8553b9b26413251cbf30e620595c7a41b3887f03da04579c0e6b0d6a06b4b2" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d6f6ff9a378485b298a5286656da665ba74413d36db0979633275d2e708145d4" + +[[package]] +name = "rustix" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "891efababe418670775f199f0d233d84843c227a0949a883ce15b37c78d6629d" +dependencies = [ + "bitflags", + "errno", + "libc", + "linux-raw-sys", + "windows-sys", +] + +[[package]] +name = "rusty-fork" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc6bf79ff24e648f6da1f8d1f011e9cac26491b619e6b9280f2b47f1774e6ee2" +dependencies = [ + "fnv", + "quick-error", + "tempfile", + "wait-timeout", +] + +[[package]] +name = "scopeguard" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" + +[[package]] +name = "serde" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4148590afebada386688f18773da617792bf2ef03ffc1e4cbd2b1d45b023e0ba" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67dca2c9c51e58a4791a4b1ed58308b39c64224d349a935ab5039aa360942a48" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7a5d71263a5a7d47b41f6b3f06ba276f10cc18b0931f1799f710578e2309348" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "serde_spanned" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6662b5879511e06e8999a8a235d848113e942c9124f211511b16466ee2995f26" +dependencies = [ + "serde_core", +] + +[[package]] +name = "sharded-slab" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "signal-hook-registry" +version = "1.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4db69cba1110affc0e9f7bcd48bbf87b3f4fc7c61fc9155afd4c469eb3d6c1b" +dependencies = [ + "errno", + "libc", +] + +[[package]] +name = "slab" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" + +[[package]] +name = "smallvec" +version = "1.16.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba467056f1b547ed52077911161fc86985becbc60e8e1857c8a144dab0def891" + +[[package]] +name = "socket2" +version = "0.6.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3d1e2c7f27f8d4cb10542a02c49005dbd6e93095799d6f3be745fae9f8fedd4" +dependencies = [ + "libc", + "windows-sys", +] + +[[package]] +name = "socketcan" +version = "4.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "472e000403167b1e2f65cfcf8ff235fe94b911f9bd9bdbc1f48047b8a1cf49aa" +dependencies = [ + "bitflags", + "embedded-can", + "futures", + "hex", + "libc", + "log", + "nb", + "neli", + "nix", + "smallvec", + "socket2", + "thiserror", + "tokio", +] + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "syn" +version = "2.0.119" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "872831b642d1a07999a962a351ed35b955ea2cfc8f3862091e2a240a84f17297" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "3.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8593e8e72159ed2257d083c7a454a85cbf854f37a0966d8d483aff8c8a3ebcee" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "tempfile" +version = "3.27.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32497e9a4c7b38532efcdebeef879707aa9f794296a4f0244f6f69e9bc8574bd" +dependencies = [ + "fastrand", + "getrandom 0.4.3", + "once_cell", + "rustix", + "windows-sys", +] + +[[package]] +name = "thiserror" +version = "2.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec86235f5fcc2a73650310756d2ac5b138a5780bbbdfae3eeccec992c435ba4f" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc04cd3e1236dd4a98afca4569f2deb3f120e5422a4023be2cb683f8486292af" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "thread_local" +version = "1.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ad99c4c6d32803332c548b1af0540b357b3f5fc0be8f6c6bfe8b2e6ae784070" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "tokio" +version = "1.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "202caea871b69668250d242070849eb495be178ed697a3e98aebce5bc81a0bed" +dependencies = [ + "libc", + "mio", + "pin-project-lite", + "signal-hook-registry", + "socket2", + "tokio-macros", + "windows-sys", +] + +[[package]] +name = "tokio-macros" +version = "2.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78773a2a397f451582ce068015985c33193cf6dea8b74d2a639fe457b2f07b0e" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "toml" +version = "0.9.12+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf92845e79fc2e2def6a5d828f0801e29a2f8acc037becc5ab08595c7d5e9863" +dependencies = [ + "indexmap", + "serde_core", + "serde_spanned", + "toml_datetime", + "toml_parser", + "toml_writer", + "winnow 0.7.15", +] + +[[package]] +name = "toml_datetime" +version = "0.7.5+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92e1cfed4a3038bc5a127e35a2d360f145e1f4b971b551a2ba5fd7aedf7e1347" +dependencies = [ + "serde_core", +] + +[[package]] +name = "toml_parser" +version = "1.1.3+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d38ac1cf9b95face32296c0a3ede1fdc270627c9d9c02a7274dd6d960dc4d56" +dependencies = [ + "winnow 1.0.4", +] + +[[package]] +name = "toml_writer" +version = "1.1.2+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d56353a2a665ad0f41a421187180aab746c8c325620617ad883a99a1cbe66d2" + +[[package]] +name = "tracing" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" +dependencies = [ + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "tracing-core" +version = "0.1.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" +dependencies = [ + "once_cell", + "valuable", +] + +[[package]] +name = "tracing-log" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3" +dependencies = [ + "log", + "once_cell", + "tracing-core", +] + +[[package]] +name = "tracing-subscriber" +version = "0.3.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319" +dependencies = [ + "matchers", + "nu-ansi-term", + "once_cell", + "regex-automata", + "sharded-slab", + "smallvec", + "thread_local", + "tracing", + "tracing-core", + "tracing-log", +] + +[[package]] +name = "unarray" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" + +[[package]] +name = "unicode-ident" +version = "1.0.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d245f478577f809a851594d02313b640fb437e0bb33866753cff937863096954" + +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + +[[package]] +name = "valuable" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65" + +[[package]] +name = "wait-timeout" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ac3b126d3914f9849036f826e054cbabdc8519970b8998ddaf3b5bd3c65f11" +dependencies = [ + "libc", +] + +[[package]] +name = "wasi" +version = "0.11.1+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" + +[[package]] +name = "wasip2" +version = "1.0.4+wasi-0.2.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b67efb37e106e55ce722a510d6b5f9c17f083e5fc79afc2badeb12cc313d9487" +dependencies = [ + "wit-bindgen", +] + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "winnow" +version = "0.7.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df79d97927682d2fd8adb29682d1140b343be4ac0f08fd68b7765d9c059d3945" + +[[package]] +name = "winnow" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "23b97319f7b8343df12cc98938e5c3eb436064524c8d2b4e30a1d3a36eecdf81" + +[[package]] +name = "wit-bindgen" +version = "0.57.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e" + +[[package]] +name = "zerocopy" +version = "0.8.57" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d35102a9f36d089ccae9e4c6802bc118be4487b80aaffc0ab4e0cf5ce92d2873" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.57" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "146c01f5ab44258da43cf276c74a2763db2ff3969c9c652c3f2de07041d0b2bc" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 0000000..8310b7b --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,65 @@ +[workspace] +resolver = "3" +members = [ + "crates/motor-core", + "crates/can-link", + "crates/dji-esc", + "crates/dji-esc-socketcan", + "crates/mecanum", + "crates/chassisd", +] + +[workspace.package] +version = "0.1.0" +edition = "2024" +rust-version = "1.89" +license = "MIT" + +# 部署二进制去符号表（armv7 3.2MB → ~1MB） +[profile.release] +strip = true + +[workspace.dependencies] +# 异步运行时 +tokio = { version = "1", features = ["rt-multi-thread", "macros", "sync", "time", "signal"] } +# CAN 总线（仅 Linux，由 dji-esc-socketcan 按 target 引入） +socketcan = { version = "4", features = ["tokio"] } +# 错误定义与结构化日志 +thiserror = "2" +tracing = "0.1" +tracing-subscriber = { version = "0.3", features = ["env-filter"] } +# 配置与命令行（chassisd） +serde = { version = "1", features = ["derive"] } +toml = "0.9" +clap = { version = "4", features = ["derive"] } +# 测试 +proptest = "1" +approx = "0.5" +# workspace 内部 crate +motor-core = { path = "crates/motor-core" } +can-link = { path = "crates/can-link" } +dji-esc = { path = "crates/dji-esc" } +dji-esc-socketcan = { path = "crates/dji-esc-socketcan" } +mecanum = { path = "crates/mecanum" } + +[workspace.lints.rust] +unsafe_code = "forbid" + +[workspace.lints.clippy] +pedantic = { level = "warn", priority = -1 } +unwrap_used = "deny" +expect_used = "deny" +dbg_macro = "deny" +# 协议编解码与嵌入式数值转换中大量的 as 转换靠测试保证正确性 +cast_possible_truncation = "allow" +cast_precision_loss = "allow" +cast_sign_loss = "allow" +# 字节 ↔ 有符号整数的补码重解释是协议解码的本意 +cast_possible_wrap = "allow" +# 中文文档中的品牌 / 型号词（RoboMaster、GM3508 等）不需要反引号 +doc_markdown = "allow" +# 以下 pedantic 项对内部库收益低 +module_name_repetitions = "allow" +missing_errors_doc = "allow" +missing_panics_doc = "allow" +must_use_candidate = "allow" diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..3b16d2e --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 sentry-chassis contributors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..c509d8f --- /dev/null +++ b/README.md @@ -0,0 +1,61 @@ +# sentry-chassis + +机甲大师哨兵机器人麦轮底盘固件：Linux ARM 主控 + 4 × GM3508（C620 电调），SocketCAN 接入，tokio 异步架构。 + +## 架构一览 + +| crate | 职责 | 平台 | +| --- | --- | --- | +| [`motor-core`](crates/motor-core) | 电机抽象：`Motor` trait、`MotorState`、PID / 速度环控制器 | 纯逻辑，全平台 | +| [`dji-esc`](crates/dji-esc) | C620/C610 CAN 协议：帧编解码、多圈累计、`EscBus` 异步驱动、仿真器 | 纯逻辑，全平台 | +| [`dji-esc-socketcan`](crates/dji-esc-socketcan) | SocketCAN 后端：真实 CAN 接口 ↔ 内存通道桥接 | Linux | +| [`mecanum`](crates/mecanum) | 麦轮正 / 逆运动学（纯数学） | 全平台 | +| [`chassisd`](crates/chassisd) | 底盘守护进程：配置、1kHz 控制循环、安全逻辑、`--mock` 模式 | 全平台（实车需 Linux） | + +分层原则：`motor-core` 与 `dji-esc` 不做任何真实 I/O——CAN 端口被抽象为一对 tokio 通道（`CanLink`）， +因此协议、运动学与控制逻辑可以在 macOS / CI 上完整测试；真实总线只出现在最外层。 + +## 开发 + +需要 Rust stable（仓库通过 `rust-toolchain.toml` 锁定）。 + +```sh +cargo test --workspace # 单测 + 闭环仿真测试（无需硬件） +cargo run -p chassisd -- --mock # 无硬件全链路演示 +just lint # fmt + clippy（与 CI 同标准） +``` + +### Linux 虚拟 CAN（vcan0）集成测试 + +```sh +just vcan-test # 或见 justfile 内的 modprobe / ip link 命令 +``` + +### 部署到目标板（armv7，实测：创龙 T153 / Buildroot） + +二进制为 musl 静态链接，不依赖板端 libc，无需交叉工具链： + +```sh +just deploy # 编译 + 推送 + mock 试运行（just deploy root@169.254.63.4） +# 或手动： +cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf +scp target/armv7-unknown-linux-musleabihf/release/chassisd root@169.254.63.4:/tmp/ +``` + +板端拉起 CAN（C620 标准 1Mbps；`restart-ms` 让 bus-off 后自动恢复）： + +```sh +ip link set can0 up type can bitrate 1000000 restart-ms 100 +``` + +实车运行：`/tmp/chassisd --config /tmp/chassis.toml --interface can0`。板上运行样例见 +[`deploy/chassisd.service`](deploy/chassisd.service)，配置样例见 +[`config/chassis.toml.example`](config/chassis.toml.example)。 + +> 注意：电调未上电 / CAN 线未接时，控制器会因无 ACK 进入 bus-off（可用 +> `ip -d link show can0` 查看），chassisd 侧表现为四电机反馈离线、强制零电流—— +> 这正是安全设计的预期行为。 + +## 文档 + +- [架构设计](docs/architecture.md)——分层、CAN 协议要点、单位与方向约定、测试策略 diff --git a/clippy.toml b/clippy.toml new file mode 100644 index 0000000..9065cae --- /dev/null +++ b/clippy.toml @@ -0,0 +1,4 @@ +# 异步代码库中禁止阻塞睡眠 +disallowed-methods = [ + { path = "std::thread::sleep", reason = "异步代码请使用 tokio::time::sleep" }, +] diff --git a/config/chassis.toml.example b/config/chassis.toml.example new file mode 100644 index 0000000..7b5b40a --- /dev/null +++ b/config/chassis.toml.example @@ -0,0 +1,67 @@ +# chassisd 配置样例 +# 复制到 /etc/chassisd/chassis.toml 并按实车修改。 + +[can] +interface = "can0" + +[chassis] +# 控制循环周期（毫秒）。C620 期望 1kHz 命令。 +control_period_ms = 1 +# 上层 twist 命令超时（毫秒），超时后目标速度归零。 +command_timeout_ms = 200 +# 电机反馈离线判定（毫秒），离线后强制 0 电流。 +feedback_timeout_ms = 100 + +[mecanum] +# 轮轴线到底盘中心的纵向 / 横向半距（米）与轮半径（米）。 +half_length = 0.18 +half_width = 0.20 +wheel_radius = 0.0762 + +# 四轮顺序：前左、前右、后左、后右（与 mecanum crate 约定一致）。 +[[motor]] +name = "front_left" +esc_id = 1 +direction = 1 +gear_ratio = 19.203 +# 电流限幅（协议 raw 值，C620 满量程 16384）。 +current_limit = 12000 +# 速度环（输入 rpm，输出 raw 电流）。按实车整定。 +kp = 18.0 +ki = 0.6 +kd = 0.0 +# 前馈（raw 电流/rpm）。 +feedforward = 9.0 + +[[motor]] +name = "front_right" +esc_id = 2 +direction = -1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +kd = 0.0 +feedforward = 9.0 + +[[motor]] +name = "rear_left" +esc_id = 3 +direction = 1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +kd = 0.0 +feedforward = 9.0 + +[[motor]] +name = "rear_right" +esc_id = 4 +direction = -1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +kd = 0.0 +feedforward = 9.0 diff --git a/crates/can-link/Cargo.toml b/crates/can-link/Cargo.toml new file mode 100644 index 0000000..9c43737 --- /dev/null +++ b/crates/can-link/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "can-link" +description = "CAN 传输抽象：帧与链路通道（无协议知识）" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true + +[dependencies] +tokio = { workspace = true } + +[lints] +workspace = true diff --git a/crates/can-link/src/lib.rs b/crates/can-link/src/lib.rs new file mode 100644 index 0000000..d354078 --- /dev/null +++ b/crates/can-link/src/lib.rs @@ -0,0 +1,75 @@ +//! CAN 传输抽象：帧与链路通道。 +//! +//! 本 crate **不含任何协议知识**（无 DJI ID 常量）：只定义经典 CAN 数据帧 +//! 与一对 tokio 通道构成的链路端点。协议族（`dji-esc` / 未来 `dji-gm6020`） +//! 与总线后端（SocketCAN 桥、仿真器）共享此类型，互不依赖。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +use tokio::sync::mpsc; + +/// 经典 CAN 数据帧（数据长度固定 8 字节）。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub struct CanFrame { + /// 11 位标准 ID。 + pub id: u32, + /// 数据域。 + pub data: [u8; 8], +} + +impl CanFrame { + /// 构造一帧。 + #[must_use] + pub const fn new(id: u32, data: [u8; 8]) -> Self { + Self { id, data } + } +} + +/// CAN 链路端点。 +/// +/// - `outgoing_tx`：本端发往总线的帧； +/// - `incoming_rx`：本端从总线收到的帧。 +/// +/// 一条链路由两个端点构成（见 [`CanLink::pair`]）。持有本端、把对端交给 +/// 对侧（SocketCAN 桥、仿真器）即可通信。 +#[derive(Debug)] +pub struct CanLink { + /// 发往总线。 + pub outgoing_tx: mpsc::Sender, + /// 来自总线。 + pub incoming_rx: mpsc::Receiver, +} + +impl CanLink { + /// 由已有通道组装端点。 + #[must_use] + pub const fn new( + outgoing_tx: mpsc::Sender, + incoming_rx: mpsc::Receiver, + ) -> Self { + Self { outgoing_tx, incoming_rx } + } + + /// 创建一对互联端点：`a` 发出的帧会被 `b` 收到，反之亦然。 + /// + /// `bound` 为每方向缓冲帧数（建议 ≥ 64，1kHz 收发下留足余量）。 + #[must_use] + pub fn pair(bound: usize) -> (Self, Self) { + let (tx_a_to_b, rx_at_b) = mpsc::channel(bound); + let (tx_b_to_a, rx_at_a) = mpsc::channel(bound); + (Self::new(tx_a_to_b, rx_at_a), Self::new(tx_b_to_a, rx_at_b)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[tokio::test] + async fn pair_loops_both_directions() { + let (mut a, mut b) = CanLink::pair(8); + a.outgoing_tx.send(CanFrame::new(0x200, [1, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); + b.outgoing_tx.send(CanFrame::new(0x201, [2, 0, 0, 0, 0, 0, 0, 0])).await.unwrap(); + assert_eq!(b.incoming_rx.recv().await.unwrap().id, 0x200); + assert_eq!(a.incoming_rx.recv().await.unwrap().id, 0x201); + } +} diff --git a/crates/chassisd/Cargo.toml b/crates/chassisd/Cargo.toml new file mode 100644 index 0000000..b0b0e83 --- /dev/null +++ b/crates/chassisd/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "chassisd" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "Mecanum chassis daemon: configuration, 1 kHz control loop, safety supervision, and mock mode" + +[lints] +workspace = true + +[dependencies] +motor-core = { workspace = true } +dji-esc = { workspace = true } +dji-esc-socketcan = { workspace = true } +mecanum = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } +tracing-subscriber = { workspace = true } +serde = { workspace = true } +toml = { workspace = true } +clap = { workspace = true } + +[dev-dependencies] +proptest = { workspace = true } +approx = { workspace = true } +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/chassisd/src/config.rs b/crates/chassisd/src/config.rs new file mode 100644 index 0000000..23d013d --- /dev/null +++ b/crates/chassisd/src/config.rs @@ -0,0 +1,504 @@ +//! `chassis.toml` 配置模型与加载。 + +use std::time::Duration; + +use dji_esc::{BusOptions, EscConfig, EscModel}; +use mecanum::Mecanum; +use serde::Deserialize; +use thiserror::Error; + +/// 配置错误。 +#[derive(Debug, Clone, PartialEq, Eq, Error)] +pub enum ConfigError { + /// TOML 语法 / 结构错误。 + #[error("failed to parse config: {0}")] + Parse(String), + /// 语义校验失败。 + #[error("invalid config: {0}")] + Invalid(String), +} + +/// 顶层配置。 +#[derive(Debug, Clone, PartialEq, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct Config { + /// CAN 配置。 + pub can: CanConfig, + /// 底盘控制参数。 + #[serde(default)] + pub chassis: ChassisConfig, + /// 麦轮几何。 + pub mecanum: MecanumConfig, + /// 四电机配置，顺序固定：前左、前右、后左、后右。 + #[serde(rename = "motor")] + pub motors: Vec, +} + +/// CAN 接口配置（双 CAN 总线 seam）。 +/// +/// 底盘与功能总线物理隔离，**每条总线只允许一种协议族**（见 [`ProtocolFamily`]）。 +#[derive(Debug, Clone, PartialEq, Eq, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct CanConfig { + /// 底盘总线 SocketCAN 接口名（如 `can0`）——C620 驱动轮，ESC 电流协议族。 + pub interface: String, + /// 功能总线（云台 / 拨弹）：与底盘物理隔离。 + /// + /// 预留 seam；填写后由上层功能模块（gimbal / feeder）使用。 + /// 典型：`can1`，承载 GM6020 云台 + M2006/C610 拨弹。 + #[serde(default)] + pub function: Option, +} + +/// 功能总线配置（双 CAN 之二）。 +#[derive(Debug, Clone, PartialEq, Eq, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct FunctionBusConfig { + /// SocketCAN 接口名（如 `can1`）。 + pub interface: String, +} + +/// 协议族——同一总线上**不得混用**。 +/// +/// C620/C610（电流）与 GM6020（电压）的控制/反馈帧 ID 空间重叠： +/// `0x1FF` 既可作 C620 5–8 电流也可作 GM6020 1–4 电压；`0x205` 同理。 +/// 双 CAN 物理隔离是首选消解手段；单总线时必须保证同族。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)] +pub enum ProtocolFamily { + /// C620/C610 电流协议（驱动轮、拨弹轮）。 + Esc, + /// GM6020 电压协议（云台）。 + Gm6020, +} + +impl ProtocolFamily { + /// 电调型号所属协议族。 + #[must_use] + pub const fn of_esc_model(model: EscModelSerde) -> Self { + match model { + EscModelSerde::C620 | EscModelSerde::C610 => Self::Esc, + } + } +} + +/// 底盘控制参数（全部可省略，采用默认值）。 +#[derive(Debug, Clone, PartialEq, Deserialize)] +#[serde(deny_unknown_fields, default)] +pub struct ChassisConfig { + /// 控制循环周期（毫秒）。 + pub control_period_ms: u64, + /// 上层指令超时（毫秒）：超时后目标速度归零。 + pub command_timeout_ms: u64, + /// 电机反馈离线阈值（毫秒）。 + pub feedback_timeout_ms: u64, + /// 最大前进 / 横移速度（m/s）。 + pub max_vx: f32, + /// 最大横移速度（m/s）。 + pub max_vy: f32, + /// 最大角速度（rad/s）。 + pub max_omega: f32, +} + +impl Default for ChassisConfig { + fn default() -> Self { + Self { + control_period_ms: 1, + command_timeout_ms: 200, + feedback_timeout_ms: 100, + max_vx: 3.0, + max_vy: 3.0, + max_omega: 6.0, + } + } +} + +/// 麦轮几何（米）。 +#[derive(Debug, Clone, Copy, PartialEq, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct MecanumConfig { + /// 前后轮轴线到中心的纵向半距。 + pub half_length: f32, + /// 左右轮到中心的横向半距。 + pub half_width: f32, + /// 车轮半径。 + pub wheel_radius: f32, +} + +/// 单电机配置。 +#[derive(Debug, Clone, PartialEq, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct MotorConfig { + /// 便于日志辨认的名字（`front_left` 等）。 + pub name: String, + /// 电调 ID（1–8）。 + pub esc_id: u8, + /// 电调型号（`C620` / `C610`）。 + #[serde(default = "default_model")] + pub model: EscModelSerde, + /// 安装方向：`1` 或 `-1`。 + #[serde(default = "default_direction")] + pub direction: f32, + /// 减速比（GM3508 ≈ 19.203）。 + pub gear_ratio: f32, + /// 电流限幅（raw）。 + #[serde(default = "default_current_limit")] + pub current_limit: i16, + /// 速度环 kp。 + #[serde(default)] + pub kp: f32, + /// 速度环 ki。 + #[serde(default)] + pub ki: f32, + /// 速度环 kd。 + #[serde(default)] + pub kd: f32, + /// 速度前馈（raw/rpm）。 + #[serde(default)] + pub feedforward: f32, +} + +fn default_model() -> EscModelSerde { + EscModelSerde::C620 +} + +fn default_direction() -> f32 { + 1.0 +} + +fn default_current_limit() -> i16 { + 12_000 +} + +/// 配置文件中的电调型号。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Deserialize)] +#[serde(deny_unknown_fields, rename_all = "UPPERCASE")] +pub enum EscModelSerde { + /// C620（GM3508）。 + C620, + /// C610（M2006）。 + C610, +} + +impl From for EscModel { + fn from(value: EscModelSerde) -> Self { + match value { + EscModelSerde::C620 => Self::C620, + EscModelSerde::C610 => Self::C610, + } + } +} + +impl Config { + /// 从 TOML 文本解析并校验。 + /// + /// # Errors + /// + /// 解析失败或语义校验失败（电机数量、几何、方向等）。 + pub fn from_toml(text: &str) -> Result { + let config: Self = toml::from_str(text).map_err(|e| ConfigError::Parse(e.to_string()))?; + config.validate()?; + Ok(config) + } + + /// 从文件读取并解析。 + /// + /// # Errors + /// + /// 读取失败、解析失败或校验失败。 + pub fn load(path: &std::path::Path) -> Result { + let text = std::fs::read_to_string(path) + .map_err(|e| ConfigError::Parse(format!("{}: {e}", path.display())))?; + Self::from_toml(&text) + } + + /// 语义校验。 + /// + /// # Errors + /// + /// 任一约束不满足时返回 [`ConfigError::Invalid`]。 + pub fn validate(&self) -> Result<(), ConfigError> { + if self.motors.len() != 4 { + return Err(ConfigError::Invalid(format!( + "mecanum chassis needs exactly 4 motors (FL, FR, RL, RR), got {}", + self.motors.len() + ))); + } + let m = &self.mecanum; + if !(m.half_length > 0.0 && m.half_width > 0.0 && m.wheel_radius > 0.0) { + return Err(ConfigError::Invalid(format!("mecanum geometry must be positive: {m:?}"))); + } + let c = &self.chassis; + if c.control_period_ms == 0 || c.command_timeout_ms == 0 || c.feedback_timeout_ms == 0 { + return Err(ConfigError::Invalid( + "control/command/feedback timeouts must be > 0 ms".into(), + )); + } + 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())); + } + for motor in &self.motors { + let gear_ratio_valid = motor.gear_ratio > 0.0; + if !gear_ratio_valid { + return Err(ConfigError::Invalid(format!( + "motor {}: gear_ratio must be > 0", + motor.name + ))); + } + } + // 每总线单协议族：底盘四轮必须同属 ESC 族（C620/C610）。 + let chassis_families: std::collections::HashSet = + self.motors.iter().map(|m| ProtocolFamily::of_esc_model(m.model)).collect(); + if chassis_families.len() > 1 { + return Err(ConfigError::Invalid( + "chassis bus mixes protocol families; keep one family per CAN bus".into(), + )); + } + // 功能总线接口名不得与底盘总线相同（双 CAN 物理隔离）。 + if let Some(function) = &self.can.function { + if function.interface == self.can.interface { + return Err(ConfigError::Invalid(format!( + "function bus interface must differ from chassis interface (both '{}')", + self.can.interface + ))); + } + } + // 电调 ID、方向、限流的细粒度校验由 EscBus::spawn 复核。 + Ok(()) + } + + /// `--mock` 演示 / 测试默认配置（与 `config/chassis.toml.example` 同构， + /// PID 与仿真器增益匹配）。 + #[must_use] + pub fn mock_default() -> Self { + let motor = |name: &str, esc_id: u8, direction: f32| MotorConfig { + name: name.into(), + esc_id, + model: EscModelSerde::C620, + direction, + gear_ratio: 3591.0 / 187.0, + current_limit: 12_000, + kp: 8.0, + ki: 2.0, + kd: 0.0, + feedforward: 16384.0 / 9254.0, + }; + Self { + can: CanConfig { interface: "vcan0".into(), function: None }, + chassis: ChassisConfig::default(), + mecanum: MecanumConfig { half_length: 0.2, half_width: 0.25, wheel_radius: 0.0762 }, + motors: vec![ + motor("front_left", 1, 1.0), + motor("front_right", 2, -1.0), + motor("rear_left", 3, 1.0), + motor("rear_right", 4, -1.0), + ], + } + } + + /// 转换为 `EscBus` 配置（顺序 FL、FR、RL、RR）。 + #[must_use] + 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, + kp: m.kp, + ki: m.ki, + kd: m.kd, + feedforward: m.feedforward, + }) + .collect() + } + + /// 转换为 `EscBus` 运行参数。 + #[must_use] + pub fn bus_options(&self) -> BusOptions { + BusOptions { + period: Duration::from_millis(self.chassis.control_period_ms), + feedback_timeout: Duration::from_millis(self.chassis.feedback_timeout_ms), + } + } + + /// 麦轮运动学模型。 + #[must_use] + pub fn kinematics(&self) -> Mecanum { + Mecanum::new(self.mecanum.half_length, self.mecanum.half_width, self.mecanum.wheel_radius) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const EXAMPLE: &str = r#" +[can] +interface = "can0" + +[chassis] +control_period_ms = 1 +command_timeout_ms = 200 +feedback_timeout_ms = 100 + +[mecanum] +half_length = 0.18 +half_width = 0.20 +wheel_radius = 0.0762 + +[[motor]] +name = "front_left" +esc_id = 1 +direction = 1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +kd = 0.0 +feedforward = 9.0 + +[[motor]] +name = "front_right" +esc_id = 2 +direction = -1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +feedforward = 9.0 + +[[motor]] +name = "rear_left" +esc_id = 3 +direction = 1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +feedforward = 9.0 + +[[motor]] +name = "rear_right" +esc_id = 4 +direction = -1 +gear_ratio = 19.203 +current_limit = 12000 +kp = 18.0 +ki = 0.6 +feedforward = 9.0 +"#; + + #[test] + fn parses_example() { + let cfg = Config::from_toml(EXAMPLE).unwrap(); + assert_eq!(cfg.can.interface, "can0"); + assert_eq!(cfg.motors.len(), 4); + assert_eq!(cfg.motors[0].name, "front_left"); + assert_eq!(cfg.motors[3].esc_id, 4); + assert_eq!(cfg.motors[1].direction.to_bits(), (-1.0f32).to_bits()); + assert!(cfg.validate().is_ok()); + assert_eq!(cfg.bus_options().period, Duration::from_millis(1)); + assert_eq!(cfg.to_esc_configs().len(), 4); + } + + #[test] + fn minimal_config_uses_defaults() { + let cfg = Config::from_toml( + r#" +[can] +interface = "can1" +[mecanum] +half_length = 0.2 +half_width = 0.25 +wheel_radius = 0.0762 +[[motor]] +name = "fl" +esc_id = 1 +gear_ratio = 19.2 +[[motor]] +name = "fr" +esc_id = 2 +gear_ratio = 19.2 +[[motor]] +name = "rl" +esc_id = 3 +gear_ratio = 19.2 +[[motor]] +name = "rr" +esc_id = 4 +gear_ratio = 19.2 +"#, + ) + .unwrap(); + assert_eq!(cfg.chassis, ChassisConfig::default()); + 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); + } + + #[test] + fn rejects_wrong_motor_count() { + let mut cfg = Config::from_toml(EXAMPLE).unwrap(); + cfg.motors.pop(); + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + } + + #[test] + fn rejects_bad_geometry() { + let mut cfg = Config::from_toml(EXAMPLE).unwrap(); + cfg.mecanum.wheel_radius = 0.0; + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + } + + #[test] + fn rejects_unknown_fields() { + let text = EXAMPLE.replace("[can]", "[can]\nstray = 1"); + assert!(matches!(Config::from_toml(&text), Err(ConfigError::Parse(_)))); + } + + #[test] + fn mock_default_is_valid_and_wired_like_example() { + let cfg = Config::mock_default(); + assert!(cfg.validate().is_ok()); + assert_eq!(cfg.motors.len(), 4); + let esc: Vec = cfg.to_esc_configs().iter().map(|m| m.esc_id).collect(); + assert_eq!(esc, [1, 2, 3, 4]); + } + + #[test] + fn function_bus_must_differ_from_chassis_bus() { + let mut cfg = Config::mock_default(); + cfg.can.function = Some(FunctionBusConfig { interface: "vcan0".into() }); + assert!(matches!(cfg.validate(), Err(ConfigError::Invalid(_)))); + + cfg.can.function = Some(FunctionBusConfig { interface: "vcan1".into() }); + assert!(cfg.validate().is_ok()); + } + + #[test] + fn rejects_mixed_protocol_families_on_chassis_bus() { + let mut cfg = Config::mock_default(); + // 当前 ESC 族包含 C620 与 C610；若引入 GM6020 到底盘总线必须拒绝。 + // 这里验证家族集合逻辑：全部 C620/C610 仍为单族。 + cfg.motors[0].model = EscModelSerde::C610; + assert!(cfg.validate().is_ok()); + + // 人为构造跨族：通过 of_esc_model 映射覆盖后仍应单族（C610→Esc）。 + assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C620), ProtocolFamily::Esc); + assert_eq!(ProtocolFamily::of_esc_model(EscModelSerde::C610), ProtocolFamily::Esc); + } + + #[test] + fn parses_function_bus_seam() { + let text = EXAMPLE.replace( + "[can]\ninterface = \"can0\"", + "[can]\ninterface = \"can0\"\n\n[can.function]\ninterface = \"can1\"", + ); + let cfg = Config::from_toml(&text).unwrap(); + let function = cfg.can.function.as_ref().unwrap(); + assert_eq!(function.interface, "can1"); + } +} diff --git a/crates/chassisd/src/controller.rs b/crates/chassisd/src/controller.rs new file mode 100644 index 0000000..d007abc --- /dev/null +++ b/crates/chassisd/src/controller.rs @@ -0,0 +1,203 @@ +//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。 + +use std::time::Duration; + +use mecanum::{Mecanum, Twist}; +use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive}; +use tokio::sync::watch; +use tokio::task::JoinHandle; +use tokio::time::{Instant, MissedTickBehavior, interval}; +use tracing::{info, warn}; + +/// 底盘控制器。 +/// +/// 职责（1kHz 节拍）： +/// 1. 读取最新 twist 指令，**指令超时则归零**； +/// 2. 速度限幅（保持方向按比例收缩）； +/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用； +/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。 +/// +/// 电机反馈离线安全在驱动层完成（强制 0 电流）。 +pub struct ChassisController { + kinematics: Mecanum, + /// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。 + wheels: [W; 4], + /// (max_vx, max_vy, max_omega)。 + limits: (f32, f32, f32), + command_timeout: Duration, +} + +impl ChassisController +where + W: VelocityDrive + MotorFeedback + Disable + 'static, +{ + /// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。 + #[must_use] + pub fn new( + kinematics: Mecanum, + wheels: [W; 4], + limits: (f32, f32, f32), + command_timeout: Duration, + ) -> Self { + Self { kinematics, wheels, limits, command_timeout } + } + + /// 启动控制循环任务。 + 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) { + let mut ticker = interval(period); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + let mut latest = Twist::ZERO; + let mut last_command_at: Option = None; + let mut ticks: u64 = 0; + + loop { + ticker.tick().await; + let now = Instant::now(); + + if commands.has_changed().unwrap_or(false) { + latest = *commands.borrow_and_update(); + last_command_at = Some(now); + } + let timed_out = match last_command_at { + Some(t) => now.duration_since(t) > self.command_timeout, + // 尚未收到任何指令：保持静止。 + None => true, + }; + let twist = if timed_out { + Twist::ZERO + } else { + latest.clamped(self.limits.0, self.limits.1, self.limits.2) + }; + + 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) { + motor.set_velocity(wheel_rpm); + } + } else { + for motor in &self.wheels { + motor.disable(); + } + if ticks.is_multiple_of(1000) { + warn!("waiting for first feedback from all motors"); + } + } + + ticks += 1; + if ticks.is_multiple_of(1000) { + self.log_status(); + } + } + } + + fn log_status(&self) { + let (rotor_rpm, effort_raw): (Vec, Vec) = self + .wheels + .iter() + .map(|m| { + let st = m.state(); + (st.rotor_speed_rpm, i32::from(st.effort_raw)) + }) + .unzip(); + info!(?rotor_rpm, ?effort_raw, "chassis status"); + } +} + +/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。 +#[must_use] +pub fn wrap_wheels( + motors: [T; N], + gear_ratios: [f32; N], +) -> [OutputShaft; N] { + let mut out = Vec::with_capacity(N); + let mut gears = gear_ratios.into_iter(); + for m in motors { + let g = gears.next().unwrap_or(1.0); + out.push(OutputShaft::new(m, g)); + } + out.try_into().unwrap_or_else(|_| unreachable!()) +} + +#[cfg(test)] +mod tests { + use super::*; + use motor_core::{Command, MockMotor, MotorState}; + + const GEAR: f32 = 19.203; + + fn mock_wheel(id: u8) -> MockMotor { + let m = MockMotor::new(id); + m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() }); + m + } + + /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。 + #[tokio::test(start_paused = true)] + async fn controller_drives_wheels_through_capability_traits() { + 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), + ); + let (tx, rx) = watch::channel(Twist::ZERO); + let handle = ctrl.spawn(rx, Duration::from_millis(1)); + + let twist = Twist { vx: 1.0, ..Twist::ZERO }; + tx.send_replace(twist); + tokio::time::sleep(Duration::from_millis(5)).await; + + let expected = kinematics.wheel_rpms(&twist); + for (i, mock) in mocks.iter().enumerate() { + match mock.last_command() { + Command::Velocity(rotor_rpm) => { + let want = expected[i] * GEAR; + assert!( + (rotor_rpm - want).abs() < 1e-2, + "wheel {i}: rotor_rpm {rotor_rpm} != {want}" + ); + } + ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"), + } + } + + handle.abort(); + } + + /// 反馈未到齐时必须全部停用，不得下发速度。 + #[tokio::test(start_paused = true)] + 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 wheels = wrap_wheels( + [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)], + [GEAR; 4], + ); + let ctrl = ChassisController::new( + kinematics, + wheels, + (3.0, 3.0, 6.0), + Duration::from_millis(200), + ); + let (tx, rx) = watch::channel(Twist::ZERO); + let handle = ctrl.spawn(rx, Duration::from_millis(1)); + + tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO }); + tokio::time::sleep(Duration::from_millis(5)).await; + + assert_eq!(ready.last_command(), Command::Disabled); + assert_eq!(missing.last_command(), Command::Disabled); + + handle.abort(); + } +} diff --git a/crates/chassisd/src/lib.rs b/crates/chassisd/src/lib.rs new file mode 100644 index 0000000..850bf4b --- /dev/null +++ b/crates/chassisd/src/lib.rs @@ -0,0 +1,15 @@ +//! `chassisd` 库部分：配置模型、底盘控制循环与指令源抽象。 +//! +//! 二进制入口见 `src/main.rs`。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod config; +pub mod controller; +pub mod source; + +pub use config::{ + CanConfig, ChassisConfig, Config, ConfigError, FunctionBusConfig, MecanumConfig, MotorConfig, + ProtocolFamily, +}; +pub use controller::{ChassisController, wrap_wheels}; +pub use source::{ChannelSource, TwistSource, ZeroSource, pump_source}; diff --git a/crates/chassisd/src/main.rs b/crates/chassisd/src/main.rs new file mode 100644 index 0000000..860feec --- /dev/null +++ b/crates/chassisd/src/main.rs @@ -0,0 +1,161 @@ +//! `chassisd`：麦轮底盘守护进程（4 × GM3508 / C620，SocketCAN）。 + +use std::path::PathBuf; +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 chassisd::config::Config; +use chassisd::controller::{ChassisController, wrap_wheels}; +use motor_core::Disable; + +/// 命令行参数。 +#[derive(Debug, Parser)] +#[command(name = "chassisd", version, about = "麦轮底盘守护进程（4×GM3508/C620）")] +struct Args { + /// 配置文件路径（实车模式必填；`--mock` 下可省略，使用内置演示配置）。 + #[arg(short, long)] + config: Option, + + /// 覆盖配置文件中的 CAN 接口名。 + #[arg(short, long)] + interface: Option, + + /// 无硬件仿真模式：进程内电机模型，全链路可离线运行。 + #[arg(long)] + mock: bool, +} + +fn main() -> ExitCode { + let filter = + EnvFilter::try_from_default_env().unwrap_or_else(|_| EnvFilter::new("info,chassisd=debug")); + tracing_subscriber::fmt().with_env_filter(filter).init(); + + let args = Args::parse(); + + let config = match load_config(&args) { + Ok(config) => config, + Err(e) => { + error!("{e}"); + return ExitCode::FAILURE; + } + }; + let interface = args.interface.clone().unwrap_or_else(|| config.can.interface.clone()); + + let runtime = match tokio::runtime::Builder::new_multi_thread().enable_all().build() { + Ok(runtime) => runtime, + Err(e) => { + error!("failed to start tokio runtime: {e}"); + return ExitCode::FAILURE; + } + }; + match runtime.block_on(run(config, &interface, args.mock)) { + Ok(()) => ExitCode::SUCCESS, + Err(e) => { + error!("{e}"); + ExitCode::FAILURE + } + } +} + +fn load_config(args: &Args) -> Result { + match (&args.config, args.mock) { + (Some(path), _) => Config::load(path).map_err(|e| e.to_string()), + (None, true) => { + info!("mock 模式未提供 --config，使用内置演示配置"); + Ok(Config::mock_default()) + } + (None, false) => { + Err("实车模式需要 --config 指向配置文件（样例见 config/chassis.toml.example）".into()) + } + } +} + +async fn run( + config: Config, + interface: &str, + mock: bool, +) -> Result<(), Box> { + let bus_options = config.bus_options(); + + // 1) CAN 链路：仿真或真实 SocketCAN。 + let link = if mock { + let (bus_side, plant_side) = dji_esc::CanLink::pair(512); + let esc_ids: Vec = config.motors.iter().map(|m| m.esc_id).collect(); + dji_esc::spawn_plant(plant_side, &esc_ids, bus_options.period); + info!(?esc_ids, "mock 模式：进程内电机仿真"); + bus_side + } else { + open_real_link(interface)? + }; + + // 2) 电调总线。 + let period = bus_options.period; + let bus = dji_esc::EscBus::spawn(link, config.to_esc_configs(), bus_options)?; + let names: Vec<&str> = bus.motors().iter().map(dji_esc::EscMotor::name).collect(); + info!( + interface, + mock, + motors = ?names, + period_ms = period.as_millis(), + "chassisd 启动" + ); + + // 3) 控制循环（当前无指令源：watch 保持 ZERO，安全静止； + // 遥控 / 自主导航后续经 chassisd::source 接入）。 + let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); + let limits = (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega); + let motors: [dji_esc::EscMotor; 4] = + bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let gears: [f32; 4] = config + .motors + .iter() + .map(|m| m.gear_ratio) + .collect::>() + .try_into() + .unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let controller = ChassisController::new( + config.kinematics(), + wrap_wheels(motors, gears), + limits, + Duration::from_millis(config.chassis.command_timeout_ms), + ) + .spawn(twist_rx, period); + let _twist_tx = twist_tx; // 保留发送端供后续指令源使用 + + // 4) 优雅退出：ctrl-c 或控制任务意外退出。 + tokio::select! { + _ = tokio::signal::ctrl_c() => info!("收到 ctrl-c，正在停车…"), + result = controller => match result { + Ok(()) => error!("控制任务意外退出"), + Err(e) => error!("控制任务 panic: {e}"), + }, + } + + // 5) 停车：显式停用电机并等若干节拍把 0 电流帧刷入总线。 + for motor in bus.motors() { + motor.disable(); + } + tokio::time::sleep(Duration::from_millis(20)).await; + bus.shutdown().await; + info!("chassisd 已退出"); + Ok(()) +} + +/// 打开真实总线（仅 Linux；其他平台给出明确错误）。 +fn open_real_link(interface: &str) -> Result> { + #[cfg(target_os = "linux")] + { + Ok(dji_esc_socketcan::open_link(interface)?) + } + #[cfg(not(target_os = "linux"))] + { + let _ = interface; + Err("实车模式仅支持 Linux（SocketCAN）；请使用 --mock 演示".into()) + } +} diff --git a/crates/chassisd/src/source.rs b/crates/chassisd/src/source.rs new file mode 100644 index 0000000..e53da84 --- /dev/null +++ b/crates/chassisd/src/source.rs @@ -0,0 +1,81 @@ +//! 上层速度指令源（twist source）。 +//! +//! 守护进程通过 [`watch`] 通道把指令交给控制循环；指令源在独立任务中 +//! 持续产出 twist。遥控器（DR16）、自主导航等后续接入时各自实现 +//! [`TwistSource`] 即可。 + +use std::future::Future; + +use mecanum::Twist; +use tokio::sync::{mpsc, watch}; + +/// 速度指令源。 +pub trait TwistSource: Send { + /// 取下一条指令；`None` 表示源关闭。 + fn next_twist(&mut self) -> impl Future> + Send; +} + +/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。 +pub struct ZeroSource; + +impl TwistSource for ZeroSource { + async fn next_twist(&mut self) -> Option { + std::future::pending().await + } +} + +/// 通道指令源（测试 / 简单进程内注入用）。 +pub struct ChannelSource { + rx: mpsc::Receiver, +} + +impl ChannelSource { + /// 由 mpsc 接收端构造。 + #[must_use] + pub const fn new(rx: mpsc::Receiver) -> Self { + Self { rx } + } + + /// 创建一对（发送端, 源）。 + #[must_use] + 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 { + 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() { + break; + } + } +} + +#[cfg(test)] +mod tests { + 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 pump = tokio::spawn(pump_source(source, watch_tx)); + + tx.send(Twist { vx: 1.0, vy: 0.0, omega: 0.0 }).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 }); + + // 源关闭 → 泵任务退出。 + drop(tx); + pump.await.unwrap(); + assert!((watch_rx.borrow().vx - 1.0).abs() < 1e-6); // watch 保持最后值 + } +} diff --git a/crates/chassisd/tests/daemon.rs b/crates/chassisd/tests/daemon.rs new file mode 100644 index 0000000..20e6791 --- /dev/null +++ b/crates/chassisd/tests/daemon.rs @@ -0,0 +1,169 @@ +//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机 +//! → 控制循环 → 麦轮运动学），全平台可跑。 + +#![allow(clippy::unwrap_used, clippy::expect_used)] + +use std::time::Duration; + +use chassisd::config::Config; +use chassisd::controller::{ChassisController, wrap_wheels}; +use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant}; +use mecanum::Twist; +use motor_core::MotorFeedback; +use tokio::sync::watch; +use tokio::time::sleep; + +/// 搭建与 `chassisd --mock` 完全同构的软件栈。 +/// +/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的 +/// 指令源），超时测试用配置原值。 +fn stack(config: &Config, command_timeout_ms: u64) -> (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); + + let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap(); + + let (twist_tx, twist_rx) = watch::channel(Twist::ZERO); + let motors: [dji_esc::EscMotor; 4] = + bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let gears: [f32; 4] = config + .motors + .iter() + .map(|m| m.gear_ratio) + .collect::>() + .try_into() + .unwrap_or_else(|_| unreachable!("exactly 4 motors")); + let controller = ChassisController::new( + config.kinematics(), + wrap_wheels(motors, gears), + (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega), + Duration::from_millis(command_timeout_ms), + ); + controller.spawn(twist_rx, config.bus_options().period); + (bus, twist_tx) +} + +async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool { + let mut waited = 0u64; + while waited < total_ms { + if cond() { + return true; + } + sleep(Duration::from_millis(5)).await; + waited += 5; + } + cond() +} + +fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] { + bus.motors() + .iter() + .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio) + .collect::>() + .try_into() + .unwrap() +} + +/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。 +const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762); + +#[tokio::test(start_paused = true)] +async fn forward_command_drives_all_wheels_equally() { + let config = Config::mock_default(); + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(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) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!(ok, "未收敛到 {target} rpm: {rpm:?}"); + + // 停车：指令归零后各轮减速到停。 + twist_tx.send_replace(Twist::ZERO); + let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; + let rpm = wheel_rpms(&bus); + assert!(stopped, "未停车: {rpm:?}"); + bus.shutdown().await; +} + +#[tokio::test(start_paused = true)] +async fn rotation_command_respects_wheel_signs() { + let config = Config::mock_default(); + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(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, || { + let rpm = wheel_rpms(&bus); + // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。 + [ + (rpm[0] + target).abs(), + (rpm[1] - target).abs(), + (rpm[2] + target).abs(), + (rpm[3] - target).abs(), + ] + .iter() + .all(|e| *e < 0.05 * target) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}"); + bus.shutdown().await; +} + +#[tokio::test(start_paused = true)] +async fn command_timeout_stops_chassis() { + let config = Config::mock_default(); + // 指令超时使用配置原值（200ms）。 + let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms); + + // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。 + twist_tx.send_replace(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; + assert!(moving, "指令后底盘应当运动"); + + sleep(Duration::from_millis(400)).await; // 超过 200ms 超时 + let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await; + let rpm = wheel_rpms(&bus); + assert!(stopped, "指令超时后未停车: {rpm:?}"); + bus.shutdown().await; +} + +#[tokio::test(start_paused = true)] +async fn speed_limits_scale_motion() { + let mut config = Config::mock_default(); + config.chassis.max_vx = 1.0; // 限速 1 m/s + let (bus, twist_tx) = stack(&config, 10_000); + + twist_tx.send_replace(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; + let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS; + let tol = 0.08 * diag_fast; + let ok = wait_until(3000, || { + let rpm = wheel_rpms(&bus); + [ + (rpm[0] - diag_slow).abs(), + (rpm[1] - diag_fast).abs(), + (rpm[2] - diag_fast).abs(), + (rpm[3] - diag_slow).abs(), + ] + .iter() + .all(|e| *e < tol) + }) + .await; + let rpm = wheel_rpms(&bus); + assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}"); + bus.shutdown().await; +} diff --git a/crates/dji-esc-socketcan/Cargo.toml b/crates/dji-esc-socketcan/Cargo.toml new file mode 100644 index 0000000..efad310 --- /dev/null +++ b/crates/dji-esc-socketcan/Cargo.toml @@ -0,0 +1,25 @@ +[package] +name = "dji-esc-socketcan" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "SocketCAN backend bridging real Linux CAN interfaces to can-link channels" + +[lints] +workspace = true + +[dependencies] +can-link = { workspace = true } +tokio = { workspace = true } +tracing = { workspace = true } + +[target.'cfg(target_os = "linux")'.dependencies] +socketcan = { workspace = true } + +[target.'cfg(target_os = "linux")'.dev-dependencies] +can-link = { workspace = true } +dji-esc = { workspace = true } +motor-core = { workspace = true } +socketcan = { workspace = true } +tokio = { workspace = true } diff --git a/crates/dji-esc-socketcan/examples/spin.rs b/crates/dji-esc-socketcan/examples/spin.rs new file mode 100644 index 0000000..d7861f0 --- /dev/null +++ b/crates/dji-esc-socketcan/examples/spin.rs @@ -0,0 +1,110 @@ +//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。 +//! +//! ```sh +//! spin [interface] [esc_id] [rotor_rpm] [duration_secs] +//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒 +//! cargo run --release -p dji-esc-socketcan --example spin -- can0 1 1000 10 +//! ``` +//! +//! 关键点在 [`spin_one`]：它只依赖 `motor_core::Motor` 抽象，完全不知道 +//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码 +//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（本 crate）在此汇合。 +//! +//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。 + +#![allow(clippy::unwrap_used, clippy::expect_used)] + +#[cfg(target_os = "linux")] +use std::time::Duration; + +#[cfg(target_os = "linux")] +use dji_esc::{BusOptions, EscBus, EscConfig}; +#[cfg(target_os = "linux")] +use dji_esc_socketcan::open_link; +#[cfg(target_os = "linux")] +use motor_core::{Disable, EffortDrive, Motor, MotorFeedback, VelocityDrive}; +#[cfg(target_os = "linux")] +use tokio::time::{MissedTickBehavior, interval}; + +#[cfg(target_os = "linux")] +#[tokio::main] +async fn main() { + let mut args = std::env::args().skip(1); + let interface = args.next().unwrap_or_else(|| "can0".into()); + let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1); + let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0); + let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0); + + println!("=== 单电机抽象验证 ==="); + println!( + "接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s" + ); + + let link = open_link(&interface).unwrap_or_else(|e| { + eprintln!( + "打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）" + ); + std::process::exit(1); + }); + let config = EscConfig::gm3508("spin_test", esc_id, 1.0); + println!( + "电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}", + config.model, config.gear_ratio, config.current_limit + ); + + let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法"); + let motor = bus.motor(esc_id).expect("刚配置的电机必然存在"); + + spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await; + + bus.shutdown().await; + println!("已退出"); +} + +/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。 +/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。 +#[cfg(target_os = "linux")] +async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) { + motor.set_velocity(target_rpm); + println!("已下发目标转速，开始观测（100ms 周期）……\n"); + + let mut ticker = interval(Duration::from_millis(100)); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + let deadline = tokio::time::Instant::now() + duration; + + loop { + tokio::select! { + _ = tokio::signal::ctrl_c() => { + println!("\n收到 ctrl-c"); + break; + } + _ = ticker.tick() => {} + } + if tokio::time::Instant::now() >= deadline { + break; + } + let st = motor.state(); + if st.has_feedback() { + println!( + "转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧", + st.rotor_speed_rpm, + st.effort_raw, + st.temperature_c, + st.rotor_revolutions, + st.feedback_count + ); + } else { + println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线"); + } + } + + motor.disable(); + // 让 0 电流帧至少发出一拍再关总线。 + tokio::time::sleep(Duration::from_millis(50)).await; + println!("已停机（0 电流）"); +} + +#[cfg(not(target_os = "linux"))] +fn main() { + eprintln!("spin 仅支持 Linux（需要 SocketCAN）"); +} diff --git a/crates/dji-esc-socketcan/src/bridge.rs b/crates/dji-esc-socketcan/src/bridge.rs new file mode 100644 index 0000000..fa867d6 --- /dev/null +++ b/crates/dji-esc-socketcan/src/bridge.rs @@ -0,0 +1,96 @@ +//! 真实总线桥接实现（Linux）。 + +use std::io; +use std::sync::Arc; + +use can_link::{CanFrame as LinkFrame, CanLink}; +use socketcan::tokio::CanSocket; +use socketcan::{ + CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, + SocketOptions as _, +}; +use tokio::sync::mpsc; +use tracing::{debug, error}; + +/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。 +const CHANNEL_BOUND: usize = 512; + +/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。 +/// +/// 内部： +/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208， +/// 非反馈帧再由协议层解码拒绝，双保险）； +/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。 +/// +/// # Errors +/// +/// 接口不存在 / 权限不足等 `io::Error`。 +pub fn open_link(interface: &str) -> io::Result { + let socket = CanSocket::open(interface)?; + socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?; + let socket = Arc::new(socket); + + let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND); + let CanLink { outgoing_tx, incoming_rx } = bridge_side; + + tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx)); + tokio::spawn(tx_bridge(socket, incoming_rx)); + Ok(bus_side) +} + +/// socket → 通道：真实总线反馈进入协议 bus。 +async fn rx_bridge(socket: Arc, tx: mpsc::Sender) { + loop { + match socket.read_frame().await { + Ok(frame) => { + if let Some(frame) = to_link_frame(&frame) { + if tx.send(frame).await.is_err() { + debug!("bus side closed, rx bridge exiting"); + return; + } + } + } + Err(e) => { + // 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程， + // 由 systemd 拉起重连。 + error!(error = %e, "CAN read failed, rx bridge exiting"); + return; + } + } + } +} + +/// 通道 → socket：协议 bus 命令写入真实总线。 +async fn tx_bridge(socket: Arc, mut rx: mpsc::Receiver) { + while let Some(frame) = rx.recv().await { + match to_socketcan_frame(&frame) { + Some(sc_frame) => { + if let Err(e) = socket.write_frame(sc_frame).await { + error!(error = %e, "CAN write failed, tx bridge exiting"); + return; + } + } + None => { + debug!(id = frame.id, "drop frame with non-standard CAN id"); + } + } + } + debug!("bus side closed, tx bridge exiting"); +} + +/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。 +fn to_link_frame(frame: &ScFrame) -> Option { + match frame { + ScFrame::Data(df) if df.data().len() == 8 => { + Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?)) + } + _ => None, + } +} + +/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。 +fn to_socketcan_frame(frame: &LinkFrame) -> Option { + let id = socketcan::StandardId::new(frame.id as u16)?; + let df = CanDataFrame::new(id, &frame.data)?; + Some(ScFrame::Data(df)) +} diff --git a/crates/dji-esc-socketcan/src/lib.rs b/crates/dji-esc-socketcan/src/lib.rs new file mode 100644 index 0000000..b4e262c --- /dev/null +++ b/crates/dji-esc-socketcan/src/lib.rs @@ -0,0 +1,11 @@ +//! SocketCAN 桥接：把真实 Linux CAN 接口接入 [`can_link::CanLink`] 通道对。 +//! +//! 本 crate 主体仅在 Linux 下编译（依赖 socketcan）。其他平台编译为空壳， +//! 使整个 workspace 可以在任意开发机上构建与测试；实车部署目标板均为 +//! Linux。 + +#[cfg(target_os = "linux")] +mod bridge; + +#[cfg(target_os = "linux")] +pub use bridge::open_link; diff --git a/crates/dji-esc-socketcan/tests/vcan.rs b/crates/dji-esc-socketcan/tests/vcan.rs new file mode 100644 index 0000000..8d2bf8f --- /dev/null +++ b/crates/dji-esc-socketcan/tests/vcan.rs @@ -0,0 +1,132 @@ +//! vcan 集成测试：在真实 Linux 内核 CAN 栈上验证桥接。 +//! +//! 仅 Linux 下编译；默认 `#[ignore]`，需要 vcan0（见 justfile `vcan-test`）。 + +#![cfg(target_os = "linux")] +#![allow(clippy::unwrap_used, clippy::expect_used)] + +use std::time::Duration; + +use dji_esc::{CanFrame, CanLink}; +use motor_core::Motor; +use socketcan::tokio::CanSocket; +use socketcan::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId}; +use tokio::time::timeout; + +const IFACE: &str = "vcan0"; + +fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame { + ScFrame::Data(CanDataFrame::new(StandardId::new(id).unwrap(), &data).unwrap()) +} + +#[tokio::test] +#[ignore = "requires vcan0 (see `just vcan-test`)"] +async fn bridge_relays_command_to_bus() { + let link = dji_esc_socketcan::open_link(IFACE).unwrap(); + let observer = CanSocket::open(IFACE).unwrap(); + + let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]); + link.outgoing_tx.send(command).await.unwrap(); + + let got = timeout(Duration::from_secs(2), observer.read_frame()) + .await + .expect("timeout waiting for command frame") + .unwrap(); + match got { + ScFrame::Data(df) => { + assert_eq!(df.raw_id(), 0x200); + assert_eq!(df.data(), &[0x04, 0xD2, 0, 0, 0, 0, 0, 0]); + } + other => panic!("expected data frame, got {other:?}"), + } +} + +#[tokio::test] +#[ignore = "requires vcan0 (see `just vcan-test`)"] +async fn bridge_relays_feedback_from_bus() { + let CanLink { outgoing_tx: _, mut incoming_rx } = dji_esc_socketcan::open_link(IFACE).unwrap(); + let simulator = CanSocket::open(IFACE).unwrap(); + + // 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。 + let feedback = sc_data_frame(0x201, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]); + simulator.write_frame(feedback).await.unwrap(); + + let got = timeout(Duration::from_secs(2), incoming_rx.recv()) + .await + .expect("timeout waiting for feedback frame") + .unwrap(); + assert_eq!(got.id, 0x201); + assert_eq!(got.data, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]); +} + +#[tokio::test] +#[ignore = "requires vcan0 (see `just vcan-test`)"] +async fn esc_bus_end_to_end_over_vcan() { + // EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。 + let link = dji_esc_socketcan::open_link(IFACE).unwrap(); + let mut esc = EscSim::new(IFACE, &[1]); + + let bus = dji_esc::EscBus::spawn( + link, + vec![dji_esc::EscConfig::gm3508("m1", 1, 1.0)], + dji_esc::BusOptions::default(), + ) + .unwrap(); + bus.motor(1).unwrap().set_velocity(2000.0); + + // 仿真 2s：模拟电调按命令回反馈，速度环应收敛。 + let motor = bus.motor(1).unwrap().clone(); + let deadline = tokio::time::Instant::now() + Duration::from_secs(2); + loop { + esc.step_once().await; + if motor.state().rotor_speed_rpm > 1900.0 { + break; + } + assert!( + tokio::time::Instant::now() < deadline, + "not converged: {}", + motor.state().rotor_speed_rpm + ); + tokio::time::sleep(Duration::from_millis(5)).await; + } + bus.shutdown().await; +} + +/// 最小电调模拟：收命令帧 → 更新转速 → 回反馈帧。 +struct EscSim { + socket: CanSocket, + speed_rpm: f32, + angle: u16, +} + +impl EscSim { + fn new(iface: &str, _esc_ids: &[u8]) -> Self { + Self { socket: CanSocket::open(iface).unwrap(), speed_rpm: 0.0, angle: 0 } + } + + /// 排空当前命令帧并推进模型一步（dt = 5ms）。 + async fn step_once(&mut self) { + const DT: f32 = 0.005; + let mut current = 0i16; + while let Ok(frame) = self.socket.try_read_frame() { + if let ScFrame::Data(df) = frame { + if df.raw_id() == 0x200 { + current = i16::from_be_bytes([df.data()[0], df.data()[1]]); + } + } + } + let target = f32::from(current) * 9254.0 / 16384.0; + self.speed_rpm += (target - self.speed_rpm) * (1.0 - (-DT / 0.12).exp()); + self.angle = + (u32::from(self.angle) + (self.speed_rpm / 60.0 * 8192.0 * DT) as u32) as u16 % 8192; + let data = { + let mut d = [0u8; 8]; + d[0..2].copy_from_slice(&self.angle.to_be_bytes()); + d[2..4].copy_from_slice(&(self.speed_rpm as i16).to_be_bytes()); + d[4..6].copy_from_slice(¤t.to_be_bytes()); + d[6] = 30; + d + }; + self.socket.write_frame(sc_data_frame(0x201, data)).await.unwrap(); + } +} diff --git a/crates/dji-esc/Cargo.toml b/crates/dji-esc/Cargo.toml new file mode 100644 index 0000000..2d0ce0e --- /dev/null +++ b/crates/dji-esc/Cargo.toml @@ -0,0 +1,20 @@ +[package] +name = "dji-esc" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "DJI RoboMaster ESC (C620/C610) CAN protocol: codec, multi-turn tracking, and async bus driver" + +[lints] +workspace = true + +[dependencies] +motor-core = { workspace = true } +can-link = { workspace = true } +tokio = { workspace = true } +thiserror = { workspace = true } +tracing = { workspace = true } + +[dev-dependencies] +tokio = { workspace = true, features = ["test-util"] } diff --git a/crates/dji-esc/src/bus.rs b/crates/dji-esc/src/bus.rs new file mode 100644 index 0000000..6656415 --- /dev/null +++ b/crates/dji-esc/src/bus.rs @@ -0,0 +1,512 @@ +//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。 +//! +//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务： +//! +//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）； +//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发 +//! effort），按 `0x200`/`0x1FF` 分组组帧发送。 +//! +//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。 +//! 命令模式切换时同样复位控制器，避免积分冲击。 + +use std::sync::{Arc, Mutex, MutexGuard}; +use std::time::Duration; + +use motor_core::{ + Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController, + PositionDrive, VelocityController, VelocityDrive, +}; +use thiserror::Error; +use tokio::sync::{mpsc, watch}; +use tokio::task::JoinHandle; +use tokio::time::{Instant, Interval, MissedTickBehavior}; +use tracing::{debug, info, warn}; + +use crate::codec::{ + EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame, +}; +use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink}; +use crate::turn::TurnTracker; + +/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌， +/// 状态本身仍一致，驱动机不应因此停摆）。 +fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { + mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) +} + +/// 单台电机的配置。 +#[derive(Debug, Clone)] +pub struct EscConfig { + /// 便于日志辨认的名字（如 `front_left`）。 + pub name: String, + /// 电调 ID（1–8，即 CAN 拨码）。 + pub esc_id: u8, + /// 电调型号。 + pub model: EscModel, + /// 减速比（电机转子 : 输出轴）。 + pub gear_ratio: f32, + /// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转， + /// 上层永远以"输出轴正转 = 前进方向"工作。 + pub direction: f32, + /// effort 限幅（raw，`1..=型号满量程`）。 + pub current_limit: i16, + /// 速度环比例增益（raw / rpm）。 + pub kp: f32, + /// 速度环积分增益（raw / (rpm·s)）。 + pub ki: f32, + /// 速度环微分增益。 + pub kd: f32, + /// 速度前馈（raw / rpm）。 + pub feedforward: f32, +} + +impl EscConfig { + /// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值，需按整车整定。 + #[must_use] + pub fn gm3508(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, + direction, + current_limit: 12_000, + kp: 10.0, + ki: 0.5, + kd: 0.0, + feedforward: 1.5, + } + } + + /// 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, + kp: 8.0, + ki: 0.4, + kd: 0.0, + feedforward: 1.0, + } + } + + fn validate(&self) -> Result<(), String> { + 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) { + return Err(format!( + "current_limit {} out of range 1..={} for {:?}", + self.current_limit, + self.model.max_current_raw(), + self.model + )); + } + Ok(()) + } +} + +/// 总线运行参数。 +#[derive(Debug, Clone)] +pub struct BusOptions { + /// 控制节拍（C620 期望 1kHz 命令）。 + pub period: Duration, + /// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。 + pub feedback_timeout: Duration, +} + +impl Default for BusOptions { + fn default() -> Self { + Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) } + } +} + +/// 启动错误。 +#[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, + }, + /// 一台电机都没有配置。 + #[error("no motors configured")] + NoMotors, +} + +/// 单电机共享状态（驱动任务与 [`EscMotor`] 句柄之间）。 +struct MotorShared { + cfg: EscConfig, + state: Mutex, + cmd: Mutex, + last_mode: Mutex, + vel_ctrl: Mutex, + pos_ctrl: Mutex, + turn: Mutex, + last_feedback: Mutex>, + offline_warned: std::sync::atomic::AtomicBool, +} + +impl MotorShared { + fn new(cfg: EscConfig) -> Self { + let limit = cfg.current_limit; + let pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit)); + let pos_pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit)); + Self { + vel_ctrl: Mutex::new(VelocityController::new(pid, cfg.feedforward)), + pos_ctrl: Mutex::new(PositionController::direct(pos_pid)), + cfg, + state: Mutex::new(MotorState::default()), + cmd: Mutex::new(Command::Disabled), + last_mode: Mutex::new(CommandMode::Disabled), + turn: Mutex::new(TurnTracker::new()), + last_feedback: Mutex::new(None), + offline_warned: std::sync::atomic::AtomicBool::new(false), + } + } + + /// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。 + fn apply_feedback(&self, fb: EscFeedback, 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_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.feedback_count = st.feedback_count.saturating_add(1); + } + *lock(&self.last_feedback) = Some(now); + self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed); + } + + /// 复位主机侧控制器（离线或模式切换时）。 + fn reset_controllers(&self) { + lock(&self.vel_ctrl).reset(); + lock(&self.pos_ctrl).reset(); + } + + /// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。 + fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 { + let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout); + if !online { + if !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"); + } + // 复位控制器，避免重新上线时旧积分冲击。 + self.reset_controllers(); + *lock(&self.last_mode) = CommandMode::Disabled; + return 0; + } + let cmd = *lock(&self.cmd); + // 模式切换复位：避免积分/微分记忆冲击。 + { + let mut last_mode = lock(&self.last_mode); + if *last_mode != cmd.mode() { + debug!(motor = %self.cfg.name, from = ?*last_mode, to = ?cmd.mode(), "command mode switch, resetting controllers"); + self.reset_controllers(); + *last_mode = cmd.mode(); + } + } + let dir = self.cfg.direction; + let raw = match cmd { + Command::Disabled => 0, + Command::EffortRaw(raw) => (f32::from(raw) * dir) as i16, + Command::Velocity(target) => { + let measured = lock(&self.state).rotor_speed_rpm; + let out = lock(&self.vel_ctrl).update(target, measured, dt); + (out * dir) as i16 + } + Command::Position(target) => { + let (measured_rev, measured_rpm) = { + let st = lock(&self.state); + (st.rotor_revolutions, st.rotor_speed_rpm) + }; + let out = lock(&self.pos_ctrl).update(target, measured_rev, measured_rpm, dt); + (out * dir) as i16 + } + }; + let limit = self.cfg.current_limit; + clamp_current(self.cfg.model, raw.clamp(-limit, limit)) + } +} + +/// 电机的公共句柄：可克隆，跨任务共享。 +#[derive(Clone)] +pub struct EscMotor { + shared: Arc, +} + +impl EscMotor { + /// 电机名。 + #[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 { + fn id(&self) -> u8 { + self.shared.cfg.esc_id + } + + fn state(&self) -> MotorState { + *lock(&self.shared.state) + } +} + +impl VelocityDrive for EscMotor { + fn set_velocity(&self, rotor_rpm: f32) { + *lock(&self.shared.cmd) = Command::Velocity(rotor_rpm); + } +} + +impl EffortDrive for EscMotor { + fn set_effort_raw(&self, raw: i16) { + *lock(&self.shared.cmd) = Command::EffortRaw(raw); + } +} + +impl PositionDrive for EscMotor { + fn set_position(&self, rotor_rev: f32) { + *lock(&self.shared.cmd) = Command::Position(rotor_rev); + } +} + +impl Disable for EscMotor { + fn disable(&self) { + *lock(&self.shared.cmd) = Command::Disabled; + } +} + +/// 运行中的电调总线。 +pub struct EscBus; + +impl EscBus { + /// 启动收发任务，返回句柄。 + /// + /// # Errors + /// + /// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。 + pub fn spawn( + link: CanLink, + configs: Vec, + options: BusOptions, + ) -> Result { + if configs.is_empty() { + return Err(EscBusError::NoMotors); + } + let mut motors = Vec::with_capacity(configs.len()); + let mut seen = std::collections::HashSet::new(); + for cfg in configs { + cfg.validate() + .map_err(|reason| EscBusError::InvalidConfig { name: cfg.name.clone(), reason })?; + if !seen.insert(cfg.esc_id) { + return Err(EscBusError::DuplicateEscId(cfg.esc_id)); + } + motors.push(EscMotor { shared: Arc::new(MotorShared::new(cfg)) }); + } + + let (shutdown_tx, shutdown_rx) = watch::channel(false); + let CanLink { outgoing_tx, incoming_rx } = link; + let motors = Arc::new(motors); + let rx_handle = + tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone())); + let tx_handle = + tokio::spawn(tx_loop(outgoing_tx, Arc::clone(&motors), options, shutdown_rx)); + + Ok(EscBusHandle { motors, shutdown_tx, tasks: vec![rx_handle, tx_handle] }) + } +} + +/// `EscBus` 的控制句柄。 +pub struct EscBusHandle { + motors: Arc>, + shutdown_tx: watch::Sender, + tasks: Vec>, +} + +impl EscBusHandle { + /// 按电调 ID 取电机句柄。 + #[must_use] + 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] { + &self.motors + } + + /// 停止收发任务并等待退出。 + pub async fn shutdown(self) { + let _ = self.shutdown_tx.send(true); + for task in self.tasks { + let _ = task.await; + } + } +} + +async fn rx_loop( + mut incoming: mpsc::Receiver, + motors: Arc>, + mut shutdown: watch::Receiver, +) { + loop { + let frame = tokio::select! { + _ = shutdown.changed() => break, + frame = incoming.recv() => match frame { + Some(frame) => frame, + None => break, + }, + }; + 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()); + } + } + Err(e) => { + debug!(error = %e, id = frame.id, "ignore non-feedback frame"); + } + } + } +} + +async fn tx_loop( + outgoing: mpsc::Sender, + motors: Arc>, + options: BusOptions, + mut shutdown: watch::Receiver, +) { + let dt = options.period.as_secs_f32(); + let mut ticker = tick_every(options.period); + loop { + tokio::select! { + _ = shutdown.changed() => break, + _ = ticker.tick() => {} + } + let now = Instant::now(); + for (frame_id, group_start) in + [(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)] + { + 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]; + for motor in &group { + let slot = usize::from(motor.id() - group_start); + currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now); + } + if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() { + info!("can link closed, tx loop exiting"); + return; + } + } + } +} + +fn tick_every(period: Duration) -> Interval { + let mut ticker = tokio::time::interval(period); + // 错过节拍后直接跳到当前节拍，避免补发帧风暴。 + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + ticker +} + +#[cfg(test)] +mod tests { + use super::*; + + #[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); + 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); + assert!((cfg.gear_ratio - 36.0).abs() < 0.01); + assert!(cfg.validate().is_ok()); + } +} diff --git a/crates/dji-esc/src/codec.rs b/crates/dji-esc/src/codec.rs new file mode 100644 index 0000000..c0ec91f --- /dev/null +++ b/crates/dji-esc/src/codec.rs @@ -0,0 +1,249 @@ +//! DJI 电调（C620 / C610）CAN 协议编解码。 +//! +//! 帧格式（大端）： +//! +//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）： +//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令， +//! 槽位按组内电调顺序。 +//! - 反馈（`0x200 + 电调ID`）： +//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。 +//! +//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在 +//! [`crate::bus`] 中按节拍调用。 + +use crate::frame::{ + COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE, +}; +use thiserror::Error; + +/// 电调 ID 下限。 +pub const MIN_ESC_ID: u8 = 1; +/// 电调 ID 上限。 +pub const MAX_ESC_ID: u8 = 8; + +/// 电调型号决定电流标度。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum EscModel { + /// C620（配 GM3508）：±16384 ↔ ±20 A。 + C620, + /// C610（配 M2006）：±10000 ↔ ±10 A。 + /// + /// 接入前需实测核对。 + C610, +} + +impl EscModel { + /// 满量程电流命令（raw）。 + #[must_use] + pub const fn max_current_raw(self) -> i16 { + match self { + Self::C620 => 16384, + Self::C610 => 10_000, + } + } + + /// 满量程电流（安培）。 + #[must_use] + pub const fn max_current_amp(self) -> f32 { + match self { + Self::C620 => 20.0, + Self::C610 => 10.0, + } + } + + /// 安培 → raw（限幅）。 + #[must_use] + pub fn raw_from_amp(self, amp: f32) -> i16 { + let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp(); + (raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16) + .clamp(-self.max_current_raw(), self.max_current_raw()) + } + + /// raw → 安培。 + #[must_use] + pub fn amp_from_raw(self, raw: i16) -> f32 { + f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw()) + } +} + +/// 一帧电调反馈（原始、电调侧，未经方向修正）。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub struct EscFeedback { + /// 转子机械角原始值（0..8191，对应 0..360°）。 + pub angle_raw: u16, + /// 转子转速（rpm，电调侧符号）。 + pub speed_rpm: i16, + /// 转矩电流 raw（电调侧符号）。 + pub current_raw: i16, + /// 电机温度（℃）。 + pub temperature_c: i8, +} + +/// 帧解码错误。 +#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)] +pub enum DecodeError { + /// 帧不属于电调反馈范围（0x201..=0x208）。 + #[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")] + NotFeedback { + /// 收到的帧 ID。 + id: u32, + }, +} + +/// 电调 ID → 其电流命令所在的组控制帧 ID。 +/// +/// 返回 `None` 表示 ID 越界。 +#[must_use] +pub const fn command_frame_id(esc_id: u8) -> Option { + match esc_id { + 1..=4 => Some(COMMAND_FRAME_ID_PRIMARY), + 5..=8 => Some(COMMAND_FRAME_ID_SECONDARY), + _ => None, + } +} + +/// 将 raw 电流限幅到电调满量程。 +#[must_use] +pub const fn clamp_current(model: EscModel, raw: i16) -> i16 { + if raw > model.max_current_raw() { + model.max_current_raw() + } else if raw < -model.max_current_raw() { + -model.max_current_raw() + } else { + raw + } +} + +/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。 +/// +/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。 +/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成 +/// （见 [`clamp_current`]）。 +#[must_use] +pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame { + let mut data = [0u8; 8]; + for (slot, raw) in currents.iter().enumerate() { + data[slot * 2] = (raw >> 8) as u8; + data[slot * 2 + 1] = (raw & 0xFF) as u8; + } + CanFrame::new(frame_id, data) +} + +/// 组装一帧电调反馈（仿真器 / 测试用）。 +#[must_use] +pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame { + let mut data = [0u8; 8]; + data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes()); + data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes()); + data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes()); + data[6] = feedback.temperature_c as u8; + CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data) +} + +/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。 +/// +/// # Errors +/// +/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。 +pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> { + if frame.id <= FEEDBACK_FRAME_ID_BASE + || frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID) + { + return Err(DecodeError::NotFeedback { id: frame.id }); + } + let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8; + Ok(( + esc_id, + EscFeedback { + angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]), + speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]), + current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]), + temperature_c: frame.data[6] as i8, + }, + )) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn command_frame_id_routing() { + assert_eq!(command_frame_id(1), Some(0x200)); + assert_eq!(command_frame_id(4), Some(0x200)); + assert_eq!(command_frame_id(5), Some(0x1FF)); + assert_eq!(command_frame_id(8), Some(0x1FF)); + assert_eq!(command_frame_id(0), None); + assert_eq!(command_frame_id(9), None); + } + + #[test] + fn encode_current_frame_bytes() { + let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]); + assert_eq!(frame.id, 0x200); + assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]); + + let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]); + assert_eq!(frame.id, 0x1FF); + assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]); + } + + #[test] + fn decode_feedback_known_bytes() { + let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]); + let (esc_id, fb) = decode_feedback(&frame).unwrap(); + assert_eq!(esc_id, 5); + assert_eq!(fb.angle_raw, 8191); + assert_eq!(fb.speed_rpm, -364); + assert_eq!(fb.current_raw, 123); + assert_eq!(fb.temperature_c, 45); + } + + #[test] + fn decode_feedback_rejects_non_feedback_ids() { + for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] { + let frame = CanFrame::new(id, [0; 8]); + assert_eq!( + decode_feedback(&frame), + Err(DecodeError::NotFeedback { id }), + "id = 0x{id:03X}" + ); + } + } + + #[test] + fn feedback_roundtrip_all_ids() { + let fb = EscFeedback { + angle_raw: 8191, + speed_rpm: -9254, + current_raw: -16384, + temperature_c: -40, + }; + for esc_id in MIN_ESC_ID..=MAX_ESC_ID { + let frame = encode_feedback_frame(esc_id, &fb); + assert_eq!(frame.id, 0x200 + u32::from(esc_id)); + assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb)); + } + } + + #[test] + fn amp_conversion() { + let c620 = EscModel::C620; + assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4); + assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4); + assert_eq!(c620.raw_from_amp(20.0), 16384); + assert_eq!(c620.raw_from_amp(-5.0), -4096); + // 超量程限幅。 + assert_eq!(c620.raw_from_amp(99.0), 16384); + let c610 = EscModel::C610; + assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4); + assert_eq!(c610.raw_from_amp(10.0), 10000); + } + + #[test] + fn clamp_current_respects_model() { + assert_eq!(clamp_current(EscModel::C620, 20000), 16384); + assert_eq!(clamp_current(EscModel::C610, 15000), 10000); + assert_eq!(clamp_current(EscModel::C620, 1000), 1000); + } +} diff --git a/crates/dji-esc/src/frame.rs b/crates/dji-esc/src/frame.rs new file mode 100644 index 0000000..41cd0f6 --- /dev/null +++ b/crates/dji-esc/src/frame.rs @@ -0,0 +1,13 @@ +//! DJI 电调协议帧 ID 常量。 +//! +//! 帧与链路类型（[`CanFrame`] / [`CanLink`]）由 [`can_link`] 提供， +//! 本模块只保留 **C620/C610 电流协议族**的 ID 约定。 + +pub use can_link::{CanFrame, CanLink}; + +/// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。 +pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200; +/// 控制帧 ID：电调 5–8 的电流命令。 +pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF; +/// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。 +pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200; diff --git a/crates/dji-esc/src/lib.rs b/crates/dji-esc/src/lib.rs new file mode 100644 index 0000000..9f17fcd --- /dev/null +++ b/crates/dji-esc/src/lib.rs @@ -0,0 +1,29 @@ +//! DJI RoboMaster 电调（C620/C610）CAN 协议与异步驱动。 +//! +//! 分层： +//! +//! - [`codec`]：纯函数帧编解码（C620 手册协议）； +//! - [`turn`]：单圈编码器 → 连续多圈角； +//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）； +//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全； +//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。 +//! +//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction` +//! 完成方向修正。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod bus; +pub mod codec; +pub mod frame; +pub mod sim; +pub mod turn; + +pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor}; +pub use can_link::{CanFrame, CanLink}; +pub use codec::{ + DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id, + decode_feedback, encode_current_frame, encode_feedback_frame, +}; +pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE}; +pub use sim::{PlantMotor, spawn_plant}; +pub use turn::{ENCODER_TICKS, TurnTracker}; diff --git a/crates/dji-esc/src/sim.rs b/crates/dji-esc/src/sim.rs new file mode 100644 index 0000000..7ecbf05 --- /dev/null +++ b/crates/dji-esc/src/sim.rs @@ -0,0 +1,116 @@ +//! 电机被控对象仿真：一阶转速模型 + 编码器积分。 +//! +//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环 +//! （测试、`chassisd --mock` 均复用）。 + +use std::collections::HashMap; +use std::time::Duration; + +use tokio::task::JoinHandle; +use tokio::time::{MissedTickBehavior, interval}; +use tracing::trace; + +use crate::codec::{EscFeedback, encode_feedback_frame}; +use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink}; + +/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。 +/// +/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数 +/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。 +#[derive(Debug, Clone)] +pub struct PlantMotor { + gain_rpm_per_raw: f32, + tau: f32, + max_rpm: f32, + speed_rpm: f32, + angle_raw: f32, +} + +impl PlantMotor { + /// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。 + #[must_use] + pub fn gm3508() -> Self { + Self { + gain_rpm_per_raw: 9254.0 / 16384.0, + tau: 0.12, + max_rpm: 9254.0, + speed_rpm: 0.0, + angle_raw: 0.0, + } + } + + /// 当前转子转速（rpm，电调侧）。 + #[must_use] + pub fn speed_rpm(&self) -> f32 { + self.speed_rpm + } + + /// 推进一个时间步，返回本步反馈帧内容。 + pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback { + let target = + (f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm); + let alpha = 1.0 - (-dt / self.tau).exp(); + 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); + EscFeedback { + angle_raw: self.angle_raw as u16, + speed_rpm: self.speed_rpm as i16, + current_raw, + temperature_c: 30, + } + } +} + +/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。 +/// +/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。 +pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> { + let mut plants: HashMap = + esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect(); + let mut commands: HashMap = esc_ids.iter().map(|&id| (id, 0)).collect(); + let dt = tick.as_secs_f32(); + let mut ticker = interval(tick); + ticker.set_missed_tick_behavior(MissedTickBehavior::Skip); + + tokio::spawn(async move { + loop { + tokio::select! { + frame = link.incoming_rx.recv() => match frame { + Some(frame) => apply_command_frame(&frame, &mut commands), + None => break, + }, + _ = ticker.tick() => { + for (&id, plant) in &mut plants { + let cmd = commands.get(&id).copied().unwrap_or(0); + let feedback = plant.step(cmd, dt); + if link + .outgoing_tx + .send(encode_feedback_frame(id, &feedback)) + .await + .is_err() + { + return; + } + } + } + } + } + trace!("plant exiting"); + }) +} + +fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap) { + let group_start = match frame.id { + COMMAND_FRAME_ID_PRIMARY => 1u8, + COMMAND_FRAME_ID_SECONDARY => 5u8, + _ => return, + }; + for slot in 0..4u8 { + if let Some(cmd) = commands.get_mut(&(group_start + slot)) { + *cmd = i16::from_be_bytes([ + frame.data[(slot * 2) as usize], + frame.data[(slot * 2 + 1) as usize], + ]); + } + } +} diff --git a/crates/dji-esc/src/turn.rs b/crates/dji-esc/src/turn.rs new file mode 100644 index 0000000..78faadd --- /dev/null +++ b/crates/dji-esc/src/turn.rs @@ -0,0 +1,134 @@ +//! 多圈累计：单圈编码器原始角 → 连续圈数。 + +/// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。 +pub const ENCODER_TICKS: u16 = 8192; + +/// 半圈阈值：帧间角变化超过它视为发生回绕。 +const HALF_TURN_TICKS: i32 = 4096; + +/// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。 +/// +/// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约 +/// 9300 rpm ≈ 0.155 圈/ms，余量充足。 +#[derive(Debug, Clone, Default)] +pub struct TurnTracker { + revolutions: i32, + prev_raw: Option, + last_angle_rev: f32, +} + +impl TurnTracker { + /// 创建跟踪器。 + #[must_use] + pub const fn new() -> Self { + Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 } + } + + /// 送入新的原始角，返回连续累计角（圈）。 + pub fn update(&mut self, raw: u16) -> f32 { + if let Some(prev) = self.prev_raw { + let delta = i32::from(raw) - i32::from(prev); + if delta > HALF_TURN_TICKS { + // 正向跨越 8191 → 0。 + self.revolutions -= 1; + } else if delta < -HALF_TURN_TICKS { + // 反向跨越 0 → 8191。 + self.revolutions += 1; + } + } + self.prev_raw = Some(raw); + self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0; + self.last_angle_rev + } + + /// 最近一次 [`TurnTracker::update`] 的返回值。 + #[must_use] + pub const fn last_angle_rev(&self) -> f32 { + self.last_angle_rev + } + + /// 清零回到初始状态。 + pub fn reset(&mut self) { + *self = Self::new(); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn first_reading_is_raw_fraction() { + let mut t = TurnTracker::new(); + assert!((t.update(4096) - 0.5).abs() < 1e-4); + } + + #[test] + fn forward_wrap_counts_up() { + let mut t = TurnTracker::new(); + t.update(8000); + let after = t.update(100); // 8000 → 8191 → 0 → 100 + assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}"); + } + + #[test] + fn reverse_wrap_counts_down() { + let mut t = TurnTracker::new(); + t.update(100); + let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向） + assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}"); + } + + #[test] + fn small_deltas_do_not_wrap() { + let mut t = TurnTracker::new(); + t.update(0); + assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4); + assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4); + } + + #[test] + fn multi_turn_forward_then_reverse_returns() { + let mut t = TurnTracker::new(); + // 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。 + for _ in 0..3 { + t.update(0); + t.update(4000); + t.update(8000); + t.update(100); + } + let fwd = t.last_angle_rev(); + let expected_fwd = 3.0 + 100.0 / 8192.0; + assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}"); + + // 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。 + for _ in 0..3 { + t.update(8000); + t.update(4000); + t.update(0); + } + let back = t.last_angle_rev(); + assert!(back.abs() < 1e-4, "should return to 0, got {back}"); + } + + #[test] + fn jitter_at_boundary_stays_put() { + // 0 附近抖动不产生虚假圈数。 + let mut t = TurnTracker::new(); + t.update(8191); + assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次 + assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4); + assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回 + assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4); + } + + #[test] + fn reset_forgets_history() { + let mut t = TurnTracker::new(); + t.update(8000); + t.update(100); + t.reset(); + assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits()); + assert!((t.update(4096) - 0.5).abs() < 1e-4); + } +} diff --git a/crates/dji-esc/tests/closed_loop.rs b/crates/dji-esc/tests/closed_loop.rs new file mode 100644 index 0000000..ddb2103 --- /dev/null +++ b/crates/dji-esc/tests/closed_loop.rs @@ -0,0 +1,275 @@ +//! 闭环集成测试：`EscBus` + 内存通道 + 一阶电机仿真。 +//! +//! 全部可在任意平台（macOS / Linux / CI）运行，无需硬件。 + +#![allow(clippy::unwrap_used, clippy::expect_used)] + +use std::time::Duration; + +use dji_esc::{ + BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback, + encode_feedback_frame, spawn_plant, +}; +use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive}; +use tokio::time::{sleep, timeout}; + +/// 与 `PlantMotor::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。 +fn tuned(esc_id: u8, direction: f32) -> EscConfig { + let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction); + cfg.kp = 8.0; + cfg.ki = 2.0; + cfg.kd = 0.0; + cfg.feedforward = 16384.0 / 9254.0; + cfg +} + +/// 轮询直到条件满足或超时（毫秒，仿真时间）。 +async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool { + let mut waited = 0u64; + while waited < total_ms { + if cond() { + return true; + } + sleep(Duration::from_millis(5)).await; + waited += 5; + } + cond() +} + +/// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。 +async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) { + for &id in esc_ids { + let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 }; + peer.outgoing_tx.send(encode_feedback_frame(id, &fb)).await.unwrap(); + } +} + +async fn recv_command(peer: &mut CanLink) -> CanFrame { + timeout(Duration::from_millis(100), peer.incoming_rx.recv()) + .await + .expect("timed out waiting for command frame") + .expect("link closed") +} + +fn slot_current(frame: &CanFrame, slot: usize) -> i16 { + i16::from_be_bytes([frame.data[slot * 2], frame.data[slot * 2 + 1]]) +} + +// ---------------------------------------------------------------- 收敛性 + +#[tokio::test(start_paused = true)] +async fn velocity_loop_converges() { + let (bus_link, plant_link) = CanLink::pair(512); + let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1)); + let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + let motor = bus.motor(1).unwrap().clone(); + + motor.set_velocity(3000.0); + let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 2950.0).await; + assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm); + + // 稳态保持（速度无漂移、反馈持续流入）。 + let count_before = motor.state().feedback_count; + sleep(Duration::from_millis(300)).await; + let st = motor.state(); + assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}"); + assert!( + st.feedback_count >= count_before + 280, + "feedback should keep flowing: {} → {}", + count_before, + st.feedback_count + ); + + plant.abort(); + bus.shutdown().await; +} + +#[tokio::test(start_paused = true)] +async fn reversed_motor_converges_in_physical_frame() { + // direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。 + let (bus_link, plant_link) = CanLink::pair(512); + let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1)); + let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); + let motor = bus.motor(2).unwrap().clone(); + + motor.set_velocity(2000.0); + let ok = wait_until(2000, || motor.state().rotor_speed_rpm >= 1950.0).await; + assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm); + + let st = motor.state(); + assert!((1950.0..=2050.0).contains(&st.rotor_speed_rpm), "{st:?}"); + // 物理转速为正 ⇒ 电调电流为负 ⇒ 物理电流 = dir × esc > 0。 + assert!(st.effort_raw > 0, "{st:?}"); + + plant.abort(); + bus.shutdown().await; +} + +// ---------------------------------------------------------------- 安全 + +#[tokio::test(start_paused = true)] +async fn motor_without_feedback_commands_zero_current() { + let (bus_link, mut peer) = CanLink::pair(64); + // 不接 plant → 永远没有反馈。 + let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + + bus.motor(1).unwrap().set_velocity(5000.0); + for _ in 0..5 { + let frame = recv_command(&mut peer).await; + assert_eq!(frame.id, 0x200); + assert_eq!(frame.data, [0; 8], "must send zero current while offline"); + } + bus.shutdown().await; +} + +#[tokio::test(start_paused = true)] +async fn current_limit_clamps_saturation() { + let (bus_link, mut peer) = CanLink::pair(64); + let mut cfg = tuned(1, 1.0); + cfg.current_limit = 5000; + let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).unwrap(); + + send_feedback(&peer, &[1]).await; + bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和 + sleep(Duration::from_millis(5)).await; + + let mut saw_saturated = false; + for _ in 0..5 { + let frame = recv_command(&mut peer).await; + let raw = slot_current(&frame, 0); + assert!(raw <= 5000, "current {raw} exceeds limit 5000"); + saw_saturated |= raw == 5000; + } + assert!(saw_saturated, "loop should reach the configured limit"); + bus.shutdown().await; +} + +// ---------------------------------------------------------------- 组帧与路由 + +#[tokio::test(start_paused = true)] +async fn current_commands_route_to_group_slots() { + let (bus_link, mut peer) = CanLink::pair(256); + let configs = (1..=5).map(|id| tuned(id, 1.0)).collect(); + let bus = EscBus::spawn(bus_link, configs, BusOptions::default()).unwrap(); + + bus.motor(3).unwrap().set_effort_raw(1234); + bus.motor(5).unwrap().set_effort_raw(-567); + send_feedback(&peer, &(1..=5).collect::>()).await; + sleep(Duration::from_millis(5)).await; + + let mut saw_200 = false; + let mut saw_1ff = false; + for _ in 0..20 { + let frame = recv_command(&mut peer).await; + match frame.id { + 0x200 if !saw_200 => { + assert_eq!(frame.data, [0, 0, 0, 0, 0x04, 0xD2, 0, 0], "esc3 slot = 1234"); + saw_200 = true; + } + 0x1FF if !saw_1ff => { + // -567 = 0xFDC9（i16 补码，大端）。 + assert_eq!(frame.data, [0xFD, 0xC9, 0, 0, 0, 0, 0, 0], "esc5 slot = -567"); + saw_1ff = true; + } + _ => {} + } + if saw_200 && saw_1ff { + break; + } + } + assert!(saw_200, "no 0x200 frame observed"); + assert!(saw_1ff, "no 0x1FF frame observed"); + + // disable 后槽位清零。 + bus.motor(3).unwrap().disable(); + let zeroed = wait_until(200, || { + try_find_frame(&mut peer, |frame| frame.id == 0x200 && slot_current(frame, 2) == 0) + }) + .await; + assert!(zeroed, "esc3 slot should return to 0 after disable"); + bus.shutdown().await; +} + +/// 非阻塞地扫一眼通道里已有的帧。 +fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool { + while let Ok(frame) = peer.incoming_rx.try_recv() { + if pred(&frame) { + return true; + } + } + false +} + +// ---------------------------------------------------------------- 反馈 → 状态 + +#[tokio::test] +async fn feedback_updates_state_with_direction() { + let (bus_link, peer) = CanLink::pair(64); + let bus = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap(); + + // 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。 + let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 }; + peer.outgoing_tx.send(encode_feedback_frame(2, &fb)).await.unwrap(); + let fb2 = EscFeedback { angle_raw: 100, ..fb }; + peer.outgoing_tx.send(encode_feedback_frame(2, &fb2)).await.unwrap(); + sleep(Duration::from_millis(50)).await; + + let st = bus.motor(2).unwrap().state(); + assert_eq!(st.rotor_angle_raw, 100); + assert!((st.rotor_speed_rpm + 600.0).abs() < 1e-3); // direction = -1 + assert_eq!(st.effort_raw, 300); + assert_eq!(st.temperature_c, 47); + assert_eq!(st.feedback_count, 2); + let esc_rev = 1.0 + 100.0 / 8192.0; + assert!( + (st.rotor_revolutions + esc_rev).abs() < 1e-3, + "revolutions = {}", + st.rotor_revolutions + ); + // 输出轴换算由 `OutputShaft` 负责（见 motor-core gearbox 单测）。 + bus.shutdown().await; +} + +// ---------------------------------------------------------------- 生命周期 + +#[tokio::test(start_paused = true)] +async fn shutdown_stops_tasks_and_closes_link() { + let (bus_link, mut peer) = CanLink::pair(128); + // 不接 plant，直接验证任务生命周期。 + let bus = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap(); + bus.motor(1).unwrap().set_velocity(1000.0); + sleep(Duration::from_millis(20)).await; + // 有命令帧在流动。 + assert!(peer.incoming_rx.recv().await.is_some()); + + bus.shutdown().await; + // 排空残留缓冲帧后应立即得到 None（发送端已全部退出）； + // 若任务仍在运行，帧会持续流入并触发超时。 + loop { + match timeout(Duration::from_millis(100), peer.incoming_rx.recv()).await { + Ok(None) => break, + Ok(Some(_)) => {} // 排空残留缓冲帧 + Err(elapsed) => panic!("bus tasks still running after shutdown (waited {elapsed:?})"), + } + } +} + +#[test] +fn spawn_rejects_bad_configs() { + let link = || CanLink::pair(8).0; + assert_eq!( + EscBus::spawn(link(), vec![], BusOptions::default()).err(), + Some(EscBusError::NoMotors) + ); + assert_eq!( + EscBus::spawn(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(), + Some(EscBusError::DuplicateEscId(2)) + ); + let mut bad = tuned(1, 1.0); + bad.direction = 0.0; + let err = EscBus::spawn(link(), vec![bad], BusOptions::default()).err(); + assert!( + matches!(err, Some(EscBusError::InvalidConfig { .. })), + "expected InvalidConfig, got {err:?}" + ); +} diff --git a/crates/mecanum/Cargo.toml b/crates/mecanum/Cargo.toml new file mode 100644 index 0000000..fbcc7d0 --- /dev/null +++ b/crates/mecanum/Cargo.toml @@ -0,0 +1,16 @@ +[package] +name = "mecanum" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "Mecanum wheel chassis forward/inverse kinematics" + +[lints] +workspace = true + +[dependencies] + +[dev-dependencies] +proptest = { workspace = true } +approx = { workspace = true } diff --git a/crates/mecanum/proptest-regressions/lib.txt b/crates/mecanum/proptest-regressions/lib.txt new file mode 100644 index 0000000..993a75c --- /dev/null +++ b/crates/mecanum/proptest-regressions/lib.txt @@ -0,0 +1,7 @@ +# Seeds for failure cases proptest has generated in the past. It is +# automatically read and these particular cases re-run before any +# novel cases are generated. +# +# It is recommended to check this file in to source control so that +# everyone who runs the test benefits from these saved cases. +cc 698ba63bf948a3702145ece17ed0f014133de0d0b3797fd8bdda514c5d2480a7 # shrinks to vx = 0.0, vy = 0.0, omega = -2.3901656 diff --git a/crates/mecanum/src/lib.rs b/crates/mecanum/src/lib.rs new file mode 100644 index 0000000..b5f4614 --- /dev/null +++ b/crates/mecanum/src/lib.rs @@ -0,0 +1,284 @@ +//! 麦克纳姆轮底盘正 / 逆运动学。 +//! +//! # 约定 +//! +//! - 机器人坐标系（ISO）：x 前、y 左、ω 绕 z 逆时针为正； +//! - 轮序固定为 `[前左, 前右, 后左, 后右]`（常量 [`FL`] / [`FR`] / [`RL`] / +//! [`RR`]）； +//! - 辊子 45°、俯视呈 X 型布置（RoboMaster 底盘标准布局）； +//! - 角速度单位 rad/s，线速度 m/s，换算见 [`wheel_rpm_from_angular`]。 +//! +//! 四轮线速度（`a = half_length + half_width`）： +//! +//! ```text +//! v_FL = vx - vy - a·ω +//! v_FR = vx + vy + a·ω +//! v_RL = vx + vy - a·ω +//! v_RR = vx - vy + a·ω +//! ``` +//! +//! 正解为逆解的左伪逆（JᵀJ 对角，可解析求平均）。 + +use std::f32::consts::TAU; + +/// 前左轮下标。 +pub const FL: usize = 0; +/// 前右轮下标。 +pub const FR: usize = 1; +/// 后左轮下标。 +pub const RL: usize = 2; +/// 后右轮下标。 +pub const RR: usize = 3; + +/// 底盘平面速度：x 前进（m/s）、y 左移（m/s）、ω 逆时针（rad/s）。 +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct Twist { + /// 前进速度（m/s，前正）。 + pub vx: f32, + /// 横移速度（m/s，左正）。 + pub vy: f32, + /// 角速度（rad/s，逆时针正）。 + pub omega: f32, +} + +impl Twist { + /// 全零速度。 + pub const ZERO: Self = Self { vx: 0.0, vy: 0.0, omega: 0.0 }; + + /// 幅值限幅：任一分量超界时按比例收缩整体，保持运动方向不变。 + /// + /// `max_forward` / `max_strafe` 单位 m/s，`max_spin` 单位 rad/s。 + #[must_use] + pub fn clamped(self, max_forward: f32, max_strafe: f32, max_spin: f32) -> Self { + let scale = [ + (self.vx.abs() / max_forward).max(f32::EPSILON), + (self.vy.abs() / max_strafe).max(f32::EPSILON), + (self.omega.abs() / max_spin).max(f32::EPSILON), + ] + .into_iter() + .fold(1.0f32, f32::max); + Self { vx: self.vx / scale, vy: self.vy / scale, omega: self.omega / scale } + } +} + +/// 麦轮底盘几何参数。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct Mecanum { + /// 前后轮轴线到底盘中心的纵向半距 lx（米，正）。 + pub half_length: f32, + /// 左右轮到底盘中心的横向半距 ly（米，正）。 + pub half_width: f32, + /// 车轮半径（米，正）。 + pub wheel_radius: f32, +} + +impl Mecanum { + /// 按几何参数构造。 + /// + /// # Panics + /// + /// debug 构建下参数非正时 panic（配置层应先行校验）。 + #[must_use] + pub fn new(half_length: f32, half_width: f32, wheel_radius: f32) -> Self { + debug_assert!( + half_length > 0.0 && half_width > 0.0 && wheel_radius > 0.0, + "mecanum geometry must be positive" + ); + Self { half_length, half_width, wheel_radius } + } + + /// 逆解：底盘速度 → 四轮角速度（rad/s，顺序 FL/FR/RL/RR）。 + #[must_use] + pub fn wheel_speeds(&self, twist: &Twist) -> [f32; 4] { + let a = self.half_length + self.half_width; + [ + twist.vx - twist.vy - a * twist.omega, + twist.vx + twist.vy + a * twist.omega, + twist.vx + twist.vy - a * twist.omega, + twist.vx - twist.vy + a * twist.omega, + ] + .map(|v| v / self.wheel_radius) + } + + /// 正解：四轮角速度（rad/s）→ 底盘速度（最小二乘意义）。 + #[must_use] + pub fn twist(&self, wheels: &[f32; 4]) -> Twist { + let w = wheels.map(|x| x * self.wheel_radius); + let a = self.half_length + self.half_width; + Twist { + vx: (w[FL] + w[FR] + w[RL] + w[RR]) / 4.0, + vy: (-w[FL] + w[FR] + w[RL] - w[RR]) / 4.0, + omega: (-w[FL] + w[FR] - w[RL] + w[RR]) / (4.0 * a), + } + } + + /// 逆解并直接输出四轮转速（rpm）。 + #[must_use] + pub fn wheel_rpms(&self, twist: &Twist) -> [f32; 4] { + self.wheel_speeds(twist).map(wheel_rpm_from_angular) + } + + /// 由四轮转速（rpm）正解底盘速度。 + #[must_use] + pub fn twist_from_wheel_rpms(&self, wheel_rpms: &[f32; 4]) -> Twist { + self.twist(&wheel_rpms.map(wheel_angular_from_rpm)) + } +} + +/// 轮角速度（rad/s）→ 转速（rpm）。 +#[must_use] +pub const fn wheel_rpm_from_angular(rad_per_s: f32) -> f32 { + rad_per_s * 60.0 / TAU +} + +/// 轮转速（rpm）→ 角速度（rad/s）。 +#[must_use] +pub const fn wheel_angular_from_rpm(rpm: f32) -> f32 { + rpm * TAU / 60.0 +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + + /// 0.2m 半轴距 / 0.25m 半轮距 / 76.2mm 轮半径的典型 RM 底盘。 + fn chassis() -> Mecanum { + Mecanum::new(0.2, 0.25, 0.0762) + } + + fn assert_twist_eq(a: &Twist, b: &Twist) { + assert_relative_eq!(a.vx, b.vx, epsilon = 1e-5); + assert_relative_eq!(a.vy, b.vy, epsilon = 1e-5); + assert_relative_eq!(a.omega, b.omega, epsilon = 1e-5); + } + + #[test] + fn pure_forward_drives_all_wheels_equally() { + let m = chassis(); + let w = m.wheel_speeds(&Twist { vx: 1.0, ..Twist::ZERO }); + for x in w { + assert_relative_eq!(x, 1.0 / 0.0762, epsilon = 1e-5); + } + } + + #[test] + fn pure_strafe_left_opposes_diagonal_wheels() { + let m = chassis(); + let w = m.wheel_speeds(&Twist { vy: 2.0, ..Twist::ZERO }); + let v = 2.0 / 0.0762; + assert_relative_eq!(w[FL], -v, epsilon = 1e-5); + assert_relative_eq!(w[FR], v, epsilon = 1e-5); + assert_relative_eq!(w[RL], v, epsilon = 1e-5); + assert_relative_eq!(w[RR], -v, epsilon = 1e-5); + } + + #[test] + fn pure_rotation_splits_left_right() { + let m = chassis(); + let w = m.wheel_speeds(&Twist { omega: 1.0, ..Twist::ZERO }); + let v = (0.2 + 0.25) / 0.0762; + // 逆时针：左侧后退、右侧前进。 + assert_relative_eq!(w[FL], -v, epsilon = 1e-5); + assert_relative_eq!(w[FR], v, epsilon = 1e-5); + assert_relative_eq!(w[RL], -v, epsilon = 1e-5); + assert_relative_eq!(w[RR], v, epsilon = 1e-5); + } + + #[test] + fn diagonal_motion_idles_two_wheels() { + // 前左 45° 对角线：vx = vy ⇒ FL、RR 停转。 + let m = chassis(); + let w = m.wheel_speeds(&Twist { vx: 1.0, vy: 1.0, omega: 0.0 }); + assert_relative_eq!(w[FL], 0.0, epsilon = 1e-6); + assert_relative_eq!(w[RR], 0.0, epsilon = 1e-6); + assert_relative_eq!(w[FR], 2.0 / 0.0762, epsilon = 1e-5); + assert_relative_eq!(w[RL], 2.0 / 0.0762, epsilon = 1e-5); + } + + #[test] + fn zero_twist_zero_wheels() { + let wheels = chassis().wheel_speeds(&Twist::ZERO); + assert!(wheels.iter().all(|w| f64::from(*w).abs() < 1e-12), "{wheels:?}"); + } + + #[test] + fn inverse_then_forward_recovers_twist() { + let m = chassis(); + let t = Twist { vx: 1.5, vy: -0.8, omega: 2.2 }; + assert_twist_eq(&m.twist(&m.wheel_speeds(&t)), &t); + } + + #[test] + fn rpm_helpers_roundtrip() { + assert_relative_eq!( + wheel_rpm_from_angular(wheel_angular_from_rpm(482.0)), + 482.0, + epsilon = 1e-4 + ); + assert_relative_eq!(wheel_angular_from_rpm(60.0 / TAU), 1.0, epsilon = 1e-6); + } + + #[test] + fn rpm_path_matches_angular_path() { + let m = chassis(); + let t = Twist { vx: 0.3, vy: 1.1, omega: -0.7 }; + assert_twist_eq(&m.twist_from_wheel_rpms(&m.wheel_rpms(&t)), &t); + } + + #[test] + fn clamp_preserves_direction_and_limits() { + let t = Twist { vx: 4.0, vy: 1.0, omega: 0.5 }.clamped(2.0, 2.0, 3.0); + assert_relative_eq!(t.vx, 2.0, epsilon = 1e-6); + assert_relative_eq!(t.vy, 0.5, epsilon = 1e-6); + assert_relative_eq!(t.omega, 0.25, epsilon = 1e-6); + // 限内不缩放。 + let keep = Twist { vx: 1.0, vy: -1.0, omega: 2.0 }.clamped(2.0, 2.0, 3.0); + assert_eq!(keep, Twist { vx: 1.0, vy: -1.0, omega: 2.0 }); + } +} + +#[cfg(test)] +mod proptests { + use super::*; + use approx::assert_relative_eq; + use proptest::prelude::*; + + proptest! { + /// 正逆解互逆：forward(inverse(t)) == t（对任意合理几何与速度）。 + #[test] + fn kinematics_roundtrip( + lx in 0.05f32..0.5, + ly in 0.05f32..0.5, + r in 0.03f32..0.2, + vx in -3.0f32..3.0, + vy in -3.0f32..3.0, + omega in -6.0f32..6.0, + ) { + let m = Mecanum::new(lx, ly, r); + let t = Twist { vx, vy, omega }; + let back = m.twist(&m.wheel_speeds(&t)); + assert_relative_eq!(back.vx, t.vx, epsilon = 1e-4); + assert_relative_eq!(back.vy, t.vy, epsilon = 1e-4); + assert_relative_eq!(back.omega, t.omega, epsilon = 1e-4); + } + + /// 左右镜像对称：vy、ω 取反（镜像底盘）等价于 FL↔FR、RL↔RR 对调。 + #[test] + fn strafe_symmetry( + vx in -2.0f32..2.0, + vy in -2.0f32..2.0, + omega in -4.0f32..4.0, + ) { + let m = Mecanum::new(0.2, 0.25, 0.0762); + let t = Twist { vx, vy, omega }; + let mirrored = Twist { vx, vy: -vy, omega: -omega }; + let w = m.wheel_speeds(&t); + let w2 = m.wheel_speeds(&mirrored); + assert_relative_eq!(w[FL], w2[FR], epsilon = 1e-4); + assert_relative_eq!(w[FR], w2[FL], epsilon = 1e-4); + assert_relative_eq!(w[RL], w2[RR], epsilon = 1e-4); + assert_relative_eq!(w[RR], w2[RL], epsilon = 1e-4); + } + } +} diff --git a/crates/motor-core/Cargo.toml b/crates/motor-core/Cargo.toml new file mode 100644 index 0000000..7daf5f3 --- /dev/null +++ b/crates/motor-core/Cargo.toml @@ -0,0 +1,12 @@ +[package] +name = "motor-core" +version.workspace = true +edition.workspace = true +rust-version.workspace = true +license.workspace = true +description = "Protocol-agnostic motor abstraction: Motor trait, feedback state, and PID controllers" + +[lints] +workspace = true + +[dependencies] diff --git a/crates/motor-core/src/command.rs b/crates/motor-core/src/command.rs new file mode 100644 index 0000000..70388f4 --- /dev/null +++ b/crates/motor-core/src/command.rs @@ -0,0 +1,41 @@ +//! 驱动层命令寄存器（最后写入胜出）。 + +/// 发给驱动层的电机命令。 +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum Command { + /// 停用：目标清零，下发 0 输出。 + Disabled, + /// 直接下发开环 raw（协议量程，物理方向），绕过闭环。 + /// ESC = 电流 raw；GM6020 = 电压 raw。 + EffortRaw(i16), + /// 速度环跟踪目标转子转速（rpm，物理方向）。 + Velocity(f32), + /// 位置环跟踪目标转子多圈位置（圈，物理方向）。 + Position(f32), +} + +/// 命令模式标签（不含设定值）——用于切换时复位控制器。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum CommandMode { + /// 停用。 + Disabled, + /// 开环 raw。 + EffortRaw, + /// 速度环。 + Velocity, + /// 位置环。 + Position, +} + +impl Command { + /// 模式标签。 + #[must_use] + pub const fn mode(self) -> CommandMode { + match self { + Self::Disabled => CommandMode::Disabled, + Self::EffortRaw(_) => CommandMode::EffortRaw, + Self::Velocity(_) => CommandMode::Velocity, + Self::Position(_) => CommandMode::Position, + } + } +} diff --git a/crates/motor-core/src/gearbox.rs b/crates/motor-core/src/gearbox.rs new file mode 100644 index 0000000..f423f3b --- /dev/null +++ b/crates/motor-core/src/gearbox.rs @@ -0,0 +1,173 @@ +//! 输出轴单位适配器：减速比换算。 +//! +//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。 +//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。 + +use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive}; + +/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。 +pub struct OutputShaft { + inner: T, + gear_ratio: f32, +} + +impl OutputShaft { + /// 包装底层电机。`gear_ratio` = 转子 : 输出轴（> 0）。 + /// + /// # Panics + /// + /// `gear_ratio` 非有限或 ≤ 0 时 panic。 + #[must_use] + pub fn new(inner: T, gear_ratio: f32) -> Self { + assert!( + gear_ratio.is_finite() && gear_ratio > 0.0, + "gear_ratio must be > 0, got {gear_ratio}" + ); + Self { inner, gear_ratio } + } + + /// 减速比。 + #[must_use] + pub fn gear_ratio(&self) -> f32 { + self.gear_ratio + } + + /// 底层电机引用。 + #[must_use] + pub fn inner(&self) -> &T { + &self.inner + } + + /// 转子量 → 输出轴量。 + #[must_use] + pub fn to_output(&self, rotor: f32) -> f32 { + rotor / self.gear_ratio + } + + /// 输出轴量 → 转子量。 + #[must_use] + pub fn to_rotor(&self, output: f32) -> f32 { + output * self.gear_ratio + } + + /// 由反馈快照推算输出轴转速（rpm）。 + #[must_use] + pub fn output_speed_rpm(&self, state: &MotorState) -> f32 { + state.rotor_speed_rpm / self.gear_ratio + } +} + +impl MotorFeedback for OutputShaft { + fn id(&self) -> u8 { + self.inner.id() + } + + fn state(&self) -> MotorState { + // 只透传转子量；不伪造输出轴字段。 + self.inner.state() + } +} + +impl VelocityDrive for OutputShaft { + /// `output_rpm` 为**输出轴**转速。 + fn set_velocity(&self, output_rpm: f32) { + self.inner.set_velocity(self.to_rotor(output_rpm)); + } +} + +impl EffortDrive for OutputShaft { + fn set_effort_raw(&self, raw: i16) { + self.inner.set_effort_raw(raw); + } +} + +impl PositionDrive for OutputShaft { + /// `output_rev` 为**输出轴**圈数。 + fn set_position(&self, output_rev: f32) { + self.inner.set_position(self.to_rotor(output_rev)); + } +} + +impl Disable for OutputShaft { + fn disable(&self) { + self.inner.disable(); + } +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::Command; + + struct Stub { + cmd: std::sync::Mutex, + } + + impl Stub { + fn new() -> Self { + Self { cmd: std::sync::Mutex::new(Command::Disabled) } + } + } + + impl MotorFeedback for Stub { + fn id(&self) -> u8 { + 1 + } + fn state(&self) -> MotorState { + MotorState::default() + } + } + impl VelocityDrive for Stub { + fn set_velocity(&self, rotor_rpm: f32) { + *self.cmd.lock().unwrap() = Command::Velocity(rotor_rpm); + } + } + impl EffortDrive for Stub { + fn set_effort_raw(&self, raw: i16) { + *self.cmd.lock().unwrap() = Command::EffortRaw(raw); + } + } + impl PositionDrive for Stub { + fn set_position(&self, rotor_rev: f32) { + *self.cmd.lock().unwrap() = Command::Position(rotor_rev); + } + } + impl Disable for Stub { + fn disable(&self) { + *self.cmd.lock().unwrap() = Command::Disabled; + } + } + + #[test] + fn velocity_scales_by_gear() { + let shaft = OutputShaft::new(Stub::new(), 19.203); + shaft.set_velocity(100.0); + match *shaft.inner().cmd.lock().unwrap() { + Command::Velocity(rpm) => assert!((rpm - 1920.3).abs() < 0.1, "{rpm}"), + ref c => panic!("unexpected {c:?}"), + } + } + + #[test] + fn position_scales_by_gear() { + let shaft = OutputShaft::new(Stub::new(), 36.0); + shaft.set_position(2.0); + match *shaft.inner().cmd.lock().unwrap() { + Command::Position(rev) => assert!((rev - 72.0).abs() < 1e-3, "{rev}"), + ref c => panic!("unexpected {c:?}"), + } + } + + #[test] + fn effort_passes_through() { + let shaft = OutputShaft::new(Stub::new(), 10.0); + shaft.set_effort_raw(-1234); + assert_eq!(*shaft.inner().cmd.lock().unwrap(), Command::EffortRaw(-1234)); + } + + #[test] + #[should_panic = "gear_ratio"] + fn rejects_nonpositive_gear() { + let _ = OutputShaft::new(Stub::new(), 0.0); + } +} diff --git a/crates/motor-core/src/heading.rs b/crates/motor-core/src/heading.rs new file mode 100644 index 0000000..d605cc0 --- /dev/null +++ b/crates/motor-core/src/heading.rs @@ -0,0 +1,21 @@ +//! 航向观测源（IMU / 磁力计 / 视觉等）——云台绝对角融合 seam。 +//! +//! 电机抽象只保证 [`crate::MotorState::rotor_revolutions`] 连续（上电相对）。 +//! 场地绝对角由上层（未来 `gimbal` crate）融合 IMU 与电机多圈得到。 + +use std::future::Future; + +/// 绝对航向观测源。 +pub trait HeadingSource: Send { + /// 机体航向角（rad，连续；与 chassis ISO 约定一致：x 前、逆时针为正）。 + fn heading_rad(&self) -> impl Future + Send; +} + +/// 云台目标角的参考系。 +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum HeadingFrame { + /// 电机转子多圈（上电相对）。 + RotorMultiTurn, + /// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。 + AbsoluteField, +} diff --git a/crates/motor-core/src/lib.rs b/crates/motor-core/src/lib.rs new file mode 100644 index 0000000..d29624f --- /dev/null +++ b/crates/motor-core/src/lib.rs @@ -0,0 +1,93 @@ +//! 协议无关的电机抽象层。 +//! +//! 本 crate 不做任何 I/O：它定义电机能力 trait（[`MotorFeedback`] / +//! [`EffortDrive`] / [`VelocityDrive`] / [`PositionDrive`] / [`Disable`]）、 +//! 公共数据类型（[`MotorState`]、[`Command`]）以及纯控制算法（[`Pid`]、 +//! [`VelocityController`]、[`PositionController`]）。具体总线协议在其上实现。 +//! +//! # 单一职责 +//! +//! - [`traits`]：能力面（小而正交）； +//! - [`state`] / [`command`]：反馈快照与命令寄存器； +//! - [`pid`] / [`velocity`] / [`position`]：纯控制律； +//! - [`gearbox`]：减速比单位适配（输出轴 ↔ 转子）； +//! - [`heading`]：航向观测 seam（云台绝对角融合用）； +//! - [`testing`]：测试替身。 +//! +//! # 单位与方向约定 +//! +//! - 所有呈现给上层的量都是**安装方向修正后**的物理量：`direction = -1` +//! 的电机由驱动层在读写两个方向上统一翻转符号。 +//! - 转速一律为电机**转子**转速（rpm）；输出轴换算见 [`OutputShaft`]。 +//! - 开环控制量以协议原始值（raw）为主（C620 电流：±16384 ↔ ±20 A； +//! GM6020 电压：±30000 ≈ 满电压）。 +#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))] + +pub mod command; +pub mod gearbox; +pub mod heading; +pub mod pid; +pub mod position; +pub mod state; +pub mod testing; +pub mod traits; +pub mod velocity; + +pub use command::{Command, CommandMode}; +pub use gearbox::OutputShaft; +pub use heading::{HeadingFrame, HeadingSource}; +pub use pid::Pid; +pub use position::{PositionController, PositionLoop}; +pub use state::MotorState; +pub use testing::{MockHeading, MockMotor}; +pub use traits::{Disable, EffortDrive, Motor, MotorFeedback, PositionDrive, VelocityDrive}; +pub use velocity::VelocityController; + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn default_state_has_no_feedback() { + let state = MotorState::default(); + assert!(!state.has_feedback()); + assert_eq!(state.feedback_count, 0); + } + + #[test] + fn feedback_gate_flips_on_first_frame() { + let state = MotorState { feedback_count: 1, ..MotorState::default() }; + assert!(state.has_feedback()); + } + + #[test] + fn command_variants_roundtrip_by_value() { + let cmds = [ + Command::Disabled, + Command::EffortRaw(-16384), + Command::Velocity(4820.5), + Command::Position(2.5), + ]; + assert_eq!(cmds[0], Command::Disabled); + assert_eq!(cmds[1], Command::EffortRaw(-16384)); + assert_eq!(cmds[2], Command::Velocity(4820.5)); + assert_eq!(cmds[3], Command::Position(2.5)); + } + + #[test] + fn command_mode_labels() { + assert_eq!(Command::Disabled.mode(), CommandMode::Disabled); + assert_eq!(Command::EffortRaw(0).mode(), CommandMode::EffortRaw); + assert_eq!(Command::Velocity(0.0).mode(), CommandMode::Velocity); + assert_eq!(Command::Position(0.0).mode(), CommandMode::Position); + } + + #[test] + fn mock_motor_is_full_motor() { + fn takes_motor(m: &impl Motor) { + m.set_velocity(1.0); + m.disable(); + } + takes_motor(&MockMotor::new(1)); + } +} diff --git a/crates/motor-core/src/pid.rs b/crates/motor-core/src/pid.rs new file mode 100644 index 0000000..19adac2 --- /dev/null +++ b/crates/motor-core/src/pid.rs @@ -0,0 +1,264 @@ +//! 通用 PID 控制器。 + +/// 并联式 PID 控制器（f32，显式时间步）。 +/// +/// 特性： +/// - 输出限幅 `[out_min, out_max]`； +/// - 积分抗饱和（clamping：输出已饱和且误差继续同向时暂停积分）； +/// - 微分作用于**测量值**（避免设定值阶跃的微分冲击），可选一阶低通滤波。 +/// +/// 时间步 `dt` 由调用方显式传入（秒）。`dt <= 0` 或非有限值时本次调用 +/// 直接返回上次输出且不更新内部状态（防御跳帧 / 首帧场景）。 +#[derive(Debug, Clone)] +pub struct Pid { + kp: f32, + ki: f32, + kd: f32, + out_min: f32, + out_max: f32, + /// 微分低通时间常数（秒）；`0` 关闭滤波。 + derivative_tau: f32, + integral: f32, + prev_measurement: Option, + derivative_filtered: f32, + last_output: f32, +} + +impl Pid { + /// 创建 PID。`out_min` 必须小于 `out_max`。 + /// + /// # Panics + /// + /// 参数非有限或 `out_min >= out_max` 时 panic。 + #[must_use] + pub fn new(kp: f32, ki: f32, kd: f32, out_min: f32, out_max: f32) -> Self { + assert!( + out_min < out_max && [kp, ki, kd, out_min, out_max].iter().all(|v| v.is_finite()), + "invalid PID parameters" + ); + Self { + kp, + ki, + kd, + out_min, + out_max, + derivative_tau: 0.0, + integral: 0.0, + prev_measurement: None, + derivative_filtered: 0.0, + last_output: 0.0, + } + } + + /// 设置微分低通时间常数（秒），`0` 关闭滤波。链式调用。 + #[must_use] + pub fn with_derivative_tau(mut self, tau: f32) -> Self { + self.derivative_tau = tau.max(0.0); + self + } + + /// 单步更新，返回限幅后的输出。 + pub fn update(&mut self, setpoint: f32, measurement: f32, dt: f32) -> f32 { + // NaN 与 +inf 的比较结果均为 false，故 `dt > 0` 已隐含有限性。 + let input_valid = dt > 0.0 && setpoint.is_finite() && measurement.is_finite(); + if !input_valid { + return self.last_output; + } + let error = setpoint - measurement; + + // 微分作用于测量值（反向），一阶低通滤波。 + let d_raw = match self.prev_measurement { + Some(prev) => -(measurement - prev) / dt, + None => 0.0, + }; + if self.derivative_tau > 0.0 { + let alpha = dt / (self.derivative_tau + dt); + self.derivative_filtered += alpha * (d_raw - self.derivative_filtered); + } else { + self.derivative_filtered = d_raw; + } + let d_term = self.kd * self.derivative_filtered; + + // 积分抗饱和（clamping）：仅在不会把输出进一步推入饱和时积分。 + let i_candidate = self.integral + self.ki * error * dt; + let unsat = self.kp * error + i_candidate + d_term; + let saturating_up = unsat > self.out_max && error > 0.0; + let saturating_down = unsat < self.out_min && error < 0.0; + if !saturating_up && !saturating_down { + self.integral = i_candidate; + } + + let output = (self.kp * error + self.integral + d_term).clamp(self.out_min, self.out_max); + self.prev_measurement = Some(measurement); + self.last_output = output; + output + } + + /// 清空积分、微分记忆与上次输出。 + pub fn reset(&mut self) { + self.integral = 0.0; + self.prev_measurement = None; + self.derivative_filtered = 0.0; + self.last_output = 0.0; + } + + /// 当前积分累加项。 + #[must_use] + pub fn integral(&self) -> f32 { + self.integral + } + + /// 比例增益。 + #[must_use] + pub fn kp(&self) -> f32 { + self.kp + } + + /// 积分增益。 + #[must_use] + pub fn ki(&self) -> f32 { + self.ki + } + + /// 微分增益。 + #[must_use] + pub fn kd(&self) -> f32 { + self.kd + } + + /// 输出下限。 + #[must_use] + pub fn output_min(&self) -> f32 { + self.out_min + } + + /// 输出上限。 + #[must_use] + pub fn output_max(&self) -> f32 { + self.out_max + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const DT: f32 = 0.001; + + #[test] + fn pure_proportional() { + let mut pid = Pid::new(2.0, 0.0, 0.0, -100.0, 100.0); + let out = pid.update(10.0, 4.0, DT); + assert!((out - 12.0).abs() < 1e-4, "out = {out}"); + } + + #[test] + fn output_clamps_both_ends() { + let mut pid = Pid::new(100.0, 0.0, 0.0, -10.0, 10.0); + assert!((pid.update(1.0, 0.0, DT) - 10.0).abs() < 1e-5); + assert!((pid.update(-1.0, 0.0, DT) - (-10.0)).abs() < 1e-5); + } + + #[test] + fn integral_accumulates() { + let mut pid = Pid::new(0.0, 10.0, 0.0, -1e6, 1e6); + pid.update(1.0, 0.0, DT); // integral += 10 * 1 * 0.001 + pid.update(1.0, 0.0, DT); + let out = pid.update(1.0, 0.0, DT); + let expected = 10.0 * 1.0 * DT * 3.0; + assert!((out - expected).abs() < 1e-4, "out = {out}, expected {expected}"); + } + + #[test] + fn anti_windup_bounds_integral_when_saturated() { + // 大误差持续饱和 200 步：kp = 0 时输出即积分项。clamping 抗饱和要求 + // 积分不超过 out_max，且进入保持区后输出不再增长。 + let ki = 5.0; + let mut pid = Pid::new(0.0, ki, 0.0, -16384.0, 16384.0); + for _ in 0..200 { + pid.update(1e6, 0.0, DT); + } + assert!(pid.integral() <= 16384.0, "integral = {}", pid.integral()); + let frozen = pid.update(1e6, 0.0, DT); + let still = pid.update(1e6, 0.0, DT); + assert!( + (frozen - still).abs() < 1e-3, + "saturated output must stop growing: {frozen} vs {still}" + ); + } + + #[test] + fn anti_windup_recovers_quickly_on_reverse() { + // 饱和后目标反向，输出应在一个反向误差步内脱离正饱和。 + let mut pid = Pid::new(1.0, 10.0, 0.0, -100.0, 100.0); + for _ in 0..500 { + pid.update(1000.0, 0.0, DT); + } + assert!((pid.update(1000.0, 0.0, DT) - 100.0).abs() < 1e-3); + // 一步反向大误差（kp=1, e=-1000）足以击穿下限，说明无深度饱和。 + let out = pid.update(-1000.0, 0.0, DT); + assert!(out <= 0.0, "expected non-positive output, got {out}"); + } + + #[test] + fn derivative_acts_on_measurement() { + // 设定值恒 0，测量值阶跃：输出应为负向微分冲击（-kd*Δ/dt）。 + let mut pid = Pid::new(0.0, 0.0, 0.01, -1e6, 1e6); + pid.update(0.0, 0.0, DT); // 建立初值，首帧微分为 0 + let out = pid.update(0.0, 0.5, DT); + let expected = -0.01 * (0.5 / DT); + assert!((out - expected).abs() < 1.0, "out = {out}, expected {expected}"); + } + + #[test] + fn derivative_filter_softens_spike() { + let step = |tau: f32| { + let mut pid = Pid::new(0.0, 0.0, 10.0, -1e9, 1e9).with_derivative_tau(tau); + pid.update(0.0, 0.0, DT); + pid.update(0.0, 1.0, DT).abs() + }; + let unfiltered = step(0.0); + let filtered = step(0.05); + assert!( + filtered < unfiltered * 0.05, + "filtered {filtered} should be far below unfiltered {unfiltered}" + ); + } + + #[test] + fn nonpositive_dt_returns_last_output_and_keeps_state() { + let mut pid = Pid::new(1.0, 1.0, 0.0, -100.0, 100.0); + let first = pid.update(10.0, 0.0, DT); + let integral_before = pid.integral().to_bits(); + // 逐位相等：无效输入必须原样返回上次输出。 + assert_eq!(pid.update(99.0, 0.0, 0.0).to_bits(), first.to_bits()); + assert_eq!(pid.update(99.0, 0.0, f32::NAN).to_bits(), first.to_bits()); + assert_eq!(pid.integral().to_bits(), integral_before); + } + + #[test] + fn nan_input_returns_last_output() { + let mut pid = Pid::new(1.0, 0.0, 0.0, -100.0, 100.0); + let first = pid.update(1.0, 0.0, DT); + assert_eq!(pid.update(f32::NAN, 0.0, DT).to_bits(), first.to_bits()); + assert_eq!(pid.update(1.0, f32::INFINITY, DT).to_bits(), first.to_bits()); + } + + #[test] + fn reset_clears_history() { + let mut pid = Pid::new(1.0, 10.0, 1.0, -1e6, 1e6); + pid.update(5.0, 1.0, DT); + pid.update(5.0, 1.0, DT); + pid.reset(); + assert_eq!(pid.integral().to_bits(), 0.0f32.to_bits()); + // 重置后首帧微分为 0，输出 = P 项 + 本步积分（ki*e*dt = 0.04）。 + let out = pid.update(5.0, 1.0, DT); + assert!((out - 4.04).abs() < 1e-4, "out = {out}"); + } + + #[test] + #[should_panic = "invalid PID parameters"] + fn new_rejects_inverted_limits() { + let _pid = Pid::new(1.0, 0.0, 0.0, 1.0, -1.0); + } +} diff --git a/crates/motor-core/src/position.rs b/crates/motor-core/src/position.rs new file mode 100644 index 0000000..8409066 --- /dev/null +++ b/crates/motor-core/src/position.rs @@ -0,0 +1,103 @@ +//! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。 + +use crate::pid::Pid; +use crate::velocity::VelocityController; + +/// 位置环拓扑。 +#[derive(Debug, Clone)] +pub enum PositionLoop { + /// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。 + Direct(Pid), + /// 级联：位置 → 目标转速 → 速度环 → effort_raw。 + Cascade { + /// 外环位置 PID（输出限幅 = 最大 rpm）。 + pos: Pid, + /// 内环速度控制器（输出限幅 = 最大 effort）。 + vel: VelocityController, + }, +} + +/// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。 +#[derive(Debug, Clone)] +pub struct PositionController { + inner: PositionLoop, +} + +impl PositionController { + /// 单环位置控制器。 + #[must_use] + pub fn direct(pid: Pid) -> Self { + Self { inner: PositionLoop::Direct(pid) } + } + + /// 级联位置控制器（限速）。 + #[must_use] + pub fn cascade(pos: Pid, vel: VelocityController) -> Self { + Self { inner: PositionLoop::Cascade { pos, vel } } + } + + /// 单步更新，返回 effort_raw（已含限幅）。 + /// + /// `measured_rpm` 仅级联内环使用；单环忽略该参数。 + pub fn update( + &mut self, + target_rev: f32, + measured_rev: f32, + measured_rpm: f32, + dt: f32, + ) -> f32 { + match &mut self.inner { + PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt), + PositionLoop::Cascade { pos, vel } => { + let vel_target = pos.update(target_rev, measured_rev, dt); + vel.update(vel_target, measured_rpm, dt) + } + } + } + + /// 清空内部状态（切换模式 / 重新使能时调用）。 + pub fn reset(&mut self) { + match &mut self.inner { + PositionLoop::Direct(pid) => pid.reset(), + PositionLoop::Cascade { pos, vel } => { + pos.reset(); + vel.reset(); + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const DT: f32 = 0.001; + + #[test] + fn direct_outputs_pid() { + let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0)); + let out = pc.update(1.0, 0.5, 0.0, DT); + assert!((out - 1.0).abs() < 1e-4, "out = {out}"); + } + + #[test] + fn cascade_limits_velocity_then_effort() { + // 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。 + let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0); + let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0); + let mut pc = PositionController::cascade(pos, vel); + let out = pc.update(1.0, 0.0, 0.0, DT); + // 外环饱和到 5 rpm，内环 ff=2 → 10 raw。 + assert!((out - 10.0).abs() < 1e-3, "out = {out}"); + } + + #[test] + fn reset_clears_state() { + let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6)); + pc.update(1.0, 0.0, 0.0, DT); + pc.reset(); + let out = pc.update(1.0, 0.0, 0.0, DT); + // 重置后只剩本步积分：10 * 1 * 0.001。 + assert!((out - 0.01).abs() < 1e-5, "out = {out}"); + } +} diff --git a/crates/motor-core/src/state.rs b/crates/motor-core/src/state.rs new file mode 100644 index 0000000..77d0727 --- /dev/null +++ b/crates/motor-core/src/state.rs @@ -0,0 +1,29 @@ +//! 电机反馈快照（协议无关，方向修正后的物理量）。 +//! +//! 只含**转子轴**物理量——减速比 / 输出轴换算由 [`crate::gearbox::OutputShaft`] +//! 或上层完成，不在此泄漏底盘语义。 + +/// 电机反馈快照。 +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct MotorState { + /// 最近一帧反馈的转子机械原始角（编码器计数，量程由具体协议决定）。 + pub rotor_angle_raw: u16, + /// 连续累计转子角（多圈回绕处理，单位：圈；随时间单调于转动方向）。 + pub rotor_revolutions: f32, + /// 转子转速（rpm，物理方向）。 + pub rotor_speed_rpm: f32, + /// 开环控制量反馈 raw（ESC=转矩电流，GM6020=电流相关量；物理方向）。 + pub effort_raw: i16, + /// 电机温度（℃）。 + pub temperature_c: i8, + /// 已收到的反馈帧总数；`0` 表示尚未收到任何反馈（上电门控用）。 + pub feedback_count: u64, +} + +impl MotorState { + /// 是否已收到过反馈（上电后首帧门控）。 + #[must_use] + pub fn has_feedback(&self) -> bool { + self.feedback_count > 0 + } +} diff --git a/crates/motor-core/src/testing.rs b/crates/motor-core/src/testing.rs new file mode 100644 index 0000000..c4a55a3 --- /dev/null +++ b/crates/motor-core/src/testing.rs @@ -0,0 +1,116 @@ +//! 测试替身：内存电机与航向源。 + +use std::future::Future; +use std::sync::{Arc, Mutex, MutexGuard}; + +use crate::{ + Command, Disable, EffortDrive, HeadingSource, MotorFeedback, MotorState, PositionDrive, + VelocityDrive, +}; + +/// 锁辅助：中毒锁直接恢复内部数据。 +fn lock(mutex: &Mutex) -> MutexGuard<'_, T> { + mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner) +} + +/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。 +/// +/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。 +#[derive(Clone)] +pub struct MockMotor { + id: u8, + published: Arc>, +} + +impl MockMotor { + /// 创建指定 ID 的空电机。 + #[must_use] + pub fn new(id: u8) -> Self { + Self { id, published: Arc::new(Mutex::new((MotorState::default(), Command::Disabled))) } + } + + /// 注入反馈快照。 + pub fn inject_state(&self, state: MotorState) { + lock(&self.published).0 = state; + } + + /// 最近一次命令。 + #[must_use] + pub fn last_command(&self) -> Command { + lock(&self.published).1 + } +} + +impl MotorFeedback for MockMotor { + fn id(&self) -> u8 { + self.id + } + fn state(&self) -> MotorState { + lock(&self.published).0 + } +} + +impl VelocityDrive for MockMotor { + fn set_velocity(&self, rotor_rpm: f32) { + lock(&self.published).1 = Command::Velocity(rotor_rpm); + } +} + +impl EffortDrive for MockMotor { + fn set_effort_raw(&self, raw: i16) { + lock(&self.published).1 = Command::EffortRaw(raw); + } +} + +impl PositionDrive for MockMotor { + fn set_position(&self, rotor_rev: f32) { + lock(&self.published).1 = Command::Position(rotor_rev); + } +} + +impl Disable for MockMotor { + fn disable(&self) { + lock(&self.published).1 = Command::Disabled; + } +} + +/// 恒定航向测试源。 +pub struct MockHeading { + rad: f32, +} + +impl MockHeading { + /// 创建固定航向源。 + #[must_use] + pub const fn new(rad: f32) -> Self { + Self { rad } + } +} + +impl HeadingSource for MockHeading { + fn heading_rad(&self) -> impl Future + Send { + std::future::ready(self.rad) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn mock_motor_records_commands() { + let m = MockMotor::new(3); + m.set_velocity(1000.0); + assert_eq!(m.last_command(), Command::Velocity(1000.0)); + m.disable(); + assert_eq!(m.last_command(), Command::Disabled); + } + + #[test] + fn mock_motor_injects_state() { + let m = MockMotor::new(1); + assert!(!m.state().has_feedback()); + m.inject_state(MotorState { feedback_count: 5, ..MotorState::default() }); + assert!(m.state().has_feedback()); + } +} diff --git a/crates/motor-core/src/traits.rs b/crates/motor-core/src/traits.rs new file mode 100644 index 0000000..247ae06 --- /dev/null +++ b/crates/motor-core/src/traits.rs @@ -0,0 +1,47 @@ +//! 能力面 trait：小而正交，按硬件事实拆分。 +//! +//! 消费者只声明自己用到的能力（静态分发，无 `dyn`）： +//! - 底盘轮 → [`VelocityDrive`] + [`MotorFeedback`] +//! - 云台 → [`PositionDrive`] + [`EffortDrive`]（+ 上层 [`crate::HeadingSource`]） +//! - 拨弹 → [`VelocityDrive`] + [`PositionDrive`] +//! - 台架 → [`VelocityDrive`] + [`MotorFeedback`] + [`Disable`] + +use crate::MotorState; + +/// 读反馈。所有可观察设备的最小面。 +pub trait MotorFeedback: Send + Sync { + /// 控制器内的电机 ID（对 DJI 电调即电调 ID）。 + fn id(&self) -> u8; + + /// 最新反馈快照。 + fn state(&self) -> MotorState; +} + +/// 开环控制量（协议 raw：ESC=电流，GM6020=电压）。 +pub trait EffortDrive: Send + Sync { + /// 直接下发开环 raw（协议量程，物理方向）。 + fn set_effort_raw(&self, raw: i16); +} + +/// 速度目标（转子 rpm，物理方向）；由驱动层速度环跟踪。 +pub trait VelocityDrive: Send + Sync { + /// 设定目标转子转速。 + fn set_velocity(&self, rotor_rpm: f32); +} + +/// 位置目标（转子多圈，圈；物理方向）。 +pub trait PositionDrive: Send + Sync { + /// 设定目标转子多圈位置。 + fn set_position(&self, rotor_rev: f32); +} + +/// 停用（零输出）。 +pub trait Disable: Send + Sync { + /// 停用电机。 + fn disable(&self); +} + +/// 便利别名（blanket impl，零成本）。实现方**不必**实现它。 +pub trait Motor: MotorFeedback + VelocityDrive + EffortDrive + Disable {} + +impl Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {} diff --git a/crates/motor-core/src/velocity.rs b/crates/motor-core/src/velocity.rs new file mode 100644 index 0000000..521cc0b --- /dev/null +++ b/crates/motor-core/src/velocity.rs @@ -0,0 +1,87 @@ +//! 速度环控制器：PID + 速度前馈。 + +use crate::pid::Pid; + +/// 速度环封装：在 [`Pid`] 之上叠加比例前馈。 +/// +/// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量 +/// effort_raw（协议 raw：ESC=电流，GM6020=电压）。 +/// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量 +/// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。 +/// +/// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。 +#[derive(Debug, Clone)] +pub struct VelocityController { + pid: Pid, + feedforward: f32, +} + +impl VelocityController { + /// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。 + #[must_use] + pub fn new(pid: Pid, feedforward: f32) -> Self { + Self { pid, feedforward } + } + + /// 单步更新，返回 effort_raw（已含 PID 输出限幅）。 + pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 { + let pid_out = self.pid.update(target_rpm, measured_rpm, dt); + (pid_out + self.feedforward * target_rpm).clamp( + // 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。 + self.pid.output_min(), + self.pid.output_max(), + ) + } + + /// 清空 PID 内部状态（切换模式 / 重新使能时调用）。 + pub fn reset(&mut self) { + self.pid.reset(); + } + + /// 前馈系数（raw / rpm）。 + #[must_use] + pub fn feedforward(&self) -> f32 { + self.feedforward + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn pure_feedforward_tracks_target() { + let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0); + assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3); + // 前馈 -18000 被限幅到 -16384。 + assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3); + } + + #[test] + fn feedforward_plus_pid_respects_limit() { + let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0); + // 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。 + let out = vc.update(1000.0, 0.0, 0.001); + assert!((out - 12000.0).abs() < 1e-3, "out = {out}"); + } + + #[test] + fn zero_error_yields_pure_feedforward() { + // 误差为 0 时 PID 贡献为 0，输出只剩前馈。 + let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0); + let out = vc.update(1000.0, 1000.0, 0.001); + assert!((out - 9000.0).abs() < 1e-4, "out = {out}"); + } + + #[test] + fn reset_clears_pid_state() { + let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0); + for _ in 0..10 { + vc.update(100.0, 0.0, 0.001); + } + vc.reset(); + // 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。 + let out = vc.update(100.0, 0.0, 0.001); + assert!((out - 1010.0).abs() < 1e-3, "out = {out}"); + } +} diff --git a/deploy/chassisd.service b/deploy/chassisd.service new file mode 100644 index 0000000..6f149f6 --- /dev/null +++ b/deploy/chassisd.service @@ -0,0 +1,19 @@ +[Unit] +Description=Sentry chassis daemon (mecanum, 4x GM3508/C620) +After=network.target +# 若使用 USB 转 CAN，等待接口就绪： +# After=systemd-networkd.service +# RequiresMountsFor=/sys/class/net/can0 + +[Service] +Type=simple +ExecStart=/usr/local/bin/chassisd --interface can0 --config /etc/chassisd/chassis.toml +Restart=always +RestartSec=1 +# 实时性：提升调度优先级（按需调整） +# CPUSchedulingPolicy=fifo +# CPUSchedulingPriority=20 +# LimitRTPRIO=95 + +[Install] +WantedBy=multi-user.target diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..b63ccc4 --- /dev/null +++ b/docs/architecture.md @@ -0,0 +1,77 @@ +# 架构设计 + +## 分层 + +``` + ┌─────────────────────────────────────────────┐ + │ chassisd（守护进程） │ + │ 配置 / CLI / 1kHz 控制循环 / 安全逻辑 / 日志 │ + └──────┬───────────────────────────┬──────────┘ + │ mecanum（麦轮正逆解） │ + ▼ ▼ + ┌────────────┐ ┌──────────────────┐ + │ motor-core │◄─实现 trait──│ dji-esc │ + │ Motor trait│ │ C620/C610 协议 │ + │ MotorState │ │ EscBus 异步驱动 │ + │ Pid │ │ TurnTracker 多圈 │ + └────────────┘ └────────┬─────────┘ + │ CanLink（tokio mpsc 通道对） + ▼ + ┌──────────────────────────┐ + │ dji-esc-socketcan(Linux) │ + │ socketcan ↔ CanLink 桥接 │ + └──────────────────────────┘ +``` + +核心解耦点：`CanLink` 是一对 tokio 通道（发送 / 接收 `CanFrame`）。`dji-esc` 的协议状态机 +只面向通道工作，因此： + +- 测试中用内存通道 + 一阶电机模型（`dji_esc::sim`）构成闭环仿真，全平台可跑； +- `dji-esc-socketcan` 把真实 SocketCAN 接口桥接到通道，只在实车上出现； +- 未来更换总线介质（串口转 CAN、MCU 转发）只需新增一个桥接 crate。 + +## DJI 电调 CAN 协议要点（C620 手册） + +| 项目 | 值 | +| --- | --- | +| 控制帧 ID | `0x200`（电调 1–4）、`0x1FF`（电调 5–8） | +| 控制帧格式 | 8 字节 = 4 × i16 大端电流命令，按电调 ID 顺序 | +| C620 电流标度 | ±16384 ↔ ±20 A | +| C610 电流标度 | ±10000 ↔ ±10 A（M2006 组合，接硬件前需核对） | +| 反馈帧 ID | `0x200 + 电调ID`（`0x201`–`0x208`） | +| 反馈帧格式 | u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃，均大端 | +| 反馈频率 | 每电调 1kHz | + +C620 内部**只闭电流环**：转速 / 位置环由主控侧 PID 实现（`motor-core::Pid`， +挂在 `EscBus` 的 1kHz 发送节拍上）。GM3508 减速比 3591 / 187 ≈ 19.203。 + +## 单位与方向约定 + +- 机器人坐标系：ISO 约定——x 前、y 左、ω 逆时针为正（见 `mecanum`）。 +- `Motor` 接口呈现的是**物理量**（安装方向修正后）： + `EscConfig::direction = -1` 时，驱动层在写命令与读反馈时统一翻转符号， + 上层永远以"轮子正转 = 底盘前进方向"的正向工作。 +- `MotorState::rotor_speed_rpm` 为电机转子转速；轮端转速 = 转子转速 / 减速比。 +- 电流一律以协议原始值（raw）为主，安培换算通过 `EscModel` 常量提供。 + +## 安全设计 + +1. **上电门控**：底盘循环在收到任一电机首帧反馈前不下发速度命令。 +2. **反馈离线**：任一电机反馈超时（默认 100ms）→ 该电机命令强制 0 电流并告警。 +3. **命令超时**：上层 twist 命令超时（默认 200ms）→ 目标速度归零。 +4. 所有超时可经 `chassis.toml` 配置。 + +## 测试策略 + +| 层级 | 内容 | 平台 | +| --- | --- | --- | +| 单元测试 | 帧编解码字节级样例 / 饱和、多圈回绕、PID（阶跃、抗饱和、滤波）、运动学解析解 + 属性测试 | 全平台 | +| 闭环仿真 | 内存通道 + 一阶电机模型：4 电机速度环收敛、群帧组装、离线安全、守护进程全链路 | 全平台 | +| vcan 集成 | vcan0 双 socket 收发，真实内核 CAN 栈 | Linux（`#[ignore]`，CI 显式跑） | +| 交叉检查 | `aarch64-unknown-linux-gnu` 全 workspace check | CI | + +## 部署 + +`chassisd` 为单二进制守护进程：`--interface can0 --config /etc/chassisd/chassis.toml`。 +systemd 样例见 `deploy/chassisd.service`。板端接口就绪依赖： +USB 转 CAN（gs_usb / candleLight）或 SPI CAN HAT（MCP2515）驱动加载后的 `can0`。 diff --git a/justfile b/justfile new file mode 100644 index 0000000..2ac9c60 --- /dev/null +++ b/justfile @@ -0,0 +1,48 @@ +# 开发常用命令。未安装 just 时可直接使用 README 中的 cargo 命令。 + +default: + @just --list + +# 格式化全部代码 +fmt: + cargo fmt --all + +# 本地检查（与 CI 同标准） +lint: + cargo fmt --all -- --check + cargo clippy --workspace --all-targets -- -D warnings + +# 运行全部单测与集成测试（Linux 专属 vcan 测试自动跳过） +test: + cargo test --workspace + +# Linux：搭建 vcan0 并运行真实内核 CAN 栈集成测试 +vcan-test: + sudo modprobe vcan + sudo ip link add dev vcan0 type vcan || true + sudo ip link set up vcan0 + cargo test -p dji-esc-socketcan -- --ignored --test-threads 1 + +# 无硬件全链路演示（macOS / Linux 均可） +run-mock: + cargo run -p chassisd -- --mock + +# 交叉检查目标板架构 +check-aarch64: + cargo check --workspace --target aarch64-unknown-linux-gnu + +# 交叉编译 armv7 静态二进制（目标板：创龙 T153 / Buildroot） +build-armv7: + rustup target add armv7-unknown-linux-musleabihf + cargo build --release -p chassisd --target armv7-unknown-linux-musleabihf + +# 推送到板子并试运行（默认 mock 模式） +deploy board="root@169.254.63.4" mode="--mock": + just build-armv7 + ssh {{board}} "mkdir -p /tmp/chassisd" + scp -q target/armv7-unknown-linux-musleabihf/release/chassisd {{board}}:/tmp/chassisd/chassisd + ssh {{board}} "/tmp/chassisd/chassisd {{mode}}" + +# 板端：拉起 CAN 接口（1Mbps，bus-off 后 100ms 自动恢复） +can-up board="root@169.254.63.4" iface="can0": + ssh {{board}} "ip link set {{iface}} down; ip link set {{iface}} up type can bitrate 1000000 restart-ms 100 && ip -br link show {{iface}}" diff --git a/rust-toolchain.toml b/rust-toolchain.toml new file mode 100644 index 0000000..73cb934 --- /dev/null +++ b/rust-toolchain.toml @@ -0,0 +1,3 @@ +[toolchain] +channel = "stable" +components = ["rustfmt", "clippy"] diff --git a/rustfmt.toml b/rustfmt.toml new file mode 100644 index 0000000..8635cf9 --- /dev/null +++ b/rustfmt.toml @@ -0,0 +1,2 @@ +max_width = 100 +use_small_heuristics = "Max" diff --git a/ubuntu.md b/ubuntu.md new file mode 100644 index 0000000..ec02648 --- /dev/null +++ b/ubuntu.md @@ -0,0 +1,5 @@ +sed -i -e 's@//ports.ubuntu.com/\? @//ports.ubuntu.com/ubuntu-ports @g' \ + -e 's@//ports.ubuntu.com@//mirrors.ustc.edu.cn@g' \ + /etc/apt/sources.list + +passwd 114514 \ No newline at end of file