damiao_codec/
ext.rs

1//! [`DamiaoCodecExt`] — the Damiao-specific parameter sub-protocol on CAN ID
2//! `0x7FF`.
3//!
4//! Reached from the rest of the stack via downcast on `&dyn MotorCodec`
5//! (see `can-motor-control` `Arm::codec_ext::<DamiaoCodec>()`).
6
7use motor_codec::{CanFrame, MotorRef, ParamValue};
8
9use crate::codec::DamiaoCodec;
10use crate::types::DamiaoRid;
11
12const PARAM_ID: u32 = 0x7FF;
13const CMD_READ: u8 = 0x33;
14const CMD_WRITE: u8 = 0x55;
15const CMD_SAVE: u8 = 0xAA;
16const CMD_REFRESH: u8 = 0xCC;
17
18/// Damiao-only extension: read/write per-motor parameters on CAN ID `0x7FF`.
19///
20/// `DamiaoCodec` is the only implementor. The trait stands alone so callers
21/// can reach it via `&dyn` downcast without bringing in `MotorCodec`.
22pub trait DamiaoCodecExt {
23    /// Encode a "read parameter" frame (command byte `0x33`).
24    fn encode_read_param(&self, motor: MotorRef<'_>, rid: DamiaoRid) -> CanFrame;
25
26    /// Encode a "write parameter" frame (command byte `0x55`).
27    fn encode_write_param(&self, motor: MotorRef<'_>, rid: DamiaoRid, val: ParamValue) -> CanFrame;
28
29    /// Encode a "save settings to flash" frame (command byte `0xAA`).
30    fn encode_save_to_flash(&self, motor: MotorRef<'_>) -> CanFrame;
31
32    /// Encode a "request state feedback" frame (Damiao `refresh_motor_status`,
33    /// command byte `0xCC` on `0x7FF`). The motor replies with a feedback frame;
34    /// this commands no motion. (Byte-for-byte the openarm
35    /// `create_refresh_command`.)
36    fn encode_refresh(&self, motor: MotorRef<'_>) -> CanFrame;
37}
38
39fn header(motor: MotorRef<'_>, cmd: u8, rid: DamiaoRid) -> [u8; 4] {
40    let id = motor.send_id as u16;
41    [
42        (id & 0xff) as u8,
43        ((id >> 8) & 0xff) as u8,
44        cmd,
45        u8::from(rid),
46    ]
47}
48
49impl DamiaoCodecExt for DamiaoCodec {
50    fn encode_read_param(&self, motor: MotorRef<'_>, rid: DamiaoRid) -> CanFrame {
51        let mut payload = [0u8; 8];
52        payload[0..4].copy_from_slice(&header(motor, CMD_READ, rid));
53        // bytes 4..8: zero (no value on read).
54        CanFrame::classical(PARAM_ID, &payload).expect("8-byte classical frame")
55    }
56
57    fn encode_write_param(&self, motor: MotorRef<'_>, rid: DamiaoRid, val: ParamValue) -> CanFrame {
58        let mut payload = [0u8; 8];
59        payload[0..4].copy_from_slice(&header(motor, CMD_WRITE, rid));
60        match val {
61            ParamValue::Float(f) => {
62                payload[4..8].copy_from_slice(&(f as f32).to_le_bytes());
63            }
64            ParamValue::UInt(u) => {
65                payload[4..8].copy_from_slice(&u.to_le_bytes());
66            }
67            _ => {
68                // ParamValue is #[non_exhaustive]; v2 variants encode to zero
69                // until this codec learns about them.
70            }
71        }
72        CanFrame::classical(PARAM_ID, &payload).expect("8-byte classical frame")
73    }
74
75    fn encode_save_to_flash(&self, motor: MotorRef<'_>) -> CanFrame {
76        // SAVE doesn't carry a register; layout is [id_lo, id_hi, 0xAA, 0, 0, 0, 0, 0]
77        let id = motor.send_id as u16;
78        let payload = [
79            (id & 0xff) as u8,
80            ((id >> 8) & 0xff) as u8,
81            CMD_SAVE,
82            0,
83            0,
84            0,
85            0,
86            0,
87        ];
88        CanFrame::classical(PARAM_ID, &payload).expect("8-byte classical frame")
89    }
90
91    fn encode_refresh(&self, motor: MotorRef<'_>) -> CanFrame {
92        let id = motor.send_id as u16;
93        let payload = [
94            (id & 0xff) as u8,
95            ((id >> 8) & 0xff) as u8,
96            CMD_REFRESH,
97            0,
98            0,
99            0,
100            0,
101            0,
102        ];
103        CanFrame::classical(PARAM_ID, &payload).expect("8-byte classical frame")
104    }
105}
106
107#[cfg(test)]
108mod tests {
109    use super::*;
110    use crate::types::DamiaoMotorType;
111    use motor_codec::MotorTypeId;
112
113    fn m(send: u32) -> MotorRef<'static> {
114        MotorRef {
115            motor_type: MotorTypeId::Damiao(DamiaoMotorType::DM4340 as u16),
116            send_id: send,
117            recv_id: 0x11,
118            name: "j",
119        }
120    }
121
122    #[test]
123    fn write_param_layout() {
124        let c = DamiaoCodec::new();
125        let f = c.encode_write_param(m(0x01), DamiaoRid::MST_ID, ParamValue::UInt(0x11));
126        assert_eq!(f.id, PARAM_ID);
127        assert_eq!(f.len, 8);
128        let p = f.payload();
129        assert_eq!(&p[0..2], &0x01u16.to_le_bytes());
130        assert_eq!(p[2], CMD_WRITE);
131        assert_eq!(p[3], u8::from(DamiaoRid::MST_ID));
132        assert_eq!(&p[4..8], &0x11u32.to_le_bytes());
133    }
134
135    #[test]
136    fn read_param_uses_0x33() {
137        let c = DamiaoCodec::new();
138        let f = c.encode_read_param(m(0x01), DamiaoRid::CTRL_MODE);
139        assert_eq!(f.payload()[2], CMD_READ);
140    }
141
142    #[test]
143    fn save_uses_0xaa() {
144        let c = DamiaoCodec::new();
145        let f = c.encode_save_to_flash(m(0x01));
146        assert_eq!(f.payload()[2], CMD_SAVE);
147    }
148
149    #[test]
150    fn refresh_uses_0xcc() {
151        let c = DamiaoCodec::new();
152        let f = c.encode_refresh(m(0x01));
153        assert_eq!(f.payload()[2], CMD_REFRESH);
154    }
155}
damiao_codec/ext.rs

damiao_codec/
ext.rs
