diff --git a/Cargo.toml b/Cargo.toml index 68e7af4..17b1668 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,6 +22,7 @@ cargo = "warn" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] -num_enum = { version = "=0.5", default-features = false } -bitfield = { version = "=0.13" } -heapless = { version = "=0.7" } +num_enum = { version = ">=0.5", default-features = false } +bitfield = ">=0.13" +heapless = ">=0.7" +crc = ">=3.0" diff --git a/src/lib.rs b/src/lib.rs index 56d8576..a70248c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,8 +1,9 @@ #![cfg_attr(not(test), no_std)] use bitfield::bitfield; -use heapless::HistoryBuffer; -use num_enum::{IntoPrimitive, TryFromPrimitive, TryFromPrimitiveError}; +use crc::{Crc, CRC_8_DVB_S2}; +use heapless::Deque; +use num_enum::{IntoPrimitive, TryFromPrimitive}; #[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)] #[repr(u8)] @@ -56,28 +57,14 @@ pub enum Address { } #[derive(Debug, PartialEq, Eq, Clone, Copy)] -struct Header { +pub struct Header { address: Address, size: u8, frame_type: FrameType, } -impl Header { - fn try_from_bytes(bytes: [u8; 3]) -> Result { - Ok(Self { - address: bytes[0] - .try_into() - .map_err(|TryFromPrimitiveError { number }| number)?, - size: bytes[1], - frame_type: bytes[2] - .try_into() - .map_err(|TryFromPrimitiveError { number }| number)?, - }) - } -} - bitfield! { - struct RcChannels([u8]); + struct RcChannelsPacked([u8]); impl Debug; u16; ch1, set_ch1: Self::upper(1), Self::lower(1); @@ -98,7 +85,7 @@ bitfield! { ch16, set_ch16: Self::upper(16), Self::lower(16); } -impl RcChannels { +impl RcChannelsPacked { const BITS_PER_CHANNEL: usize = 11; const fn upper(ch: usize) -> usize { @@ -111,21 +98,107 @@ impl RcChannels { } pub struct CrsfParser { - buffer: HistoryBuffer, + buffer: Deque, } impl CrsfParser { const MAX_PACKET_SIZE: usize = 64; + + #[must_use] + pub const fn new() -> Self { + let buffer = Deque::new(); + Self { buffer } + } + + pub fn push(&mut self, byte: u8) { + if self.buffer.is_full() { + // deque used as ringbuffer. The oldest data is overwritten. + self.buffer.pop_front(); + } + let _ = self.buffer.push_front(byte); + } + + pub fn next_packet(&mut self) -> Option
{ + const CRC8_ALG: Crc = Crc::::new(&CRC_8_DVB_S2); + + while Address::try_from(*self.buffer.back()?).is_err() { + // The buffer doesn't start with a valid Address + self.buffer.pop_back()?; + } + if self.buffer.len() < 4 { + // The buffer doesn't even have enough data to contain a valid header + crc byte + return None; + } + + // Parse the packet header + let mut iter = self.buffer.iter().rev().copied(); + let address = Address::try_from(iter.next()?).ok()?; + let size = iter.next()?; + let frame_type = FrameType::try_from(iter.next()?).ok()?; + let header = Header { + address, + size, + frame_type, + }; + // TODO: check the frame size against the frame type. + + // The size field contains the number of bytes coming after it. + let packet_length = header.size + 2; + if self.buffer.len() < usize::from(packet_length) { + // The buffer doesn't have enough data to contain the specified packet length + return None; + } + + let mut crc_calc = CRC8_ALG.digest(); + crc_calc.update(&[header.frame_type.into()]); + let payload_iter = iter.clone().take(usize::from(packet_length - 4)); + for _ in 0..packet_length - 4 { + crc_calc.update(&[iter.next()?]); + } + let crc8 = crc_calc.finalize(); + let crc = iter.next()?; + if crc != crc8 { + // Invalid checksum. Make sure to pop The oldest byte so we don't parse this packet + // again + self.buffer.pop_back()?; + return None; + } + + // TODO: decode payload using type field + + // sucessfully decoded a packet + for _ in 0..packet_length { + self.buffer.pop_back()?; + } + Some(header) + } } #[cfg(test)] mod tests { + use crate::{Address, CrsfParser, FrameType, Header}; + #[test] fn rc_channels() { let input = [ - 0xc8, 0x18, 0x16, 0xe0, 0x3, 0x1f, 0xb3, 0xc0, 0xf7, 0xb, 0xee, 0x69, 0x8f, 0x15, 0xe0, - 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x78, 0xd9, 0xdb, 0xb4, 0xc8, + 0xc8, 0x18, 0x16, 0xe0, 0x03, 0x1f, 0xb3, 0xc0, 0xf7, 0x0b, 0xee, 0x69, 0x8f, 0x15, + 0xe0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0xd9, 0xdb, 0xb4, ]; - unimplemented!(); + + let mut parser = CrsfParser::new(); + for byte in input { + assert_eq!(parser.next_packet(), None); + parser.push(byte); + } + + assert_eq!( + parser.next_packet(), + Some(Header { + address: Address::FlightController, + size: 24, + frame_type: FrameType::RcChannelsPacked + }) + ); + assert_eq!(parser.next_packet(), None); } }