damiao_codec/
codec.rs

1//! [`DamiaoCodec`] — implements [`motor_codec::MotorCodec`] for Damiao motors.
2
3use motor_codec::{
4    BusCapabilities, CanFrame, CodecError, Command, CommandKind, Event, FrameFlags, Limits,
5    MotorCodec, MotorRef, MotorTypeId, ParamValue,
6};
7
8use crate::bitpack::{pack_mit_payload, unpack_state_payload};
9use crate::limits::limits_for;
10use crate::types::DamiaoMotorType;
11
12const VENDOR: &str = "damiao";
13
14/// Damiao motor codec.
15///
16/// Construct with [`DamiaoCodec::new`]. Implements
17/// [`MotorCodec`] for the vendor-agnostic surface and [`crate::DamiaoCodecExt`]
18/// for the Damiao-specific `0x7FF` parameter sub-protocol.
19#[derive(Default, Debug, Clone)]
20pub struct DamiaoCodec {
21    bound_caps: Option<BusCapabilities>,
22}
23
24impl DamiaoCodec {
25    /// Construct a new codec. `bind_to_bus` must be called before any encode.
26    pub fn new() -> Self {
27        Self { bound_caps: None }
28    }
29
30    fn motor_type(&self, id: MotorTypeId) -> Result<DamiaoMotorType, CodecError> {
31        match id {
32            MotorTypeId::Damiao(d) => {
33                DamiaoMotorType::from_discriminant(d).ok_or(CodecError::UnknownMotorType {
34                    vendor: VENDOR,
35                    type_id: d,
36                })
37            }
38            _ => Err(CodecError::UnknownMotorType {
39                vendor: VENDOR,
40                type_id: 0,
41            }),
42        }
43    }
44
45    fn encode_special(
46        &self,
47        motor: MotorRef<'_>,
48        trailing_byte: u8,
49    ) -> Result<CanFrame, CodecError> {
50        let payload = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, trailing_byte];
51        CanFrame::classical(motor.send_id, &payload).map_err(|_| CodecError::DecodeFailed {
52            reason: "classical frame construction failed",
53        })
54    }
55}
56
57impl MotorCodec for DamiaoCodec {
58    fn vendor_name(&self) -> &'static str {
59        VENDOR
60    }
61
62    fn supports(&self, motor_type: MotorTypeId) -> bool {
63        matches!(motor_type, MotorTypeId::Damiao(d) if DamiaoMotorType::from_discriminant(d).is_some())
64    }
65
66    fn limits(&self, motor_type: MotorTypeId) -> Result<Limits, CodecError> {
67        let t = self.motor_type(motor_type)?;
68        Ok(limits_for(t))
69    }
70
71    fn bind_to_bus(&mut self, caps: BusCapabilities) {
72        // The bound capabilities decide which frame formats this codec accepts
73        // on decode. Emission stays conservative — Damiao command/state frames
74        // are 8 bytes, valid on a classical or an FD bus — so a classical
75        // binding reproduces the v1 byte layout exactly.
76        self.bound_caps = Some(caps);
77    }
78
79    fn encode_enable(&self, motor: MotorRef<'_>) -> Result<CanFrame, CodecError> {
80        self.encode_special(motor, 0xFC)
81    }
82
83    fn encode_disable(&self, motor: MotorRef<'_>) -> Result<CanFrame, CodecError> {
84        self.encode_special(motor, 0xFD)
85    }
86
87    fn encode_set_zero(&self, motor: MotorRef<'_>) -> Result<CanFrame, CodecError> {
88        self.encode_special(motor, 0xFE)
89    }
90
91    fn encode_command(&self, motor: MotorRef<'_>, cmd: &Command) -> Result<CanFrame, CodecError> {
92        let t = self.motor_type(motor.motor_type)?;
93        let lim = limits_for(t);
94        match cmd {
95            Command::Mit { kp, kd, q, dq, tau } => {
96                check_range("q", *q, lim.p_max)?;
97                check_range("dq", *dq, lim.v_max)?;
98                check_range("tau", *tau, lim.t_max)?;
99                check_unsigned("kp", *kp, 500.0)?;
100                check_unsigned("kd", *kd, 5.0)?;
101                let payload =
102                    pack_mit_payload(*q, *dq, *kp, *kd, *tau, lim.p_max, lim.v_max, lim.t_max);
103                CanFrame::classical(motor.send_id, &payload).map_err(|_| CodecError::DecodeFailed {
104                    reason: "MIT frame construction failed",
105                })
106            }
107            Command::PosVel { q, dq } => {
108                let mut payload = [0u8; 8];
109                payload[0..4].copy_from_slice(&(*q as f32).to_le_bytes());
110                payload[4..8].copy_from_slice(&(*dq as f32).to_le_bytes());
111                CanFrame::classical(0x100 + motor.send_id, &payload).map_err(|_| {
112                    CodecError::DecodeFailed {
113                        reason: "PosVel frame construction failed",
114                    }
115                })
116            }
117            Command::Vel { dq } => {
118                let mut payload = [0u8; 8];
119                payload[0..4].copy_from_slice(&(*dq as f32).to_le_bytes());
120                CanFrame::classical(0x200 + motor.send_id, &payload).map_err(|_| {
121                    CodecError::DecodeFailed {
122                        reason: "Vel frame construction failed",
123                    }
124                })
125            }
126            Command::PosForce { q, dq, i_pu } => {
127                let mut payload = [0u8; 8];
128                payload[0..4].copy_from_slice(&(*q as f32).to_le_bytes());
129                let dq_u = (dq * 100.0) as u16;
130                let i_u = (i_pu * 10000.0) as u16;
131                payload[4..6].copy_from_slice(&dq_u.to_le_bytes());
132                payload[6..8].copy_from_slice(&i_u.to_le_bytes());
133                CanFrame::classical(0x300 + motor.send_id, &payload).map_err(|_| {
134                    CodecError::DecodeFailed {
135                        reason: "PosForce frame construction failed",
136                    }
137                })
138            }
139            _ => Err(CodecError::CommandNotSupported {
140                vendor: VENDOR,
141                mode: cmd.kind(),
142            }),
143        }
144    }
145
146    fn encode_refresh(&self, motor: MotorRef<'_>) -> Result<Option<CanFrame>, CodecError> {
147        // The `refresh_motor_status` query (0xCC on 0x7FF) requests a feedback
148        // frame without commanding motion. Disambiguate from the same-named
149        // `DamiaoCodecExt::encode_refresh`, which returns the raw frame.
150        Ok(Some(crate::DamiaoCodecExt::encode_refresh(self, motor)))
151    }
152
153    fn encode_set_mode(
154        &self,
155        motor: MotorRef<'_>,
156        mode: CommandKind,
157    ) -> Result<Option<CanFrame>, CodecError> {
158        // Damiao CTRL_MODE register (RID 10): MIT=1, PosVel=2, Vel=3, PosForce=4.
159        let value: u32 = match mode {
160            CommandKind::Mit => 1,
161            CommandKind::PosVel => 2,
162            CommandKind::Vel => 3,
163            CommandKind::PosForce => 4,
164            _ => {
165                return Err(CodecError::CommandNotSupported {
166                    vendor: VENDOR,
167                    mode,
168                })
169            }
170        };
171        Ok(Some(crate::DamiaoCodecExt::encode_write_param(
172            self,
173            motor,
174            crate::DamiaoRid::CTRL_MODE,
175            ParamValue::UInt(value),
176        )))
177    }
178
179    fn encode_control_mode_readback(
180        &self,
181        motor: MotorRef<'_>,
182    ) -> Result<Option<CanFrame>, CodecError> {
183        Ok(Some(crate::DamiaoCodecExt::encode_read_param(
184            self,
185            motor,
186            crate::DamiaoRid::CTRL_MODE,
187        )))
188    }
189
190    fn decode_control_mode_readback(
191        &self,
192        frame: &CanFrame,
193        motor: MotorRef<'_>,
194    ) -> Result<Option<u32>, CodecError> {
195        if frame.len != 8 {
196            return Ok(None);
197        }
198        let p = frame.payload();
199        let send_id = u16::from_le_bytes([p[0], p[1]]) as u32;
200        if send_id != motor.send_id || p[2] != 0x33 || p[3] != u8::from(crate::DamiaoRid::CTRL_MODE)
201        {
202            return Ok(None);
203        }
204        Ok(Some(u32::from_le_bytes([p[4], p[5], p[6], p[7]])))
205    }
206
207    fn decode(&self, frame: &CanFrame) -> Result<Option<Event>, CodecError> {
208        // Damiao state responses come on the recv_id assigned to the motor, with
209        // the response command in the top nibble of byte 0.
210        if frame.is_fd() && !self.bound_caps.is_some_and(|c| c.supports_fd) {
211            // Not bound to an FD bus: an FD frame isn't a Damiao state frame we
212            // expect here. (A classical binding therefore behaves exactly as v1.)
213            return Ok(None);
214        }
215        if frame.flags.contains(FrameFlags::REMOTE_REQUEST) {
216            return Ok(None);
217        }
218        // Parameter traffic is not a state frame. In particular, a 0x55 write
219        // acknowledgement must never update motor state.
220        if frame.id == 0x7FF || (frame.payload()[2] == 0x33 || frame.payload()[2] == 0x55) {
221            return Ok(None);
222        }
223        if frame.len != 8 {
224            // Damiao always uses 8 bytes; a different length is not a Damiao state frame.
225            return Ok(None);
226        }
227        let byte0 = frame.payload()[0];
228        let reported_id = byte0 & 0x0f;
229        // Limits: the codec doesn't have a per-recv_id motor-type registry in
230        // v1, so it uses DM4340 limits as the OpenArm walking-skeleton default.
231        // A future change will register per-motor limits at bind time so mixed-
232        // SKU buses decode each motor with its own limits.
233        let payload: [u8; 8] =
234            frame
235                .payload()
236                .try_into()
237                .map_err(|_| CodecError::DecodeFailed {
238                    reason: "expected 8-byte payload",
239                })?;
240        let dm = limits_for(DamiaoMotorType::DM4340);
241        let (_, _, q, dq, tau, t_mos, t_rotor) =
242            unpack_state_payload(&payload, dm.p_max, dm.v_max, dm.t_max);
243        if reported_id == 0 && frame.id == 0 {
244            return Ok(None);
245        }
246        Ok(Some(Event::State {
247            motor_id: frame.id,
248            q,
249            dq,
250            tau,
251            t_mos,
252            t_rotor,
253        }))
254    }
255}
256
257fn check_range(field: &'static str, x: f64, magnitude: f64) -> Result<(), CodecError> {
258    if x.abs() > magnitude {
259        Err(CodecError::OutOfRange { field })
260    } else {
261        Ok(())
262    }
263}
264
265fn check_unsigned(field: &'static str, x: f64, max: f64) -> Result<(), CodecError> {
266    if !(0.0..=max).contains(&x) {
267        Err(CodecError::OutOfRange { field })
268    } else {
269        Ok(())
270    }
271}
272
273#[cfg(test)]
274mod tests {
275    use super::*;
276    use alloc::boxed::Box;
277
278    fn ref_motor(name: &str, mt: DamiaoMotorType, send: u32, recv: u32) -> MotorRef<'_> {
279        MotorRef {
280            motor_type: mt.into(),
281            send_id: send,
282            recv_id: recv,
283            name,
284        }
285    }
286
287    #[test]
288    fn constructible_as_trait_object() {
289        let c: Box<dyn MotorCodec> = Box::new(DamiaoCodec::new());
290        assert_eq!(c.vendor_name(), "damiao");
291    }
292
293    #[test]
294    fn supports_all_known_skus() {
295        let c = DamiaoCodec::new();
296        for d in 0..=12u16 {
297            assert!(c.supports(MotorTypeId::Damiao(d)), "discriminant {d}");
298            assert!(c.limits(MotorTypeId::Damiao(d)).is_ok());
299        }
300    }
301
302    #[test]
303    fn rejects_unknown_and_other_vendors() {
304        let c = DamiaoCodec::new();
305        assert!(!c.supports(MotorTypeId::Damiao(0xFFFF)));
306        assert!(!c.supports(MotorTypeId::Robostride(0)));
307        assert!(matches!(
308            c.limits(MotorTypeId::Damiao(0xFFFF)),
309            Err(CodecError::UnknownMotorType {
310                vendor: "damiao",
311                type_id: 0xFFFF,
312            })
313        ));
314    }
315
316    #[test]
317    fn enable_disable_setzero_byte_patterns() {
318        let c = DamiaoCodec::new();
319        let m = ref_motor("g", DamiaoMotorType::DM4310, 0x05, 0x18);
320        let e = c.encode_enable(m).unwrap();
321        assert_eq!(e.id, 0x05);
322        assert_eq!(e.len, 8);
323        assert_eq!(
324            e.payload(),
325            &[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC]
326        );
327
328        let d = c.encode_disable(m).unwrap();
329        assert_eq!(d.payload()[7], 0xFD);
330        let z = c.encode_set_zero(m).unwrap();
331        assert_eq!(z.payload()[7], 0xFE);
332    }
333
334    #[test]
335    fn mit_frame_id_equals_send_id_and_not_fd() {
336        let c = DamiaoCodec::new();
337        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
338        let f = c
339            .encode_command(
340                m,
341                &Command::Mit {
342                    kp: 50.0,
343                    kd: 1.0,
344                    q: 0.0,
345                    dq: 0.0,
346                    tau: 0.0,
347                },
348            )
349            .unwrap();
350        assert_eq!(f.id, 0x01);
351        assert_eq!(f.len, 8);
352        assert!(!f.is_fd());
353    }
354
355    #[test]
356    fn mit_out_of_range_tau() {
357        let c = DamiaoCodec::new();
358        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
359        let r = c.encode_command(
360            m,
361            &Command::Mit {
362                kp: 0.0,
363                kd: 0.0,
364                q: 0.0,
365                dq: 0.0,
366                tau: 1000.0,
367            },
368        );
369        assert!(matches!(r, Err(CodecError::OutOfRange { field: "tau" })));
370    }
371
372    #[test]
373    fn posvel_uses_0x100_offset() {
374        let c = DamiaoCodec::new();
375        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
376        let f = c
377            .encode_command(m, &Command::PosVel { q: 1.0, dq: 2.0 })
378            .unwrap();
379        assert_eq!(f.id, 0x101);
380        assert_eq!(&f.payload()[0..4], &1.0f32.to_le_bytes());
381        assert_eq!(&f.payload()[4..8], &2.0f32.to_le_bytes());
382    }
383
384    #[test]
385    fn vel_uses_0x200_offset() {
386        let c = DamiaoCodec::new();
387        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
388        let f = c.encode_command(m, &Command::Vel { dq: 1.5 }).unwrap();
389        assert_eq!(f.id, 0x201);
390        assert_eq!(&f.payload()[0..4], &1.5f32.to_le_bytes());
391    }
392
393    #[test]
394    fn posforce_uses_0x300_offset_with_integer_scaling() {
395        let c = DamiaoCodec::new();
396        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
397        let f = c
398            .encode_command(
399                m,
400                &Command::PosForce {
401                    q: 1.0,
402                    dq: 2.0,
403                    i_pu: 0.5,
404                },
405            )
406            .unwrap();
407        assert_eq!(f.id, 0x301);
408        assert_eq!(&f.payload()[0..4], &1.0f32.to_le_bytes());
409        assert_eq!(&f.payload()[4..6], &200u16.to_le_bytes());
410        assert_eq!(&f.payload()[6..8], &5000u16.to_le_bytes());
411    }
412
413    #[test]
414    fn decode_foreign_frame_returns_none() {
415        let c = DamiaoCodec::new();
416        let f = CanFrame::classical(0x00, &[0x00; 8]).unwrap();
417        assert!(matches!(c.decode(&f), Ok(None)));
418    }
419
420    #[test]
421    fn decode_truncated_returns_none() {
422        let c = DamiaoCodec::new();
423        let f = CanFrame::classical(0x11, &[0x11, 0x00, 0x00]).unwrap();
424        assert!(matches!(c.decode(&f), Ok(None)));
425    }
426
427    #[test]
428    fn decode_state_frame_round_trips_within_lsb() {
429        let c = DamiaoCodec::new();
430        let lim = limits_for(DamiaoMotorType::DM4340);
431        let (q, dq, tau) = (1.0, 0.5, 5.0);
432        let q_u = crate::bitpack::float_to_uint(q, -lim.p_max, lim.p_max, 16);
433        let dq_u = crate::bitpack::float_to_uint(dq, -lim.v_max, lim.v_max, 12);
434        let tau_u = crate::bitpack::float_to_uint(tau, -lim.t_max, lim.t_max, 12);
435        let payload = [
436            // byte 0: high nibble = err (0), low nibble = cmd_id (1 = MIT response)
437            0x01,
438            ((q_u >> 8) & 0xff) as u8,
439            (q_u & 0xff) as u8,
440            ((dq_u >> 4) & 0xff) as u8,
441            ((((dq_u & 0xf) << 4) | ((tau_u >> 8) & 0xf)) & 0xff) as u8,
442            (tau_u & 0xff) as u8,
443            30,
444            35,
445        ];
446        let f = CanFrame::classical(0x11, &payload).unwrap();
447        match c.decode(&f).unwrap().unwrap() {
448            Event::State {
449                motor_id,
450                q: qo,
451                dq: dqo,
452                tau: tauo,
453                t_mos,
454                t_rotor,
455            } => {
456                assert_eq!(motor_id, 0x11);
457                assert!((qo - q).abs() < 0.001);
458                assert!((dqo - dq).abs() < 0.01);
459                assert!((tauo - tau).abs() < 0.05);
460                assert_eq!(t_mos, 30);
461                assert_eq!(t_rotor, 35);
462            }
463            _ => panic!("expected State"),
464        }
465    }
466
467    #[test]
468    fn decode_dm4310_enable_disable_replies_from_socketcan_capture() {
469        let c = DamiaoCodec::new();
470        for payload in [
471            [0x18, 0x81, 0x07, 0x7F, 0xE7, 0xFF, 0x1D, 0x1B],
472            [0x08, 0x81, 0x07, 0x80, 0x07, 0xFF, 0x1D, 0x1B],
473        ] {
474            match c
475                .decode(&CanFrame::classical(0x18, &payload).unwrap())
476                .unwrap()
477            {
478                Some(Event::State {
479                    motor_id,
480                    t_mos,
481                    t_rotor,
482                    ..
483                }) => {
484                    assert_eq!(motor_id, 0x18);
485                    assert_eq!(t_mos, 29);
486                    assert_eq!(t_rotor, 27);
487                }
488                other => panic!("expected State, got {other:?}"),
489            }
490        }
491    }
492
493    #[test]
494    fn classical_emission_regardless_of_bound_caps() {
495        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
496        for caps in [BusCapabilities::classical(), BusCapabilities::fd()] {
497            let mut c = DamiaoCodec::new();
498            c.bind_to_bus(caps);
499            let f = c
500                .encode_command(
501                    m,
502                    &Command::Mit {
503                        kp: 0.0,
504                        kd: 0.0,
505                        q: 0.0,
506                        dq: 0.0,
507                        tau: 0.0,
508                    },
509                )
510                .unwrap();
511            assert!(!f.is_fd(), "caps={caps:?} produced FD frame");
512            assert_eq!(f.len, 8);
513        }
514    }
515
516    /// Encoding is byte-for-byte identical whether bound classical, bound FD, or
517    /// unbound — the conservative-emission invariant the FD change must preserve.
518    #[test]
519    fn classical_binding_byte_identical_to_unbound() {
520        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
521        let cmd = Command::Mit {
522            kp: 50.0,
523            kd: 1.0,
524            q: 0.25,
525            dq: -0.5,
526            tau: 0.1,
527        };
528        let unbound = DamiaoCodec::new().encode_command(m, &cmd).unwrap();
529        let mut classical = DamiaoCodec::new();
530        classical.bind_to_bus(BusCapabilities::classical());
531        let mut fd = DamiaoCodec::new();
532        fd.bind_to_bus(BusCapabilities::fd());
533        let cf = classical.encode_command(m, &cmd).unwrap();
534        let ff = fd.encode_command(m, &cmd).unwrap();
535        assert!(!cf.is_fd() && !ff.is_fd() && !unbound.is_fd());
536        assert_eq!(cf.id, unbound.id);
537        assert_eq!(cf.payload(), unbound.payload());
538        assert_eq!(ff.payload(), unbound.payload());
539    }
540
541    /// A representative Damiao state frame, carried in FD format, decodes only
542    /// when the codec is bound to an FD bus; a classical/unbound codec treats it
543    /// as not-ours (preserving v1 behavior).
544    #[test]
545    fn fd_state_frame_decoded_only_when_bound_fd() {
546        let lim = limits_for(DamiaoMotorType::DM4340);
547        let (q, dq, tau) = (1.0, 0.5, 5.0);
548        let q_u = crate::bitpack::float_to_uint(q, -lim.p_max, lim.p_max, 16);
549        let dq_u = crate::bitpack::float_to_uint(dq, -lim.v_max, lim.v_max, 12);
550        let tau_u = crate::bitpack::float_to_uint(tau, -lim.t_max, lim.t_max, 12);
551        let payload = [
552            0x01,
553            ((q_u >> 8) & 0xff) as u8,
554            (q_u & 0xff) as u8,
555            ((dq_u >> 4) & 0xff) as u8,
556            ((((dq_u & 0xf) << 4) | ((tau_u >> 8) & 0xf)) & 0xff) as u8,
557            (tau_u & 0xff) as u8,
558            30,
559            35,
560        ];
561        // 8-byte payload is a valid FD DLC, so this is a genuine FD-format frame.
562        let fd_frame = CanFrame::fd(0x11, &payload).unwrap();
563        assert!(fd_frame.is_fd());
564
565        // Unbound and classical-bound: not ours → None (v1 behavior).
566        assert!(matches!(DamiaoCodec::new().decode(&fd_frame), Ok(None)));
567        let mut classical = DamiaoCodec::new();
568        classical.bind_to_bus(BusCapabilities::classical());
569        assert!(matches!(classical.decode(&fd_frame), Ok(None)));
570
571        // FD-bound: decoded as a state event.
572        let mut fd = DamiaoCodec::new();
573        fd.bind_to_bus(BusCapabilities::fd());
574        match fd.decode(&fd_frame).unwrap().unwrap() {
575            Event::State { motor_id, .. } => assert_eq!(motor_id, 0x11),
576            _ => panic!("expected State"),
577        }
578    }
579
580    #[test]
581    fn encode_refresh_matches_openarm_layout() {
582        let c = DamiaoCodec::new();
583        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
584        // The MotorCodec trait method wraps the Damiao 0xCC/0x7FF query.
585        let f = MotorCodec::encode_refresh(&c, m)
586            .unwrap()
587            .expect("damiao supports refresh");
588        assert_eq!(f.id, 0x7FF);
589        assert_eq!(f.len, 8);
590        assert_eq!(f.payload(), &[0x01, 0x00, 0xCC, 0, 0, 0, 0, 0]);
591        assert!(!f.is_fd());
592    }
593
594    #[test]
595    fn encode_set_mode_writes_ctrl_mode_register() {
596        let c = DamiaoCodec::new();
597        let m = ref_motor("j0", DamiaoMotorType::DM4340, 0x01, 0x11);
598        let f = MotorCodec::encode_set_mode(&c, m, CommandKind::Mit)
599            .unwrap()
600            .expect("damiao supports set_mode");
601        assert_eq!(f.id, 0x7FF);
602        assert_eq!(f.len, 8);
603        let p = f.payload();
604        assert_eq!(&p[0..2], &0x01u16.to_le_bytes()); // send id, LE
605        assert_eq!(p[2], 0x55); // write-param command
606        assert_eq!(p[3], 10); // CTRL_MODE register
607        assert_eq!(&p[4..8], &1u32.to_le_bytes()); // MIT == 1
608    }
609
610    #[test]
611    fn ctrl_mode_readback_is_contextual_and_not_state() {
612        let c = DamiaoCodec::new();
613        let m = ref_motor("g", DamiaoMotorType::DM4310, 0x05, 0x18);
614        let query = c.encode_control_mode_readback(m).unwrap().unwrap();
615        assert_eq!(query.id, 0x7FF);
616        assert_eq!(&query.payload()[0..4], &[0x05, 0x00, 0x33, 0x0A]);
617
618        let mut response = [0u8; 8];
619        response[0..2].copy_from_slice(&0x05u16.to_le_bytes());
620        response[2] = 0x33;
621        response[3] = 0x0A;
622        response[4..8].copy_from_slice(&4u32.to_le_bytes());
623        let response = CanFrame::classical(0x18, &response).unwrap();
624        assert_eq!(
625            c.decode_control_mode_readback(&response, m).unwrap(),
626            Some(4)
627        );
628        assert!(c.decode(&response).unwrap().is_none());
629
630        let wrong_send = CanFrame::classical(0x18, &[0x06, 0, 0x33, 0x0A, 4, 0, 0, 0]).unwrap();
631        assert_eq!(
632            c.decode_control_mode_readback(&wrong_send, m).unwrap(),
633            None
634        );
635        let write_ack = CanFrame::classical(0x18, &[0x05, 0, 0x55, 0x0A, 4, 0, 0, 0]).unwrap();
636        assert_eq!(c.decode_control_mode_readback(&write_ack, m).unwrap(), None);
637    }
638
639    extern crate alloc;
640}
damiao_codec/codec.rs

damiao_codec/
codec.rs
