sbus_parser/
packet.rs

1#[derive(Default, PartialEq, Debug)]
2pub struct Data {
3    pub channels: [u16; 16],
4    pub channel17: bool,
5    pub channel18: bool,
6    pub frame_lost: bool,
7    pub failsafe: bool,
8}
9
10#[repr(C)]
11pub struct Packet {
12    _padding: u8,
13    header: u8,
14    channel_words: [u16; 11],
15    digital_and_flags: u8,
16    footer: u8,
17}
18
19pub const SBUS_PACKET_BEGIN: u8 = 0xF;
20pub const SBUS_PACKET_SIZE: usize = core::mem::size_of::<Packet>() - 1;
21
22impl Packet {
23    pub fn parse(&self) -> Data {
24        // shift(i) = (16 - (11 * i) % 16) % 16
25        const SHIFT: [u8; 16] = [0, 5, 10, 15, 4, 9, 14, 3, 8, 13, 2, 7, 12, 1, 6, 11];
26        // index(i) = (11 * (i + 1)) / 16
27        const INDEX: [u8; 16] = [0, 1, 2, 2, 3, 4, 4, 5, 6, 6, 7, 8, 8, 9, 10, 10];
28
29        let mut data = Data::default();
30        let mut bits: u32 = 0;
31        for i in 0..16 {
32            let word = u16::from_le(self.channel_words[INDEX[i] as usize]) as u32;
33            bits |= word << (SHIFT[i] as usize);
34            data.channels[i] = bits as u16 & ((1 << 11) - 1);
35            bits >>= 11;
36        }
37
38        data.channel17 = (self.digital_and_flags & (1 << 7)) > 0;
39        data.channel18 = (self.digital_and_flags & (1 << 6)) > 0;
40        data.frame_lost = (self.digital_and_flags & (1 << 5)) > 0;
41        data.failsafe = (self.digital_and_flags & (1 << 4)) > 0;
42        data
43    }
44}
45
46#[cfg(test)]
47mod tests {
48    use hex_literal::hex;
49
50    #[test]
51    fn test_sbus_packet() {
52        use super::{Data, Packet, SBUS_PACKET_SIZE};
53
54        assert_eq!(SBUS_PACKET_SIZE, 25);
55        let bytes =
56            hex!("00 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");
57
58        let sbus_packet: &Packet = unsafe { core::mem::transmute(&bytes) };
59        assert_eq!(
60            Data {
61                channels: [
62                    992, 992, 352, 992, 1376, 367, 352, 1376, 357, 352, 2020, 997, 0, 14, 0, 0
63                ],
64                channel17: false,
65                channel18: false,
66                frame_lost: false,
67                failsafe: false,
68            },
69            sbus_packet.parse()
70        );
71    }
72}
sbus_parser/packet.rs

sbus_parser/
packet.rs
