Parse the next message from a byte buffer. Returns (message, bytes_consumed) or None if incomplete.
(buf: &[u8])
| 59 | /// Parse the next message from a byte buffer. |
| 60 | /// Returns (message, bytes_consumed) or None if incomplete. |
| 61 | pub fn decode_message(buf: &[u8]) -> Option<(Message, usize)> { |
| 62 | if buf.is_empty() { |
| 63 | return None; |
| 64 | } |
| 65 | match buf[0] { |
| 66 | MSG_CAN_FRAME => { |
| 67 | // type(1) + flags(1) + id(4) + len(1) = 7 minimum |
| 68 | if buf.len() < 7 { |
| 69 | return None; |
| 70 | } |
| 71 | let flags = buf[1]; |
| 72 | let id = u32::from_le_bytes([buf[2], buf[3], buf[4], buf[5]]); |
| 73 | let len = buf[6] as usize; |
| 74 | if len > 8 || buf.len() < 7 + len { |
| 75 | return None; |
| 76 | } |
| 77 | let data = buf[7..7 + len].to_vec(); |
| 78 | let extended = (flags & 0x01) != 0; |
| 79 | Some(( |
| 80 | Message::Frame(CanFrame { |
| 81 | id, |
| 82 | extended, |
| 83 | data, |
| 84 | }), |
| 85 | 7 + len, |
| 86 | )) |
| 87 | } |
| 88 | MSG_POSE => { |
| 89 | if buf.len() < 25 { |
| 90 | return None; |
| 91 | } |
| 92 | let x = f64::from_le_bytes(buf[1..9].try_into().unwrap()); |
| 93 | let y = f64::from_le_bytes(buf[9..17].try_into().unwrap()); |
| 94 | let theta = f64::from_le_bytes(buf[17..25].try_into().unwrap()); |
| 95 | Some((Message::Pose { x, y, theta }, 25)) |
| 96 | } |
| 97 | _ => { |
| 98 | // Skip unknown byte |
| 99 | Some((Message::Frame(CanFrame { id: 0, extended: false, data: vec![] }), 1)) |
| 100 | } |
| 101 | } |
| 102 | } |
no test coverage detected