feat: 初始化
This commit is contained in:
Horldsence
@@ -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"
|
||||
@@ -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
|
||||
@@ -0,0 +1 @@
|
||||
/target
|
||||
@@ -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<T>：输出轴单位适配（新增）
|
||||
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<T: MotorFeedback + VelocityDrive + EffortDrive + Disable> 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<Output = f32> + 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<T>`
|
||||
|
||||
```rust
|
||||
/// 输出轴单位 ↔ 转子单位。gear_ratio = 转子 : 输出轴。
|
||||
pub struct OutputShaft<T> { inner: T, gear_ratio: f32 }
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
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<W> {
|
||||
kinematics: Mecanum,
|
||||
wheels: [W; 4],
|
||||
limits: (f32, f32, f32),
|
||||
command_timeout: Duration,
|
||||
}
|
||||
impl<W: VelocityDrive + MotorFeedback> ChassisController<W> { ... }
|
||||
```
|
||||
|
||||
- **不 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<T>`；`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
|
||||
@@ -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<P: CanPort>`：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 专属测试自动跳过）
|
||||
@@ -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 默认排除规则（自定义规则请写在本行下方，不会被同步/恢复改动）-----
|
||||
# 自定义规则写在下方（本行提示可删除）
|
||||
Generated
+1226
File diff suppressed because it is too large
Load Diff
+65
@@ -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"
|
||||
@@ -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.
|
||||
@@ -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 协议要点、单位与方向约定、测试策略
|
||||
@@ -0,0 +1,4 @@
|
||||
# 异步代码库中禁止阻塞睡眠
|
||||
disallowed-methods = [
|
||||
{ path = "std::thread::sleep", reason = "异步代码请使用 tokio::time::sleep" },
|
||||
]
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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<CanFrame>,
|
||||
/// 来自总线。
|
||||
pub incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
}
|
||||
|
||||
impl CanLink {
|
||||
/// 由已有通道组装端点。
|
||||
#[must_use]
|
||||
pub const fn new(
|
||||
outgoing_tx: mpsc::Sender<CanFrame>,
|
||||
incoming_rx: mpsc::Receiver<CanFrame>,
|
||||
) -> 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);
|
||||
}
|
||||
}
|
||||
@@ -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"] }
|
||||
@@ -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<MotorConfig>,
|
||||
}
|
||||
|
||||
/// 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<FunctionBusConfig>,
|
||||
}
|
||||
|
||||
/// 功能总线配置（双 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<EscModelSerde> 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<Self, ConfigError> {
|
||||
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<Self, ConfigError> {
|
||||
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<ProtocolFamily> =
|
||||
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<EscConfig> {
|
||||
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<u8> = 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");
|
||||
}
|
||||
}
|
||||
@@ -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<W> {
|
||||
kinematics: Mecanum,
|
||||
/// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。
|
||||
wheels: [W; 4],
|
||||
/// (max_vx, max_vy, max_omega)。
|
||||
limits: (f32, f32, f32),
|
||||
command_timeout: Duration,
|
||||
}
|
||||
|
||||
impl<W> ChassisController<W>
|
||||
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<Twist>, period: Duration) -> JoinHandle<()> {
|
||||
tokio::spawn(self.run(commands, period))
|
||||
}
|
||||
|
||||
async fn run(self, mut commands: watch::Receiver<Twist>, 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<Instant> = 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<f32>, Vec<i32>) = 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<T: Clone, const N: usize>(
|
||||
motors: [T; N],
|
||||
gear_ratios: [f32; N],
|
||||
) -> [OutputShaft<T>; 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();
|
||||
}
|
||||
}
|
||||
@@ -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};
|
||||
@@ -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<PathBuf>,
|
||||
|
||||
/// 覆盖配置文件中的 CAN 接口名。
|
||||
#[arg(short, long)]
|
||||
interface: Option<String>,
|
||||
|
||||
/// 无硬件仿真模式：进程内电机模型，全链路可离线运行。
|
||||
#[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<Config, String> {
|
||||
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<dyn std::error::Error>> {
|
||||
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<u8> = 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::<Vec<_>>()
|
||||
.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<dji_esc::CanLink, Box<dyn std::error::Error>> {
|
||||
#[cfg(target_os = "linux")]
|
||||
{
|
||||
Ok(dji_esc_socketcan::open_link(interface)?)
|
||||
}
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
{
|
||||
let _ = interface;
|
||||
Err("实车模式仅支持 Linux（SocketCAN）；请使用 --mock 演示".into())
|
||||
}
|
||||
}
|
||||
@@ -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<Output = Option<Twist>> + Send;
|
||||
}
|
||||
|
||||
/// 恒零占位源：永不产出（watch 保持初值 `Twist::ZERO`，指令超时逻辑兜底）。
|
||||
pub struct ZeroSource;
|
||||
|
||||
impl TwistSource for ZeroSource {
|
||||
async fn next_twist(&mut self) -> Option<Twist> {
|
||||
std::future::pending().await
|
||||
}
|
||||
}
|
||||
|
||||
/// 通道指令源（测试 / 简单进程内注入用）。
|
||||
pub struct ChannelSource {
|
||||
rx: mpsc::Receiver<Twist>,
|
||||
}
|
||||
|
||||
impl ChannelSource {
|
||||
/// 由 mpsc 接收端构造。
|
||||
#[must_use]
|
||||
pub const fn new(rx: mpsc::Receiver<Twist>) -> Self {
|
||||
Self { rx }
|
||||
}
|
||||
|
||||
/// 创建一对（发送端, 源）。
|
||||
#[must_use]
|
||||
pub fn pair(bound: usize) -> (mpsc::Sender<Twist>, Self) {
|
||||
let (tx, rx) = mpsc::channel(bound);
|
||||
(tx, Self { rx })
|
||||
}
|
||||
}
|
||||
|
||||
impl TwistSource for ChannelSource {
|
||||
async fn next_twist(&mut self) -> Option<Twist> {
|
||||
self.rx.recv().await
|
||||
}
|
||||
}
|
||||
|
||||
/// 把任意指令源持续泵入 watch 通道（源关闭或接收端关闭时退出）。
|
||||
pub async fn pump_source(mut source: impl TwistSource, tx: watch::Sender<Twist>) {
|
||||
while let Some(twist) = source.next_twist().await {
|
||||
if tx.send(twist).is_err() {
|
||||
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 保持最后值
|
||||
}
|
||||
}
|
||||
@@ -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<Twist>) {
|
||||
let (bus_side, plant_side) = CanLink::pair(512);
|
||||
let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
|
||||
spawn_plant(plant_side, &esc_ids, config.bus_options().period);
|
||||
|
||||
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::<Vec<_>>()
|
||||
.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::<Vec<_>>()
|
||||
.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;
|
||||
}
|
||||
@@ -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 }
|
||||
@@ -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）");
|
||||
}
|
||||
@@ -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<CanLink> {
|
||||
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<CanSocket>, tx: mpsc::Sender<LinkFrame>) {
|
||||
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<CanSocket>, mut rx: mpsc::Receiver<LinkFrame>) {
|
||||
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<LinkFrame> {
|
||||
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<ScFrame> {
|
||||
let id = socketcan::StandardId::new(frame.id as u16)?;
|
||||
let df = CanDataFrame::new(id, &frame.data)?;
|
||||
Some(ScFrame::Data(df))
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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"] }
|
||||
@@ -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<T>(mutex: &Mutex<T>) -> 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<String>, esc_id: u8, direction: f32) -> Self {
|
||||
Self {
|
||||
name: name.into(),
|
||||
esc_id,
|
||||
model: EscModel::C620,
|
||||
// 3591 / 187 ≈ 19.203
|
||||
gear_ratio: 3591.0 / 187.0,
|
||||
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<String>, esc_id: u8, direction: f32) -> Self {
|
||||
Self {
|
||||
name: name.into(),
|
||||
esc_id,
|
||||
model: EscModel::C610,
|
||||
// M2006 P36 减速比（接入前核对）
|
||||
gear_ratio: 36.0,
|
||||
direction,
|
||||
current_limit: 8_000,
|
||||
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<MotorState>,
|
||||
cmd: Mutex<Command>,
|
||||
last_mode: Mutex<CommandMode>,
|
||||
vel_ctrl: Mutex<VelocityController>,
|
||||
pos_ctrl: Mutex<PositionController>,
|
||||
turn: Mutex<TurnTracker>,
|
||||
last_feedback: Mutex<Option<Instant>>,
|
||||
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<MotorShared>,
|
||||
}
|
||||
|
||||
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<Instant> {
|
||||
*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<EscConfig>,
|
||||
options: BusOptions,
|
||||
) -> Result<EscBusHandle, EscBusError> {
|
||||
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<Vec<EscMotor>>,
|
||||
shutdown_tx: watch::Sender<bool>,
|
||||
tasks: Vec<JoinHandle<()>>,
|
||||
}
|
||||
|
||||
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<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
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<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
options: BusOptions,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
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());
|
||||
}
|
||||
}
|
||||
@@ -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<u32> {
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
@@ -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};
|
||||
@@ -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<u8, PlantMotor> =
|
||||
esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect();
|
||||
let mut commands: HashMap<u8, i16> = 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<u8, i16>) {
|
||||
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],
|
||||
]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<u16>,
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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::<Vec<_>>()).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:?}"
|
||||
);
|
||||
}
|
||||
@@ -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 }
|
||||
@@ -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
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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]
|
||||
@@ -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,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
//! 输出轴单位适配器：减速比换算。
|
||||
//!
|
||||
//! 把「输出轴单位」的驱动接口适配到「转子单位」的底层电机。
|
||||
//! `gear_ratio = 转子 : 输出轴`（GM3508 ≈ 19.203）。
|
||||
|
||||
use crate::{Disable, EffortDrive, MotorFeedback, MotorState, PositionDrive, VelocityDrive};
|
||||
|
||||
/// 输出轴包装：对外接口使用输出轴单位，对内换算到转子单位。
|
||||
pub struct OutputShaft<T> {
|
||||
inner: T,
|
||||
gear_ratio: f32,
|
||||
}
|
||||
|
||||
impl<T> OutputShaft<T> {
|
||||
/// 包装底层电机。`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<T: MotorFeedback> MotorFeedback for OutputShaft<T> {
|
||||
fn id(&self) -> u8 {
|
||||
self.inner.id()
|
||||
}
|
||||
|
||||
fn state(&self) -> MotorState {
|
||||
// 只透传转子量；不伪造输出轴字段。
|
||||
self.inner.state()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: VelocityDrive> VelocityDrive for OutputShaft<T> {
|
||||
/// `output_rpm` 为**输出轴**转速。
|
||||
fn set_velocity(&self, output_rpm: f32) {
|
||||
self.inner.set_velocity(self.to_rotor(output_rpm));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: EffortDrive> EffortDrive for OutputShaft<T> {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
self.inner.set_effort_raw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: PositionDrive> PositionDrive for OutputShaft<T> {
|
||||
/// `output_rev` 为**输出轴**圈数。
|
||||
fn set_position(&self, output_rev: f32) {
|
||||
self.inner.set_position(self.to_rotor(output_rev));
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: Disable> Disable for OutputShaft<T> {
|
||||
fn disable(&self) {
|
||||
self.inner.disable();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::Command;
|
||||
|
||||
struct Stub {
|
||||
cmd: std::sync::Mutex<Command>,
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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<Output = f32> + Send;
|
||||
}
|
||||
|
||||
/// 云台目标角的参考系。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum HeadingFrame {
|
||||
/// 电机转子多圈（上电相对）。
|
||||
RotorMultiTurn,
|
||||
/// 场地绝对角（经 [`HeadingSource`] 对齐到转子基准后再下发）。
|
||||
AbsoluteField,
|
||||
}
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -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<f32>,
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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}");
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||||
}
|
||||
|
||||
/// 内存电机：记录命令、可注入状态。供上层单测，无需总线。
|
||||
///
|
||||
/// 句柄可克隆（内部 `Arc` 共享），便于一边注入/断言一边把所有权交给控制器。
|
||||
#[derive(Clone)]
|
||||
pub struct MockMotor {
|
||||
id: u8,
|
||||
published: Arc<Mutex<(MotorState, Command)>>,
|
||||
}
|
||||
|
||||
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<Output = f32> + 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());
|
||||
}
|
||||
}
|
||||
@@ -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<T> Motor for T where T: MotorFeedback + VelocityDrive + EffortDrive + Disable {}
|
||||
@@ -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}");
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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`。
|
||||
@@ -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}}"
|
||||
@@ -0,0 +1,3 @@
|
||||
[toolchain]
|
||||
channel = "stable"
|
||||
components = ["rustfmt", "clippy"]
|
||||
@@ -0,0 +1,2 @@
|
||||
max_width = 100
|
||||
use_small_heuristics = "Max"
|
||||
Reference in New Issue
Block a user