hex_motor/canopen/
nmt.rs

1//! NMT 命令帧构造 + 心跳帧解码（含 NMT state 字节）。
2
3use can_transport::{CanFrame, CanId, CanIoError, FrameKind};
4
5/// 电机当前 NMT 状态（出现在入向心跳帧的 data[0]）。
6#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
7#[repr(u8)]
8pub enum NmtState {
9    /// 0x00：刚上电的 Boot-Up 帧。
10    BootUp = 0x00,
11    /// 0x04：Stopped。
12    Stopped = 0x04,
13    /// 0x05：Operational，正常工作态。
14    Operational = 0x05,
15    /// 0x7F：Pre-Operational。
16    PreOperational = 0x7F,
17}
18
19impl NmtState {
20    pub fn try_from_byte(b: u8) -> Option<Self> {
21        Some(match b {
22            0x00 => Self::BootUp,
23            0x04 => Self::Stopped,
24            0x05 => Self::Operational,
25            0x7F => Self::PreOperational,
26            _ => return None,
27        })
28    }
29}
30
31/// NMT 命令字节 (主站 → 节点)。
32#[derive(Debug, Clone, Copy, PartialEq, Eq)]
33#[repr(u8)]
34pub enum NmtCommand {
35    /// 0x01：进入 Operational。
36    StartRemoteNode = 0x01,
37    /// 0x02：进入 Stopped。
38    StopRemoteNode = 0x02,
39    /// 0x80：进入 Pre-Operational。
40    EnterPreOperational = 0x80,
41    /// 0x81：复位节点。
42    ResetNode = 0x81,
43    /// 0x82：复位通讯。
44    ResetCommunication = 0x82,
45}
46
47/// 构造一帧 NMT 命令帧。
48///
49/// `target_node_id == 0` 表示广播给所有节点。
50pub fn build_nmt_command(cmd: NmtCommand, target_node_id: u8) -> Result<CanFrame, CanIoError> {
51    let data = [cmd as u8, target_node_id];
52    CanFrame::new_data(CanId::Standard(0x000), &data)
53}
54
55/// 尝试解析心跳帧，返回 `(node_id, NMT 状态)`。
56///
57/// 不是心跳帧或长度不对，返回 `None`。
58pub fn parse_heartbeat(frame: &CanFrame) -> Option<(u8, NmtState)> {
59    if !matches!(frame.kind(), FrameKind::Data) {
60        return None;
61    }
62    let CanId::Standard(cob_id) = frame.id() else {
63        return None;
64    };
65    // function code 0x700, node id 1..=127
66    if cob_id & 0x780 != 0x700 {
67        return None;
68    }
69    let nid = (cob_id & 0x7F) as u8;
70    if nid == 0 {
71        return None;
72    }
73    let data = frame.data();
74    if data.len() != 1 {
75        return None;
76    }
77    NmtState::try_from_byte(data[0]).map(|s| (nid, s))
78}
79
80#[cfg(test)]
81mod tests {
82    use super::*;
83
84    #[test]
85    fn nmt_cmd_start_node() {
86        let f = build_nmt_command(NmtCommand::StartRemoteNode, 0x10).unwrap();
87        assert_eq!(f.id(), CanId::Standard(0x000));
88        assert_eq!(f.data(), &[0x01, 0x10]);
89    }
90
91    #[test]
92    fn nmt_cmd_broadcast_preop() {
93        let f = build_nmt_command(NmtCommand::EnterPreOperational, 0).unwrap();
94        assert_eq!(f.data(), &[0x80, 0x00]);
95    }
96
97    #[test]
98    fn parse_hb_operational() {
99        let f = CanFrame::new_data(CanId::Standard(0x710), &[0x05]).unwrap();
100        assert_eq!(parse_heartbeat(&f), Some((0x10, NmtState::Operational)));
101    }
102
103    #[test]
104    fn parse_hb_bootup() {
105        let f = CanFrame::new_data(CanId::Standard(0x711), &[0x00]).unwrap();
106        assert_eq!(parse_heartbeat(&f), Some((0x11, NmtState::BootUp)));
107    }
108
109    #[test]
110    fn parse_hb_preop() {
111        let f = CanFrame::new_data(CanId::Standard(0x77F), &[0x7F]).unwrap();
112        assert_eq!(parse_heartbeat(&f), Some((0x7F, NmtState::PreOperational)));
113    }
114
115    #[test]
116    fn parse_hb_wrong_size() {
117        let f = CanFrame::new_data(CanId::Standard(0x710), &[0x05, 0x00]).unwrap();
118        assert_eq!(parse_heartbeat(&f), None);
119    }
120
121    #[test]
122    fn parse_hb_wrong_cob() {
123        let f = CanFrame::new_data(CanId::Standard(0x180), &[0x05]).unwrap();
124        assert_eq!(parse_heartbeat(&f), None);
125    }
126
127    #[test]
128    fn parse_hb_unknown_state_byte() {
129        let f = CanFrame::new_data(CanId::Standard(0x710), &[0x42]).unwrap();
130        assert_eq!(parse_heartbeat(&f), None);
131    }
132
133    #[test]
134    fn parse_hb_node_zero() {
135        let f = CanFrame::new_data(CanId::Standard(0x700), &[0x05]).unwrap();
136        assert_eq!(parse_heartbeat(&f), None);
137    }
138}
hex_motor/canopen/nmt.rs

hex_motor/canopen/
nmt.rs
