sbus_parser/
receiver.rs

1use crate::packet::{Data, Packet, SBUS_PACKET_BEGIN, SBUS_PACKET_SIZE};
2
3pub fn is_sbus_packet_end(byte: u8) -> bool {
4    match byte {
5        0x0 => true,  // S.BUS 1
6        0x4 => true,  // S.BUS 2 receiver voltage
7        0x14 => true, // S.BUS 2 GPS/baro
8        0x24 => true, // Unknown SBUS2 data
9        0x34 => true, // Unknown SBUS2 data
10        _ => false,
11    }
12}
13
14#[derive(Debug)]
15pub struct Receiver {
16    packet: [u8; 1 + SBUS_PACKET_SIZE],
17    size: usize,
18}
19
20impl Receiver {
21    pub fn new() -> Self {
22        Self { packet: [0u8; 1 + SBUS_PACKET_SIZE], size: 0 }
23    }
24
25    fn find_partial_packet(&mut self, bytes: &[u8]) {
26        for i in 0..bytes.len() {
27            if bytes[i] == SBUS_PACKET_BEGIN {
28                self.size = bytes.len() - i;
29                self.packet[1..1 + self.size].copy_from_slice(&bytes[i..]);
30                break;
31            }
32        }
33    }
34
35    fn continue_receive(&mut self, bytes: &[u8]) -> Option<Data> {
36        let packet = &mut self.packet[1..];
37        for offset in 0..self.size {
38            if packet[offset] != SBUS_PACKET_BEGIN {
39                continue;
40            }
41            let size = self.size - offset;
42            let remain_size = SBUS_PACKET_SIZE - size;
43            if bytes.len() < remain_size {
44                packet[size..size + bytes.len()].copy_from_slice(bytes);
45                self.size += bytes.len();
46                return None;
47            }
48            if is_sbus_packet_end(bytes[remain_size - 1]) {
49                packet.copy_within(offset..self.size, 0);
50                packet[size..].copy_from_slice(&bytes[..remain_size]);
51                let packet: &Packet = unsafe { core::mem::transmute(&self.packet) };
52                let data = Some(packet.parse());
53                self.size = 0;
54                self.find_partial_packet(&bytes[remain_size..]);
55                return data;
56            }
57        }
58        self.size = 0;
59        None
60    }
61
62    /// Must be chunk of SBUS PACKET SIZE or less
63    pub fn receive(&mut self, bytes: &[u8]) -> Option<Data> {
64        assert!(bytes.len() <= SBUS_PACKET_SIZE);
65        if self.size > 0 {
66            if let Some(data) = self.continue_receive(bytes) {
67                return Some(data);
68            }
69        }
70        let mut index = 0;
71        if bytes.len() == SBUS_PACKET_SIZE {
72            if bytes[0] == SBUS_PACKET_BEGIN && is_sbus_packet_end(bytes[SBUS_PACKET_SIZE - 1]) {
73                self.packet[1..].copy_from_slice(bytes);
74                let packet: &Packet = unsafe { core::mem::transmute(&self.packet) };
75                return Some(packet.parse());
76            }
77            index = 1;
78        }
79        self.find_partial_packet(&bytes[index..]);
80        None
81    }
82
83    pub fn reset(&mut self) {
84        self.size = 0;
85    }
86}
87
88#[cfg(test)]
89mod tests {
90    use hex_literal::hex;
91
92    use crate::packet::SBUS_PACKET_SIZE;
93
94    const SBUS_SAMPLE_PACKET: [u8; SBUS_PACKET_SIZE] =
95        hex!("0F E0 03 1F 58 C0 07 16 B0 80 05 2C 60 01 0B F8 C0 07 00 00 00 00 00 03 00");
96
97    #[test]
98    fn test_received_some() {
99        let mut receiver = super::Receiver::new();
100        assert_eq!(receiver.receive(&SBUS_SAMPLE_PACKET).unwrap().channels[0], 992);
101    }
102
103    #[test]
104    fn test_received_none() {
105        let mut receiver = super::Receiver::new();
106        assert!(receiver.receive(&[0xFFu8; SBUS_PACKET_SIZE]).is_none());
107    }
108
109    #[test]
110    fn test_partially_receive() {
111        for i in 1..SBUS_PACKET_SIZE {
112            let mut receiver = super::Receiver::new();
113            let mut bytes = [0u8; SBUS_PACKET_SIZE];
114            bytes[..i].copy_from_slice(&SBUS_SAMPLE_PACKET[SBUS_PACKET_SIZE - i..]);
115            bytes[i..].copy_from_slice(&SBUS_SAMPLE_PACKET[..SBUS_PACKET_SIZE - i]);
116            assert!(receiver.receive(&bytes).is_none());
117            assert_eq!(receiver.receive(&bytes).unwrap().channels[0], 992);
118        }
119    }
120
121    #[test]
122    fn test_footer_not_sbus() {
123        let mut receiver = super::Receiver::new();
124        let b = hex!("0F 00 FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF 01");
125        assert!(receiver.receive(&b).is_none());
126    }
127
128    #[test]
129    fn test_header_not_sbus() {
130        let mut receiver = super::Receiver::new();
131
132        let b = hex!("00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 0F 0F E0");
133        assert!(receiver.receive(&b).is_none());
134
135        let b = hex!("03 1F 58 C0 07 16 B0 80 05 2C 60 01 0B F8 C0 07 00 00 00 00 00 03 00 FF FF");
136        assert_eq!(receiver.receive(&b).unwrap().channels[0], 992);
137    }
138
139    #[test]
140    fn test_fragment() {
141        let mut receiver = super::Receiver::new();
142
143        assert!(receiver.receive(&[0xF]).is_none());
144        let b = hex!("00 FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF FF");
145        assert!(receiver.receive(&b).is_none());
146        assert!(receiver.receive(&[0]).is_some());
147    }
148}
sbus_parser/receiver.rs

sbus_parser/
receiver.rs
