bluerobotics_ping/
decoder.rs

1use tracing::debug;
2
3use crate::message::{ProtocolMessage, HEADER};
4#[cfg(feature = "serde")]
5use serde::{Deserialize, Serialize};
6
7#[derive(Debug, PartialEq, Clone)]
8#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
9pub enum ParseError {
10    InvalidStartByte,
11    IncompleteData,
12    ChecksumError(ProtocolMessage),
13}
14
15#[derive(Clone, Debug)]
16pub enum DecoderState {
17    AwaitingStart1,
18    AwaitingStart2,
19    ReadingHeader,
20    ReadingPayload,
21    ReadingChecksum,
22}
23
24#[derive(Debug)]
25pub enum DecoderResult {
26    Success(ProtocolMessage),
27    InProgress(DecoderState),
28    Error(ParseError),
29}
30
31pub struct Decoder {
32    pub state: DecoderState,
33    buffer: Vec<u8>,
34    message: ProtocolMessage,
35}
36
37impl Decoder {
38    pub fn new() -> Self {
39        Self {
40            state: DecoderState::AwaitingStart1,
41            buffer: Vec::new(),
42            message: ProtocolMessage::new(),
43        }
44    }
45
46    pub fn parse_byte(&mut self, byte: u8) -> DecoderResult {
47        debug!("Parsing byte: 0x{byte:02x} ({byte})");
48        let state = &self.state;
49        match state {
50            DecoderState::AwaitingStart1 => {
51                if byte == HEADER[0] {
52                    self.state = DecoderState::AwaitingStart2;
53                    return DecoderResult::InProgress(self.state.clone());
54                }
55                return DecoderResult::Error(ParseError::InvalidStartByte);
56            }
57            DecoderState::AwaitingStart2 => {
58                if byte == HEADER[1] {
59                    self.state = DecoderState::ReadingHeader;
60                    self.buffer.clear();
61                    return DecoderResult::InProgress(self.state.clone());
62                }
63                self.state = DecoderState::AwaitingStart1;
64                return DecoderResult::Error(ParseError::InvalidStartByte);
65            }
66            DecoderState::ReadingHeader => {
67                self.buffer.push(byte);
68                // Basic information is available, moving to payload state
69                if self.buffer.len() == 6 {
70                    self.message.payload_length =
71                        u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
72                    self.message.message_id = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
73                    self.message.src_device_id = self.buffer[4];
74                    self.message.dst_device_id = self.buffer[5];
75
76                    if self.message.payload_length == 0 {
77                        self.state = DecoderState::ReadingChecksum
78                    } else {
79                        self.state = DecoderState::ReadingPayload;
80                    }
81                    self.buffer.clear();
82                }
83                return DecoderResult::InProgress(self.state.clone());
84            }
85            DecoderState::ReadingPayload => {
86                self.buffer.push(byte);
87                debug!(
88                    "DecoderState : ReadingPayload {:?} of {:?}",
89                    self.buffer.len(),
90                    self.message.payload_length
91                );
92                if self.buffer.len() == self.message.payload_length as usize {
93                    self.message.payload = self.buffer.clone();
94                    self.state = DecoderState::ReadingChecksum;
95                    self.buffer.clear();
96                }
97                return DecoderResult::InProgress(self.state.clone());
98            }
99            DecoderState::ReadingChecksum => {
100                self.buffer.push(byte);
101                if self.buffer.len() == 2 {
102                    self.message.checksum = u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
103                    self.reset();
104                    let message = self.message.clone();
105                    self.message = ProtocolMessage::new();
106                    if !message.has_valid_crc() {
107                        return DecoderResult::Error(ParseError::ChecksumError(message));
108                    }
109                    return DecoderResult::Success(message);
110                }
111                return DecoderResult::InProgress(self.state.clone());
112            }
113        }
114    }
115
116    fn reset(&mut self) {
117        self.state = DecoderState::AwaitingStart1;
118        self.buffer.clear();
119    }
120}
bluerobotics_ping/decoder.rs

bluerobotics_ping/
decoder.rs
