Major refactoring of all parts of parser, simpler and faster.

Cargo.toml

@@ -1,17 +1,17 @@
 [package]
 name = "sbus"
-version = "0.2.2"
-authors = ["Ze'ev Klapow <zklapow@gmail.com>"]
-repository = "https://github.com/zklapow/sbus"
-categories = ["embedded", "no-std", "parser-implementations", "science::robotics"]
+version = "0.3.0"
+authors = ["Peter Krull"]
+repository = "https://github.com/peterkrull/sbus"
+categories = ["embedded-hal", "embedded", "no-std", "parser-implementations", "science::robotics"]
 license = "Apache-2.0"
 maintenance = { status = "actively-developed" }
-description = "A simple parser for the S.Bus RC protocol"
-edition = "2018"
+description = "A simple parser for the S.Bus RC protocol, with optional embedded-hal UART support"
+edition = "2021"
 
 [dependencies]
 arraydeque = { version = "~0.4", default-features = false }
-embedded-hal = { version = "0.2.7" , optional = true }
+embedded-hal = { version = "*" , optional = true }
 
 [features]
 embedded-hal = ["dep:embedded-hal"]

src/lib.rs

@@ -5,12 +5,13 @@ use arraydeque::{ArrayDeque, Wrapping};
 #[cfg(feature = "embedded-hal")]
 use embedded_hal::serial::Read;
 
-// The flag by should start with 4 0s
-const SBUS_FLAG_BYTE_MASK: u8 = 0b11110000;
-const SBUS_HEADER_BYTE: u8 = 0x0F;
-const SBUS_FOOTER_BYTE: u8 = 0b00000000;
+// Important bytes for correctnes checks
+const FLAG_MASK: u8 = 0b11110000;
+const HEAD_BYTE: u8 = 0b00001111;
+const FOOT_BYTE: u8 = 0b00000000;
 
-const SBUS_PACKET_SIZE: usize = 25;
+// Number of bytes in SBUS message
+const PACKET_SIZE: usize = 25;
 
 #[derive(Debug, Ord, PartialOrd, Eq, PartialEq, Copy, Clone)]
 pub struct SBusPacket {
@@ -22,7 +23,7 @@ pub struct SBusPacket {
 }
 
 pub struct SBusPacketParser {
-    buffer: ArrayDeque<[u8; (SBUS_PACKET_SIZE * 2) as usize], Wrapping>,
+    buffer: ArrayDeque<[u8; (PACKET_SIZE * 2) as usize], Wrapping>,
 }
 
 impl SBusPacketParser {
@@ -32,102 +33,100 @@ impl SBusPacketParser {
         }
     }
 
+    /// Push single `u8` byte into buffer.
+    #[inline(always)]
+    pub fn push_byte(&mut self, byte: u8) {
+        let _ = self.buffer.push_back(byte);
+    }
+
+    /// Push array of `u8` bytes into buffer.
     pub fn push_bytes(&mut self, bytes: &[u8]) {
-        bytes.iter().for_each(|b| {
-            self.buffer.push_back(*b);
+        bytes.iter().for_each(|byte| {
+            self.push_byte(*byte);
         })
     }
 
+    /// Exhaustively reads the bytes from uart device implenting
+    /// the `embedded_hal::serial::Read<u8>` trait.
     #[cfg(feature = "embedded-hal")]
     pub fn read_serial<U : Read<u8>>(&mut self, uart : & mut U) {
-        while let Ok(word) = uart.read() {
-            self.push_bytes(&[word])
+        while let Ok(byte) = uart.read() {
+            self.push_byte(byte);
         }
     }
 
+    /// Equivalent to consequtively calling `read_serial()` and `try_parse()`.
+    #[cfg(feature = "embedded-hal")]
+    pub fn read_serial_try_parse<U : Read<u8>>
+    (&mut self, uart : & mut U) -> Option<SBusPacket> {
+        self.read_serial(uart);
+        self.try_parse()
+    }
+
+    /// Attempts to parse a valid SBUS packet from the buffer
     pub fn try_parse(&mut self) -> Option<SBusPacket> {
-        // We can't have a packet if we don't have enough bytes
-        if self.buffer.len() < SBUS_PACKET_SIZE {
-            return None;
+
+        // Pop bytes until head byte is first        
+        while *self.buffer.front()? != HEAD_BYTE
+        && self.buffer.len() > PACKET_SIZE {
+            self.buffer.pop_front()?;
         }
 
-        // If the first byte is not a header byte,
-        if *self.buffer.get(0).unwrap() != SBUS_HEADER_BYTE {
-            while self.buffer.len() > 0 && *self.buffer.get(0).unwrap() != SBUS_HEADER_BYTE {
-                let _ = self.buffer.pop_front().unwrap();
-                //println!("Popped byte: {:#?}", popped);
-            }
-
-            return None;
-        } else if *self.buffer.get(SBUS_PACKET_SIZE - 1).unwrap() == SBUS_FOOTER_BYTE
-            && self.buffer.get(SBUS_PACKET_SIZE - 2).unwrap() & SBUS_FLAG_BYTE_MASK == 0
-        {
-            // This seems like a valid packet!
-            // Start popping the bytes
-
-            let mut data_bytes: [u16; 23] = [0; 23];
-            for i in 0..23 {
-                data_bytes[i] = self.buffer.pop_front().unwrap_or(0) as u16;
-            }
-
-            let mut channels: [u16; 16] = [0; 16];
-
-            channels[0] = (((data_bytes[1]) | (data_bytes[2] << 8)) as u16 & 0x07FF).into();
-            channels[1] = ((((data_bytes[2] >> 3) | (data_bytes[3] << 5)) as u16) & 0x07FF).into();
-            channels[2] = ((((data_bytes[3] >> 6) | (data_bytes[4] << 2) | (data_bytes[5] << 10))
-                as u16)
-                & 0x07FF)
-                .into();
-            channels[3] = ((((data_bytes[5] >> 1) | (data_bytes[6] << 7)) as u16) & 0x07FF).into();
-            channels[4] = ((((data_bytes[6] >> 4) | (data_bytes[7] << 4)) as u16) & 0x07FF).into();
-            channels[5] = ((((data_bytes[7] >> 7) | (data_bytes[8] << 1) | (data_bytes[9] << 9))
-                as u16)
-                & 0x07FF)
-                .into();
-            channels[6] = ((((data_bytes[9] >> 2) | (data_bytes[10] << 6)) as u16) & 0x07FF).into();
-            channels[7] =
-                ((((data_bytes[10] >> 5) | (data_bytes[11] << 3)) as u16) & 0x07FF).into();
-            channels[8] = ((((data_bytes[12]) | (data_bytes[13] << 8)) as u16) & 0x07FF).into();
-            channels[9] =
-                ((((data_bytes[13] >> 3) | (data_bytes[14] << 5)) as u16) & 0x07FF).into();
-            channels[10] =
-                ((((data_bytes[14] >> 6) | (data_bytes[15] << 2) | (data_bytes[16] << 10)) as u16)
-                    & 0x07FF)
-                    .into();
-            channels[11] =
-                ((((data_bytes[16] >> 1) | (data_bytes[17] << 7)) as u16) & 0x07FF).into();
-            channels[12] =
-                ((((data_bytes[17] >> 4) | (data_bytes[18] << 4)) as u16) & 0x07FF).into();
-            channels[13] =
-                ((((data_bytes[18] >> 7) | (data_bytes[19] << 1) | (data_bytes[20] << 9)) as u16)
-                    & 0x07FF)
-                    .into();
-            channels[14] =
-                ((((data_bytes[20] >> 2) | (data_bytes[21] << 6)) as u16) & 0x07FF).into();
-            channels[15] =
-                ((((data_bytes[21] >> 5) | (data_bytes[22] << 3)) as u16) & 0x07FF).into();
-
-            let flag_byte = self.buffer.pop_front().unwrap_or(0);
-
-            return Some(SBusPacket {
-                channels,
-                d1: is_flag_set(flag_byte, 0),
-                d2: is_flag_set(flag_byte, 1),
-                frame_lost: is_flag_set(flag_byte, 2),
-                failsafe: is_flag_set(flag_byte, 3),
-            });
-        } else {
-            // We had a header byte, but this doesnt appear to be a valid frame, we are probably out of sync
-            // Pop until we find a header again
-            while self.buffer.len() > 0 && *self.buffer.get(0).unwrap() != SBUS_HEADER_BYTE {
-                self.buffer.pop_front();
-            }
+        // Check if entire frame is valid
+        if !self._valid_frame() { return None }
+
+        // Extract the relevant data from buffer
+        let mut data = [0; 24];
+        for d in data.iter_mut() {
+            *d = self.buffer.pop_front()? as u16
         }
 
-        return None;
+        // Initialize channels with 11-bit mask
+        let mut ch: [u16; 16] = [0x07FF; 16];
+
+        // Trust me bro
+        ch[0]  &= data[1]       | data[2]  << 8;
+        ch[1]  &= data[2]  >> 3 | data[3]  << 5;
+        ch[2]  &= data[3]  >> 6 | data[4]  << 2 | data[5] << 10;
+        ch[3]  &= data[5]  >> 1 | data[6]  << 7;
+        ch[4]  &= data[6]  >> 4 | data[7]  << 4;
+        ch[5]  &= data[7]  >> 7 | data[8]  << 1 | data[9] << 9;
+        ch[6]  &= data[9]  >> 2 | data[10] << 6;
+        ch[7]  &= data[10] >> 5 | data[11] << 3;
+
+        ch[8]  &= data[12]      | data[13] << 8;
+        ch[9]  &= data[13] >> 3 | data[14] << 5;
+        ch[10] &= data[14] >> 6 | data[15] << 2 | data[16] << 10;
+        ch[11] &= data[16] >> 1 | data[17] << 7;
+        ch[12] &= data[17] >> 4 | data[18] << 4;
+        ch[13] &= data[18] >> 7 | data[19] << 1 | data[20] << 9;
+        ch[14] &= data[20] >> 2 | data[21] << 6;
+        ch[15] &= data[21] >> 5 | data[22] << 3;
+
+        let flag_byte = *data.get(23)? as u8;
+
+        return Some(SBusPacket {
+            channels:   ch,
+            d1:         is_flag_set(flag_byte, 0),
+            d2:         is_flag_set(flag_byte, 1),
+            frame_lost: is_flag_set(flag_byte, 2),
+            failsafe:   is_flag_set(flag_byte, 3),
+        });
+    }
+
+    /// Returns `true` if the first part of the buffer contains a valid SBUS frame
+    fn _valid_frame (&self) -> bool {
+        if let (Some(head),Some(foot),Some(flag)) =
+        (self.buffer.front(),self.buffer.get(PACKET_SIZE - 1),self.buffer.get(PACKET_SIZE - 2)) {
+
+            // If the header, footer, and flag bytes exist, this condition should hold true
+            *head == HEAD_BYTE && *foot == FOOT_BYTE && *flag & FLAG_MASK == 0
+
+        } else { false }
     }
 }
 
-fn is_flag_set(flag_byte: u8, idx: u8) -> bool {
-    ( flag_byte >> idx ) & 1 == 1
+#[inline(always)]
+fn is_flag_set(flag_byte: u8, shift_by: u8) -> bool {
+    ( flag_byte >> shift_by ) & 1 == 1
 }