ELCT/linux_sentry_chassis

Fork 0

Files

T

Horldsence 7d87987e41 feat: 初始化

2026-09-22 20:05:16 +08:00

16 KiB

Raw Blame History

审计 + 电机抽象层改造 + 实车台架单轮测试

目标（用户已确认）

审计现有抽象层级与抽象功能是否正确。
改造抽象层（本次就做），为后续派生铺路：
- 云台 = GM6020（位置环，绝对/多圈角度）
- 拨弹轮 = M2006 + C610（速度环 + 定量拨弹位置环）
小车轮子测试 = 实车台架单轮验证（现有 4× GM3508 轮子）。
硬约束：合适的 Rust 抽象层与 crate，明确单一职责。
架构决策（本轮确认）：
- 双 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）

// 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（用户要求）

// 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`

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>`

/// 输出轴单位 ↔ 转子单位。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（组合根）

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`（分阶段自动跑）

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`）

架空车轮，急停可及；ip link set can0 up type can bitrate 1000000 restart-ms 100。
首次 --current-limit 3000~5000。
一次只驱一台；顺序 FL→FR→RL→RR。
跑完自动 stage；方向错改配置 direction。
记录 direction 终值、实测 gear_ratio、PID 备注。
填表归档后才考虑落地四轮联动（不在本次）。

通过标准：反馈无连续丢帧；方向一致；无持续振荡、稳态 <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

六、验证

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

cargo test --workspace + just lint 绿。
chassisd --mock 行为不变。
rg 'wheel_speed_rpm|CurrentRaw|torque_current_raw' 为空。
rg 'dji_esc' crates/chassisd/src/controller.rs 为空。
motor-core 无运行时依赖。
wheel-bench + 台架文档就位；实车四轮台架表归档。
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

16 KiB Raw Blame History Unescape Escape