262 lines
7.4 KiB
Rust
262 lines
7.4 KiB
Rust
// #![cfg_attr(not(test), no_std)]
|
|
|
|
use bitfield::bitfield;
|
|
use crc::{Crc, CRC_8_DVB_S2};
|
|
use heapless::{Deque, Vec};
|
|
use num_enum::{IntoPrimitive, TryFromPrimitive};
|
|
|
|
#[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
|
|
#[repr(u8)]
|
|
pub enum FrameType {
|
|
Gps = 0x02,
|
|
Vario = 0x07,
|
|
BatterySensor = 0x08,
|
|
BaroAltitude = 0x09,
|
|
LinkStatistics = 0x14,
|
|
OpenTxSync = 0x10,
|
|
RadioId = 0x3A,
|
|
RcChannelsPacked = 0x16,
|
|
Attitude = 0x1E,
|
|
FlightMode = 0x21,
|
|
// Extended Header Frames
|
|
DevicePing = 0x28,
|
|
DeviceInfo = 0x29,
|
|
ParameterSettingsEntry = 0x2B,
|
|
ParameterRead = 0x2C,
|
|
ParameterWrite = 0x2D,
|
|
ELRSStatus = 0x2E,
|
|
Command = 0x32,
|
|
// KISS frames
|
|
KissReq = 0x78,
|
|
KissResp = 0x79,
|
|
// MSP commands
|
|
MspReq = 0x7A,
|
|
MspResp = 0x7B,
|
|
MspWrite = 0x7C,
|
|
// Ardupilot frames
|
|
ArdupilotResp = 0x80,
|
|
}
|
|
|
|
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
|
|
pub enum CrsfPacket {
|
|
RcChannels([u16; 16]),
|
|
}
|
|
|
|
impl CrsfPacket {
|
|
fn from_type_and_data(
|
|
frame_type: FrameType,
|
|
data: impl Iterator<Item = u8> + ExactSizeIterator,
|
|
) -> Option<Self> {
|
|
match frame_type {
|
|
FrameType::RcChannelsPacked => {
|
|
if data.len() != 22 {
|
|
return None;
|
|
}
|
|
let mut buf = Vec::<u8, 22>::new();
|
|
buf.extend(data);
|
|
let channels = RcChannelsPacked(&mut buf[..]);
|
|
Some(Self::RcChannels([
|
|
channels.ch1(),
|
|
channels.ch2(),
|
|
channels.ch3(),
|
|
channels.ch4(),
|
|
channels.ch5(),
|
|
channels.ch6(),
|
|
channels.ch7(),
|
|
channels.ch8(),
|
|
channels.ch9(),
|
|
channels.ch10(),
|
|
channels.ch11(),
|
|
channels.ch12(),
|
|
channels.ch13(),
|
|
channels.ch14(),
|
|
channels.ch15(),
|
|
channels.ch16(),
|
|
]))
|
|
}
|
|
_ => None,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
|
|
#[repr(u8)]
|
|
pub enum Address {
|
|
Broadcast = 0x00,
|
|
Usb = 0x10,
|
|
TbsCorePnpPro = 0x80,
|
|
Reserved1 = 0x8A,
|
|
CurrentSensor = 0xC0,
|
|
Gps = 0xC2,
|
|
TbsBlackbox = 0xC4,
|
|
FlightController = 0xC8,
|
|
Reserved2 = 0xCA,
|
|
RaceTag = 0xCC,
|
|
RadioTransmitter = 0xEA,
|
|
CrsfReceiver = 0xEC,
|
|
CrsfTrasmitter = 0xEE,
|
|
ElrsLua = 0xEF,
|
|
}
|
|
|
|
#[derive(Debug, PartialEq, Eq, Clone, Copy)]
|
|
pub struct Header {
|
|
address: Address,
|
|
size: u8,
|
|
frame_type: FrameType,
|
|
}
|
|
|
|
impl Header {
|
|
const fn valid(self) -> bool {
|
|
match self.frame_type {
|
|
FrameType::RcChannelsPacked => self.size == 24,
|
|
_ => true,
|
|
}
|
|
}
|
|
}
|
|
|
|
bitfield! {
|
|
struct RcChannelsPacked([u8]);
|
|
impl Debug;
|
|
u16;
|
|
ch1, set_ch1: Self::upper(1), Self::lower(1);
|
|
ch2, set_ch2: Self::upper(2), Self::lower(2);
|
|
ch3, set_ch3: Self::upper(3), Self::lower(3);
|
|
ch4, set_ch4: Self::upper(4), Self::lower(4);
|
|
ch5, set_ch5: Self::upper(5), Self::lower(5);
|
|
ch6, set_ch6: Self::upper(6), Self::lower(6);
|
|
ch7, set_ch7: Self::upper(7), Self::lower(7);
|
|
ch8, set_ch8: Self::upper(8), Self::lower(8);
|
|
ch9, set_ch9: Self::upper(9), Self::lower(9);
|
|
ch10, set_ch10: Self::upper(10), Self::lower(10);
|
|
ch11, set_ch11: Self::upper(11), Self::lower(11);
|
|
ch12, set_ch12: Self::upper(12), Self::lower(12);
|
|
ch13, set_ch13: Self::upper(13), Self::lower(13);
|
|
ch14, set_ch14: Self::upper(14), Self::lower(14);
|
|
ch15, set_ch15: Self::upper(15), Self::lower(15);
|
|
ch16, set_ch16: Self::upper(16), Self::lower(16);
|
|
}
|
|
|
|
impl<T> RcChannelsPacked<T> {
|
|
const BITS_PER_CHANNEL: usize = 11;
|
|
|
|
const fn upper(ch: usize) -> usize {
|
|
Self::lower(ch) + Self::BITS_PER_CHANNEL - 1
|
|
}
|
|
|
|
const fn lower(ch: usize) -> usize {
|
|
Self::BITS_PER_CHANNEL * (ch - 1)
|
|
}
|
|
}
|
|
|
|
pub struct CrsfParser {
|
|
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<(Address, CrsfPacket)> {
|
|
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.
|
|
if !header.valid() {
|
|
self.buffer.pop_back();
|
|
return None;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
// decode payload using type field
|
|
let packet = CrsfPacket::from_type_and_data(header.frame_type, payload_iter);
|
|
|
|
// sucessfully decoded a packet
|
|
for _ in 0..packet_length {
|
|
self.buffer.pop_back()?;
|
|
}
|
|
packet.map(|packet| (header.address, packet))
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use crate::{Address, CrsfPacket, CrsfParser};
|
|
|
|
#[test]
|
|
fn rc_channels() {
|
|
let input = [
|
|
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,
|
|
];
|
|
|
|
let mut parser = CrsfParser::new();
|
|
for byte in input {
|
|
assert_eq!(parser.next_packet(), None);
|
|
parser.push(byte);
|
|
}
|
|
|
|
assert_eq!(
|
|
parser.next_packet(),
|
|
Some((
|
|
Address::FlightController,
|
|
CrsfPacket::RcChannels([
|
|
992, 992, 716, 992, 191, 988, 986, 172, 992, 0, 0, 0, 0, 0, 1630, 1758
|
|
])
|
|
))
|
|
);
|
|
assert_eq!(parser.next_packet(), None);
|
|
}
|
|
}
|