piper_driver/
command.rs

1//! 命令类型定义模块
2//!
3//! 提供命令优先级和类型区分机制，优化丢弃策略。
4
5use piper_can::PiperFrame;
6use smallvec::SmallVec;
7
8// 编译期断言：确保 PiperFrame 永远实现 Copy，这对 SmallVec 性能至关重要
9// 如果未来有人给 PiperFrame 添加非 Copy 字段（如 String），这里会编译失败
10#[cfg(test)]
11const _: () = {
12    fn assert_copy<T: Copy>() {}
13    fn check() {
14        assert_copy::<piper_can::PiperFrame>();
15    }
16    // 调用 check 以触发编译期检查
17    let _ = check;
18};
19
20/// 帧缓冲区类型
21///
22/// 使用 SmallVec 在栈上预留 6 个位置，足以覆盖：
23/// - MIT 控制：6 帧（0x15A, 0x15B, 0x15C, 0x15D, 0x15E, 0x15F）- **高频控制协议**
24/// - 位置控制：3 帧（0x155, 0x156, 0x157）
25/// - 末端位姿控制：3 帧（0x152, 0x153, 0x154）
26/// - 单个帧：1 帧（向后兼容）
27///
28/// **为什么是 6？**
29/// - MIT 控制是高频控制协议（通常 500Hz-1kHz），需要同时控制所有 6 个关节
30/// - 每个关节需要 1 帧（CAN ID: 0x15A + joint_index）
31/// - 总共需要 6 帧，必须一次性打包发送以避免覆盖问题
32/// - 使用栈缓冲区（6 帧）可以避免堆分配，确保实时性能
33///
34/// 占用空间约：24 bytes * 6 + overhead ≈ 150 bytes，对于 Mutex 内容来说仍然轻量
35///
36/// **性能要求**：`PiperFrame` 必须实现 `Copy` Trait，这样 `SmallVec` 在收集和迭代时
37/// 会编译为高效的内存拷贝指令（`memcpy`），避免调用 `Clone::clone`。
38///
39/// **确认**：`PiperFrame` 已实现 `Copy` Trait（见 `src/can/mod.rs:35`），满足性能要求。
40pub type FrameBuffer = SmallVec<[PiperFrame; 6]>;
41
42/// 实时命令类型（统一使用 FrameBuffer）
43///
44/// **设计决策**：不再区分 Single 和 Package，统一使用 FrameBuffer。
45/// - Single 只是 len=1 的 FrameBuffer
46/// - 简化 TX 线程逻辑（不需要 match 分支）
47/// - 消除 CPU 分支预测压力
48#[derive(Debug, Clone)]
49pub struct RealtimeCommand {
50    frames: FrameBuffer,
51}
52
53impl RealtimeCommand {
54    /// 创建单个帧命令（向后兼容）
55    ///
56    /// **性能优化**：添加 `#[inline]` 属性，因为此方法处于热路径（Hot Path）上。
57    #[inline]
58    pub fn single(frame: PiperFrame) -> Self {
59        let mut buffer = FrameBuffer::new();
60        buffer.push(frame); // 不会分配堆内存（len=1 < 6）
61        RealtimeCommand { frames: buffer }
62    }
63
64    /// 创建帧包命令
65    ///
66    /// **性能优化**：添加 `#[inline]` 属性，因为此方法处于热路径（Hot Path）上。
67    ///
68    /// **注意**：如果用户传入 `Vec<PiperFrame>`，`into_iter()` 会消耗这个 `Vec`。
69    /// 如果 `Vec` 长度 > 6，`SmallVec` 可能会尝试重用 `Vec` 的堆内存或重新分配。
70    /// 虽然这是安全的，但为了最佳性能，建议用户传入数组（栈分配）。
71    #[inline]
72    pub fn package(frames: impl IntoIterator<Item = PiperFrame>) -> Self {
73        let buffer: FrameBuffer = frames.into_iter().collect();
74        RealtimeCommand { frames: buffer }
75    }
76
77    /// 获取帧数量
78    #[inline]
79    pub fn len(&self) -> usize {
80        self.frames.len()
81    }
82
83    /// 检查是否为空
84    #[inline]
85    pub fn is_empty(&self) -> bool {
86        self.frames.is_empty()
87    }
88
89    /// 获取帧迭代器（用于 TX 线程发送）
90    #[inline]
91    pub fn iter(&self) -> impl Iterator<Item = &PiperFrame> {
92        self.frames.iter()
93    }
94
95    /// 消费并获取帧（用于 TX 线程发送）
96    #[inline]
97    pub fn into_frames(self) -> FrameBuffer {
98        self.frames
99    }
100}
101
102/// 命令优先级
103///
104/// 用于区分不同类型的命令，优化发送策略。
105#[derive(Debug, Clone, Copy, PartialEq, Eq)]
106pub enum CommandPriority {
107    /// 实时控制命令（可丢弃）
108    ///
109    /// 用于高频控制命令（500Hz-1kHz），如关节位置控制。
110    /// 如果队列满，新命令会覆盖旧命令（Overwrite 策略）。
111    /// 这确保了最新的控制命令总是被发送，即使意味着丢弃旧命令。
112    RealtimeControl,
113
114    /// 可靠命令（不可丢弃）
115    ///
116    /// 用于配置帧、状态机切换帧等关键命令。
117    /// 使用 FIFO 队列，按顺序发送，不会覆盖。
118    /// 如果队列满，会阻塞或返回错误（取决于 API）。
119    ReliableCommand,
120}
121
122/// 带优先级的命令
123///
124/// 封装 CAN 帧和优先级信息，用于类型安全的命令发送。
125#[derive(Debug, Clone, Copy)]
126pub struct PiperCommand {
127    /// CAN 帧
128    pub frame: PiperFrame,
129    /// 命令优先级
130    pub priority: CommandPriority,
131}
132
133impl PiperCommand {
134    /// 创建实时控制命令
135    pub fn realtime(frame: PiperFrame) -> Self {
136        Self {
137            frame,
138            priority: CommandPriority::RealtimeControl,
139        }
140    }
141
142    /// 创建可靠命令
143    pub fn reliable(frame: PiperFrame) -> Self {
144        Self {
145            frame,
146            priority: CommandPriority::ReliableCommand,
147        }
148    }
149
150    /// 获取命令帧
151    pub fn frame(&self) -> PiperFrame {
152        self.frame
153    }
154
155    /// 获取命令优先级
156    pub fn priority(&self) -> CommandPriority {
157        self.priority
158    }
159}
160
161impl From<PiperFrame> for PiperCommand {
162    /// 默认转换为可靠命令（向后兼容）
163    fn from(frame: PiperFrame) -> Self {
164        Self::reliable(frame)
165    }
166}
167
168impl From<PiperCommand> for PiperFrame {
169    fn from(cmd: PiperCommand) -> Self {
170        cmd.frame
171    }
172}
173
174#[cfg(test)]
175mod tests {
176    use super::*;
177
178    #[test]
179    fn test_command_priority() {
180        let frame = PiperFrame::new_standard(0x123, &[1, 2, 3]);
181
182        let realtime_cmd = PiperCommand::realtime(frame);
183        assert_eq!(realtime_cmd.priority(), CommandPriority::RealtimeControl);
184
185        let reliable_cmd = PiperCommand::reliable(frame);
186        assert_eq!(reliable_cmd.priority(), CommandPriority::ReliableCommand);
187    }
188
189    #[test]
190    fn test_command_from_frame() {
191        let frame = PiperFrame::new_standard(0x123, &[1, 2, 3]);
192        let cmd: PiperCommand = frame.into();
193
194        // 默认转换为可靠命令
195        assert_eq!(cmd.priority(), CommandPriority::ReliableCommand);
196        assert_eq!(cmd.frame().id, 0x123);
197    }
198
199    #[test]
200    fn test_command_to_frame() {
201        let frame = PiperFrame::new_standard(0x123, &[1, 2, 3]);
202        let cmd = PiperCommand::realtime(frame);
203
204        let converted_frame: PiperFrame = cmd.into();
205        assert_eq!(converted_frame.id, 0x123);
206    }
207}
208
209#[cfg(test)]
210mod realtime_command_tests {
211    use super::*;
212
213    #[test]
214    fn test_realtime_command_single() {
215        let frame = PiperFrame::new_standard(0x123, &[0x01, 0x02]);
216        let cmd = RealtimeCommand::single(frame);
217        assert_eq!(cmd.len(), 1);
218        assert!(!cmd.is_empty());
219    }
220
221    #[test]
222    fn test_realtime_command_package() {
223        let frames = [
224            PiperFrame::new_standard(0x155, &[0x01]),
225            PiperFrame::new_standard(0x156, &[0x02]),
226            PiperFrame::new_standard(0x157, &[0x03]),
227        ];
228        let cmd = RealtimeCommand::package(frames);
229        assert_eq!(cmd.len(), 3);
230        assert!(!cmd.is_empty());
231    }
232
233    #[test]
234    fn test_realtime_command_empty() {
235        let frames: [PiperFrame; 0] = [];
236        let cmd = RealtimeCommand::package(frames);
237        assert_eq!(cmd.len(), 0);
238        assert!(cmd.is_empty());
239    }
240
241    #[test]
242    fn test_realtime_command_iter() {
243        let frames = [
244            PiperFrame::new_standard(0x155, &[0x01]),
245            PiperFrame::new_standard(0x156, &[0x02]),
246        ];
247        let cmd = RealtimeCommand::package(frames);
248        let collected: Vec<_> = cmd.iter().collect();
249        assert_eq!(collected.len(), 2);
250    }
251
252    #[test]
253    fn test_realtime_command_into_frames() {
254        let frames = [
255            PiperFrame::new_standard(0x155, &[0x01]),
256            PiperFrame::new_standard(0x156, &[0x02]),
257        ];
258        let cmd = RealtimeCommand::package(frames);
259        let buffer = cmd.into_frames();
260        assert_eq!(buffer.len(), 2);
261    }
262}
piper_driver/command.rs

piper_driver/
command.rs
