moteus_protocol/
frame.rs

1// Copyright 2026 mjbots Robotic Systems, LLC.  info@mjbots.com
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! CAN-FD frame types and moteus arbitration ID helpers.
16
17/// Toggle state for CAN-FD frame options.
18#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
19pub enum Toggle {
20    /// Use the default behavior
21    #[default]
22    Default,
23    /// Force the option off
24    ForceOff,
25    /// Force the option on
26    ForceOn,
27}
28
29impl core::fmt::Display for Toggle {
30    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
31        match self {
32            Toggle::Default => write!(f, "default"),
33            Toggle::ForceOff => write!(f, "off"),
34            Toggle::ForceOn => write!(f, "on"),
35        }
36    }
37}
38
39/// A CAN-FD frame.
40///
41/// This structure represents a CAN-FD frame with only standard CAN fields:
42/// arbitration ID, payload data, and CAN-FD options. Moteus-specific routing
43/// (destination, source, can_prefix) is handled by the higher-level `Command`
44/// type in the `moteus` crate.
45#[non_exhaustive]
46#[derive(Clone, PartialEq)]
47pub struct CanFdFrame {
48    /// Channel index identifying which transport device this frame was
49    /// received from or should be sent to.
50    pub channel: Option<usize>,
51    /// CAN arbitration ID
52    pub arbitration_id: u32,
53    /// Frame payload data (up to 64 bytes for CAN-FD)
54    pub data: [u8; 64],
55    /// Actual size of data in the frame
56    pub size: u8,
57
58    /// Bit rate switch (BRS) toggle
59    pub brs: Toggle,
60    /// FD frame format toggle
61    pub fdcan_frame: Toggle,
62}
63
64impl Default for CanFdFrame {
65    fn default() -> Self {
66        Self::new()
67    }
68}
69
70impl CanFdFrame {
71    /// Creates a new empty CAN-FD frame.
72    pub const fn new() -> Self {
73        CanFdFrame {
74            channel: None,
75            arbitration_id: 0,
76            data: [0u8; 64],
77            size: 0,
78            brs: Toggle::Default,
79            fdcan_frame: Toggle::Default,
80        }
81    }
82
83    /// Returns the payload data as a slice.
84    pub fn payload(&self) -> &[u8] {
85        &self.data[..self.size as usize]
86    }
87
88    /// Returns the payload data as a mutable slice.
89    pub fn payload_mut(&mut self) -> &mut [u8] {
90        &mut self.data[..self.size as usize]
91    }
92
93    /// Clears the frame data, keeping metadata.
94    pub fn clear_data(&mut self) {
95        self.data = [0u8; 64];
96        self.size = 0;
97    }
98
99    /// Returns the remaining capacity in bytes.
100    pub fn remaining_capacity(&self) -> usize {
101        64 - self.size as usize
102    }
103
104    /// Returns true if the frame is empty (no data).
105    pub fn is_empty(&self) -> bool {
106        self.size == 0
107    }
108
109    /// Returns true if BRS (bit rate switching) should be enabled.
110    pub fn brs_enabled(&self) -> bool {
111        matches!(self.brs, Toggle::ForceOn | Toggle::Default)
112    }
113
114    /// Returns true if FD CAN frame format should be used.
115    pub fn fdcan_enabled(&self) -> bool {
116        matches!(self.fdcan_frame, Toggle::ForceOn | Toggle::Default)
117    }
118
119    /// Sets BRS toggle.
120    pub fn set_brs(&mut self, enabled: bool) {
121        self.brs = if enabled {
122            Toggle::ForceOn
123        } else {
124            Toggle::ForceOff
125        };
126    }
127
128    /// Sets FD CAN frame format toggle.
129    pub fn set_fdcan(&mut self, enabled: bool) {
130        self.fdcan_frame = if enabled {
131            Toggle::ForceOn
132        } else {
133            Toggle::ForceOff
134        };
135    }
136
137    /// Rounds up a data length to the next valid CAN-FD DLC size.
138    pub fn round_up_dlc(size: usize) -> usize {
139        match size {
140            0..=8 => size,
141            9..=12 => 12,
142            13..=16 => 16,
143            17..=20 => 20,
144            21..=24 => 24,
145            25..=32 => 32,
146            33..=48 => 48,
147            _ => 64,
148        }
149    }
150
151    /// Pads the frame data with `0x50` to the next valid CAN-FD DLC size.
152    pub fn pad_to_dlc(&mut self) {
153        let on_wire_size = Self::round_up_dlc(self.size as usize);
154        for i in self.size as usize..on_wire_size {
155            self.data[i] = 0x50;
156        }
157        self.size = on_wire_size as u8;
158    }
159}
160
161impl core::fmt::Debug for CanFdFrame {
162    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
163        let (source, destination, _) = parse_arbitration_id(self.arbitration_id);
164        f.debug_struct("CanFdFrame")
165            .field("channel", &self.channel)
166            .field(
167                "arbitration_id",
168                &format_args!("0x{:08x}", self.arbitration_id),
169            )
170            .field("size", &self.size)
171            .field("data", &format_args!("{:02x?}", self.payload()))
172            .field("destination", &destination)
173            .field("source", &source)
174            .finish()
175    }
176}
177
178/// Computes a moteus CAN arbitration ID from routing fields.
179///
180/// The moteus CAN protocol encodes routing information in the arbitration ID:
181/// - bits 28:16 = can_prefix (13 bits)
182/// - bit 15     = reply expected flag
183/// - bits 14:8  = source (7 bits)
184/// - bits 7:0   = destination (8 bits)
185///
186/// # Examples
187///
188/// ```
189/// use moteus_protocol::{calculate_arbitration_id, parse_arbitration_id};
190///
191/// // Build an arbitration ID for source=0, dest=1, no prefix, reply expected
192/// let arb_id = calculate_arbitration_id(0, 1, 0, true);
193/// assert_eq!(arb_id, 0x8001);
194///
195/// // Parse it back
196/// let (source, dest, prefix) = parse_arbitration_id(arb_id);
197/// assert_eq!(source, 0);
198/// assert_eq!(dest, 1);
199/// assert_eq!(prefix, 0);
200/// ```
201pub fn calculate_arbitration_id(
202    source: i8,
203    destination: i8,
204    can_prefix: u16,
205    reply_required: bool,
206) -> u32 {
207    let prefix = (can_prefix as u32) << 16;
208    let src = ((source as u8) as u32) << 8;
209    let dest = (destination as u8) as u32;
210    let reply = if reply_required { 0x8000 } else { 0 };
211    prefix | src | dest | reply
212}
213
214/// Extracts routing fields from a moteus CAN arbitration ID.
215///
216/// Returns (source, destination, can_prefix).
217/// This is the inverse of [`calculate_arbitration_id`].
218///
219/// # Examples
220///
221/// ```
222/// use moteus_protocol::parse_arbitration_id;
223///
224/// // Extract routing from a received frame's arbitration ID
225/// let (source, dest, prefix) = parse_arbitration_id(0x8100);
226/// assert_eq!(source, 1);  // device 1 sent this
227/// assert_eq!(dest, 0);    // addressed to us
228/// ```
229pub fn parse_arbitration_id(arb_id: u32) -> (i8, i8, u16) {
230    let source = ((arb_id >> 8) & 0x7F) as i8;
231    let destination = (arb_id & 0xFF) as i8;
232    let can_prefix = ((arb_id >> 16) & 0x1FFF) as u16;
233    (source, destination, can_prefix)
234}
235
236#[cfg(test)]
237mod tests {
238    use super::*;
239
240    #[test]
241    fn test_new_frame() {
242        let frame = CanFdFrame::new();
243        assert_eq!(frame.size, 0);
244        assert_eq!(frame.arbitration_id, 0);
245        assert!(frame.channel.is_none());
246    }
247
248    #[test]
249    fn test_payload() {
250        let mut frame = CanFdFrame::new();
251        frame.data[0] = 0x01;
252        frame.data[1] = 0x02;
253        frame.size = 2;
254
255        assert_eq!(frame.payload(), &[0x01, 0x02]);
256    }
257
258    #[test]
259    fn test_calculate_arbitration_id() {
260        // source=0, dest=1, no prefix, reply required
261        let arb_id = calculate_arbitration_id(0, 1, 0, true);
262        assert_eq!(arb_id, 0x8001);
263
264        // source=0, dest=1, no prefix, no reply
265        let arb_id = calculate_arbitration_id(0, 1, 0, false);
266        assert_eq!(arb_id, 0x0001);
267
268        // source=5, dest=3, prefix=0x10, reply required
269        let arb_id = calculate_arbitration_id(5, 3, 0x10, true);
270        assert_eq!(arb_id, 0x00_10_85_03);
271    }
272
273    #[test]
274    fn test_parse_arbitration_id() {
275        let (source, dest, prefix) = parse_arbitration_id(0x00_10_85_03);
276        assert_eq!(source, 5);
277        assert_eq!(dest, 3);
278        assert_eq!(prefix, 0x10);
279    }
280
281    #[test]
282    fn test_arbitration_id_roundtrip() {
283        let arb_id = calculate_arbitration_id(7, 42, 0x1A, true);
284        let (source, dest, prefix) = parse_arbitration_id(arb_id);
285        assert_eq!(source, 7);
286        assert_eq!(dest, 42);
287        assert_eq!(prefix, 0x1A);
288    }
289}
moteus_protocol/frame.rs

moteus_protocol/
frame.rs
