ross_protocol/
frame.rs

1use alloc::vec;
2use alloc::vec::Vec;
3use cobs::{decode, encode, max_encoding_length};
4
5use bxcan::{Data, ExtendedId, Frame as BxFrame, Id};
6
7/// Frame id for packets with more than one frame
8#[derive(Debug, PartialEq)]
9pub enum FrameId {
10    /// Last frame id inside current packet (12 bits)
11    LastFrameId(u16),
12    /// Current frame id (12 bits)
13    CurrentFrameId(u16),
14}
15
16#[derive(Debug, PartialEq)]
17pub enum FrameError {
18    /// Received a standard frame instead of an extended one
19    FrameIsStandard,
20    /// Received a remote frame instead of a data one
21    FrameIsRemote,
22    /// Part of the frame id is missing
23    FrameIdMissing,
24    /// Frame has a different size than expected
25    WrongSize,
26    // COBS decoding error
27    CobsError,
28}
29
30/// Ross compatible representation of a CAN frame
31#[derive(Debug, PartialEq)]
32pub struct Frame {
33    /// If this bit is low, the frame is considered to be an error frame
34    pub not_error_flag: bool,
35    /// If this bit is high, the frame is considered to be the first frame of a packet
36    pub start_frame_flag: bool,
37    /// If this bit is high, the frame is considered to be only a part of a packet
38    pub multi_frame_flag: bool,
39    /// Either the last or the current frame id inside current packet, depending on `start_frame_flag`
40    pub frame_id: FrameId,
41    /// Transmitting device's address
42    pub device_address: u16,
43    /// Length of frame data
44    pub data_len: u8,
45    /// Frame data
46    pub data: [u8; 8],
47}
48
49impl Frame {
50    /// Converts a bxcan frame to a ross frame
51    ///
52    /// This is the extended id structure for a ross frame:
53    /// bit 0:          NOT_ERROR_FLAG (if this bit is low, the frame is considered to be an error frame)
54    /// bit 1:          START_FRAME_FLAG (if this bit is high, the frame is considered to be the first frame of a packet)
55    /// bit 2:          MULTI_FRAME_FLAG (if this bit is high, the frame is considered to be only a part of a packet)
56    /// bits 3 - 7:     RESERVED (reserved for future use)
57    /// bits 8 - 11:    LAST_FRAME_ID (most significant nibble (0xf00) of the last frame id)
58    ///                 FRAME_ID (most significant nibble (0xf00) of the current frame id)
59    /// bits 12 - 27    DEVICE_ADDRESS (transmitting device's address)
60    ///
61    pub fn from_bxcan_frame(frame: BxFrame) -> Result<Self, FrameError> {
62        if let Id::Extended(id) = frame.id() {
63            let id = id.as_raw();
64
65            let not_error_flag = ((id >> 28) & 0x0001) != 0;
66            let start_frame_flag = ((id >> 27) & 0x0001) != 0;
67            let multi_frame_flag = ((id >> 26) & 0x0001) != 0;
68            let frame_id_nibble = ((id >> 16) & 0x000f) as u16;
69            let device_address = ((id >> 0) & 0xffff) as u16;
70
71            if let Some(frame_data) = frame.data() {
72                let data_len = frame.dlc();
73                let mut data = [0u8; 8];
74
75                for i in 0..(data_len as usize) {
76                    data[i] = frame_data[i];
77                }
78
79                if multi_frame_flag {
80                    if data_len == 0 {
81                        return Err(FrameError::FrameIdMissing);
82                    }
83
84                    let frame_id = if start_frame_flag {
85                        FrameId::LastFrameId((frame_id_nibble << 8) | data[0] as u16)
86                    } else {
87                        FrameId::CurrentFrameId((frame_id_nibble << 8) | data[0] as u16)
88                    };
89
90                    Ok(Frame {
91                        not_error_flag,
92                        start_frame_flag,
93                        multi_frame_flag,
94                        frame_id,
95                        device_address,
96                        data_len,
97                        data,
98                    })
99                } else {
100                    let start_frame_flag = true;
101                    let frame_id = FrameId::LastFrameId(0x00);
102
103                    Ok(Frame {
104                        not_error_flag,
105                        start_frame_flag,
106                        multi_frame_flag,
107                        frame_id,
108                        device_address,
109                        data_len,
110                        data,
111                    })
112                }
113            } else {
114                Err(FrameError::FrameIsRemote)
115            }
116        } else {
117            Err(FrameError::FrameIsStandard)
118        }
119    }
120
121    /// Converts a ross frame to a bxcan frame
122    pub fn to_bxcan_frame(&self) -> BxFrame {
123        let mut id = 0x00;
124        id |= (self.not_error_flag as u32) << 28;
125        id |= (self.start_frame_flag as u32) << 27;
126        id |= (self.multi_frame_flag as u32) << 26;
127        match self.frame_id {
128            FrameId::LastFrameId(frame_id) => id |= ((frame_id & 0x0f00) as u32 >> 8) << 16,
129            FrameId::CurrentFrameId(frame_id) => id |= ((frame_id & 0x0f00) as u32 >> 8) << 16,
130        }
131        id |= (self.device_address & 0xffff) as u32;
132
133        BxFrame::new_data(
134            ExtendedId::new(id).unwrap(),
135            Data::new(&self.data[0..self.data_len as usize]).unwrap(),
136        )
137    }
138
139    /// Converts a USART frame to a ross frame
140    ///
141    /// This is the structure for a USART frame:
142    /// byte 0:
143    ///     bit 0:      NOT_ERROR_FLAG (if this bit is low, the frame is considered to be an error frame)
144    ///     bit 1:      START_FRAME_FLAG (if this bit is high, the frame is considered to be the first frame of a packet)
145    ///     bit 2:      MULTI_FRAME_FLAG (if this bit is high, the frame is considered to be only a part of a packet)s
146    ///     bit 3:      RESERVED (reserved for future use)
147    ///     bits 4 - 7: LAST_FRAME_ID (most significant nibble (0xf00) of the last frame id)
148    ///                 FRAME_ID (most significant nibble (0xf00) of the current frame id)
149    ///
150    /// byte 1:         LAST_FRAME_ID (least significant byte (0x0ff) of the last frame id)
151    ///                 FRAME_ID (least significant byte (0x0ff) of the current frame id)
152    ///
153    /// byte 2:         DEVICE_ADDRESS (most significant byte (0xff00) of the device address)
154    /// byte 3:         DEVICE_ADDRESS (least significant byte (0x00ff) of the device address)
155    ///
156    /// byte 4:         DATA_LEN (length of frame data)
157    /// bytes 5 - 12:   DATA (frame data)
158    pub fn from_usart_frame(encoded: Vec<u8>) -> Result<Self, FrameError> {
159        let mut frame = vec![0; encoded.len()];
160        match decode(&encoded[..], &mut frame[..]) {
161            Ok(n) => frame.truncate(n),
162            Err(_) => return Err(FrameError::CobsError),
163        }
164
165        if frame.len() < 5 || frame.len() != frame[4] as usize + 5 {
166            return Err(FrameError::WrongSize);
167        }
168
169        let not_error_flag = ((frame[0] >> 7) & 0x01) != 0;
170        let start_frame_flag = ((frame[0] >> 6) & 0x01) != 0;
171        let multi_frame_flag = ((frame[0] >> 5) & 0x01) != 0;
172
173        let frame_id = if start_frame_flag {
174            FrameId::LastFrameId((((frame[0] & 0x0f) as u16) << 8) | frame[1] as u16)
175        } else {
176            FrameId::CurrentFrameId((((frame[0] & 0x0f) as u16) << 8) | frame[1] as u16)
177        };
178
179        let device_address = ((frame[2] as u16) << 8) | frame[3] as u16;
180        let data_len = frame[4];
181        let mut data = [0u8; 8];
182
183        for i in 0..data_len as usize {
184            data[i] = frame[i + 5];
185        }
186
187        Ok(Frame {
188            not_error_flag,
189            start_frame_flag,
190            multi_frame_flag,
191            frame_id,
192            device_address,
193            data_len,
194            data,
195        })
196    }
197
198    /// Converts a ross frame to a USART frame
199    pub fn to_usart_frame(&self) -> Vec<u8> {
200        let mut frame = vec![0x00u8; self.data_len as usize + 5];
201
202        // byte 0
203        frame[0] |= (self.not_error_flag as u8) << 7;
204        frame[0] |= (self.start_frame_flag as u8) << 6;
205        frame[0] |= (self.multi_frame_flag as u8) << 5;
206
207        match self.frame_id {
208            FrameId::LastFrameId(frame_id) => frame[0] |= ((frame_id & 0x0f00) >> 8) as u8,
209            FrameId::CurrentFrameId(frame_id) => frame[0] |= ((frame_id & 0x0f00) >> 8) as u8,
210        }
211
212        // byte 1
213        match self.frame_id {
214            FrameId::LastFrameId(frame_id) => frame[1] |= (frame_id & 0x00ff) as u8,
215            FrameId::CurrentFrameId(frame_id) => frame[1] |= (frame_id & 0x00ff) as u8,
216        }
217
218        // bytes 2 & 3
219        frame[2] = ((self.device_address & 0xff00) >> 8) as u8;
220        frame[3] = (self.device_address & 0x00ff) as u8;
221
222        // byte 4
223        frame[4] = self.data_len;
224
225        // bytes 5 - 12
226        for i in 0..self.data_len as usize {
227            frame[i + 5] = self.data[i];
228        }
229
230        let mut encoded = vec![0; max_encoding_length(frame.len())];
231        let encoded_len = encode(&frame[..], &mut encoded[..]);
232        encoded.truncate(encoded_len);
233        return encoded;
234    }
235}
236
237#[cfg(test)]
238mod tests {
239    use super::*;
240
241    const FRAME_ID: u32 = 0x1405_5555;
242    const FRAME_DATA: [u8; 8] = [0x55; 8];
243    const FRAME: Frame = Frame {
244        not_error_flag: true,
245        start_frame_flag: false,
246        multi_frame_flag: true,
247        frame_id: FrameId::CurrentFrameId(0x0555),
248        device_address: 0x5555,
249        data_len: 8,
250        data: FRAME_DATA,
251    };
252
253    #[test]
254    fn from_bxcan_frame_test() {
255        let bxcan_frame = BxFrame::new_data(ExtendedId::new(FRAME_ID).unwrap(), FRAME_DATA);
256        let ross_frame = Frame::from_bxcan_frame(bxcan_frame).unwrap();
257
258        assert_eq!(ross_frame, FRAME);
259    }
260
261    #[test]
262    fn to_bxcan_frame_test() {
263        let bxcan_frame = FRAME.to_bxcan_frame();
264        let bxcan_frame_expected =
265            BxFrame::new_data(ExtendedId::new(FRAME_ID).unwrap(), FRAME_DATA);
266
267        assert_eq!(bxcan_frame, bxcan_frame_expected);
268    }
269
270    #[test]
271    fn from_usart_frame_test() {
272        let usart_frame = vec![
273            0x0e, // cobs
274            0xa5, // byte 0
275            0x55, // frame id
276            0x55, // device address
277            0x55, // device address
278            0x08, // data len
279            0x55, // data
280            0x55, // data
281            0x55, // data
282            0x55, // data
283            0x55, // data
284            0x55, // data
285            0x55, // data
286            0x55, // data
287        ];
288
289        let ross_frame = Frame::from_usart_frame(usart_frame).unwrap();
290
291        assert_eq!(ross_frame, FRAME);
292    }
293
294    #[test]
295    fn to_usart_frame_test() {
296        let usart_frame = FRAME.to_usart_frame();
297        let usart_frame_expected = vec![
298            0x0e, // cobs
299            0xa5, // byte 0
300            0x55, // frame id
301            0x55, // device address
302            0x55, // device address
303            0x08, // data len
304            0x55, // data
305            0x55, // data
306            0x55, // data
307            0x55, // data
308            0x55, // data
309            0x55, // data
310            0x55, // data
311            0x55, // data
312        ];
313
314        assert_eq!(usart_frame, usart_frame_expected);
315    }
316}
ross_protocol/frame.rs

ross_protocol/
frame.rs
