385 lines
16 KiB
Markdown
385 lines
16 KiB
Markdown
# 审计 + 电机抽象层改造 + 实车台架单轮测试
|
||
|
||
## 目标（用户已确认）
|
||
|
||
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
|