add crc checking

This commit is contained in:
Max Känner 2024-01-30 22:52:18 +01:00
parent eb5befa55c
commit be930d313b
2 changed files with 100 additions and 26 deletions

View File

@ -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"

View File

@ -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<Self, u8> {
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<T> RcChannels<T> {
impl<T> RcChannelsPacked<T> {
const BITS_PER_CHANNEL: usize = 11;
const fn upper(ch: usize) -> usize {
@ -111,21 +98,107 @@ impl<T> RcChannels<T> {
}
pub struct CrsfParser {
buffer: HistoryBuffer<u8, { Self::MAX_PACKET_SIZE }>,
buffer: Deque<u8, { Self::MAX_PACKET_SIZE }>,
}
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<Header> {
const CRC8_ALG: Crc<u8> = Crc::<u8>::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);
}
}