bluerobotics_ping/
decoder.rs

1use tracing::info;
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(Debug)]
16pub enum DecoderResult {
17    Success(ProtocolMessage),
18    InProgress,
19    Error(ParseError),
20}
21
22#[derive(Debug)]
23pub enum DecoderState {
24    AwaitingStart1,
25    AwaitingStart2,
26    ReadingHeader,
27    ReadingPayload,
28    ReadingChecksum,
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        match self.state {
48            DecoderState::AwaitingStart1 => {
49                if byte == HEADER[0] {
50                    self.state = DecoderState::AwaitingStart2;
51                    return DecoderResult::InProgress;
52                }
53                return DecoderResult::Error(ParseError::InvalidStartByte);
54            }
55            DecoderState::AwaitingStart2 => {
56                if byte == HEADER[1] {
57                    self.state = DecoderState::ReadingHeader;
58                    self.buffer.clear();
59                    return DecoderResult::InProgress;
60                }
61                self.state = DecoderState::AwaitingStart1;
62                return DecoderResult::Error(ParseError::InvalidStartByte);
63            }
64            DecoderState::ReadingHeader => {
65                self.buffer.push(byte);
66                // Basic information is available, moving to payload state
67                if self.buffer.len() == 6 {
68                    self.message.payload_length =
69                        u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
70                    self.message.message_id = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
71                    self.message.src_device_id = self.buffer[4];
72                    self.message.dst_device_id = self.buffer[5];
73
74                    if self.message.payload_length == 0 {
75                        self.state = DecoderState::ReadingChecksum
76                    } else {
77                        self.state = DecoderState::ReadingPayload;
78                    }
79                    self.buffer.clear();
80                }
81                return DecoderResult::InProgress;
82            }
83            DecoderState::ReadingPayload => {
84                self.buffer.push(byte);
85                info!(
86                    "DecoderState : ReadingPayload {:?} {:?}",
87                    self.buffer.len(),
88                    self.message.payload_length
89                );
90                if self.buffer.len() == self.message.payload_length as usize {
91                    self.message.payload = self.buffer.clone();
92                    self.state = DecoderState::ReadingChecksum;
93                    self.buffer.clear();
94                }
95                return DecoderResult::InProgress;
96            }
97            DecoderState::ReadingChecksum => {
98                self.buffer.push(byte);
99                if self.buffer.len() == 2 {
100                    self.message.checksum = u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
101                    self.reset();
102                    let message = self.message.clone();
103                    self.message = ProtocolMessage::new();
104                    if !message.has_valid_crc() {
105                        return DecoderResult::Error(ParseError::ChecksumError(message));
106                    }
107                    return DecoderResult::Success(message);
108                }
109                return DecoderResult::InProgress;
110            }
111        }
112    }
113
114    fn reset(&mut self) {
115        self.state = DecoderState::AwaitingStart1;
116        self.buffer.clear();
117    }
118}
bluerobotics_ping/decoder.rs

bluerobotics_ping/
decoder.rs
