crsf/
lib.rs

1//! This crate provides a #\[no_std\] parser for the crossfire protocol.
2//! # Usage
3//! ### Packet Parsing
4//! ```rust
5//! use crsf::{PacketParser, PacketAddress, PacketType};
6//!
7//! let mut parser = PacketParser::<1024>::new();
8//!
9//! // Sync
10//! parser.push_bytes(&[PacketAddress::Controller as u8]);
11//! // Len
12//! parser.push_bytes(&[24]);
13//! // Type
14//! parser.push_bytes(&[PacketType::RcChannelsPacked as u8]);
15//! // Payload
16//! parser.push_bytes(&[0; 22]);
17//! // Checksum
18//! parser.push_bytes(&[239]);
19//!
20//! while let Some(Ok((dest, packet))) = parser.next_packet() {
21//!     println!("{:?} {:?}", dest, packet);
22//! }
23//! ```
24//! ### Packet Construction
25//! ```rust
26//! use crsf::{Packet, PacketAddress, RcChannels};
27//!
28//! let channels: [u16; 16] = [0xffff; 16];
29//! let packet = Packet::RcChannels(RcChannels(channels));
30//! let raw_packet = packet.into_raw(PacketAddress::Transmitter);
31//! println!("{:?}", raw_packet.data());
32//! ```
33
34#![no_std]
35// #![warn(missing_docs)]
36
37use crc::{Crc, CRC_8_DVB_S2};
38#[cfg(feature = "defmt")]
39use defmt;
40use snafu::prelude::*;
41
42use buffer::CircularBuffer;
43
44pub use packets::*;
45
46mod buffer;
47mod packets;
48
49/// Max crsf packet length
50pub const MAX_PACKET_LENGTH: usize = 64;
51/// Crsf packet header length
52pub const PACKET_HEADER_LENGTH: usize = 2;
53
54/// Crossfire packet parser
55pub struct PacketParser<const C: usize> {
56    buf: CircularBuffer<C>,
57}
58
59impl<const C: usize> PacketParser<C> {
60    // Packet type and checksum bytes are mandatory
61    const MIN_DATA_LENGTH: u8 = 2;
62    // Number of bytes of packet type, payload and checksum
63    const MAX_DATA_LENGTH: u8 = MAX_PACKET_LENGTH as u8 - Self::MIN_DATA_LENGTH;
64
65    /// Creates a new PacketParser struct
66    pub const fn new() -> Self {
67        Self {
68            buf: CircularBuffer::new(),
69        }
70    }
71
72    /// Clears the buffer
73    pub fn clear(&mut self) {
74        self.buf.clear();
75    }
76
77    /// Pushes the given bytes into the buffer
78    pub fn push_bytes(&mut self, bytes: &[u8]) {
79        bytes.iter().for_each(|&val| {
80            self.buf.push_back(val);
81        });
82    }
83
84    /// Reads from the buffer the next packet without parsing it's payload
85    pub fn next_raw_packet(&mut self) -> Option<Result<RawPacket, PacketError>> {
86        self.sync();
87
88        if self.buf.len() < PACKET_HEADER_LENGTH {
89            return None;
90        }
91
92        let len_byte = self.buf.peek_front(1).unwrap();
93        if !(Self::MIN_DATA_LENGTH..=Self::MAX_DATA_LENGTH).contains(&len_byte) {
94            for _ in 0..PACKET_HEADER_LENGTH {
95                self.buf.pop_front();
96            }
97            return Some(Err(PacketError::InvalidLength { len: len_byte }));
98        }
99        let len = PACKET_HEADER_LENGTH + len_byte as usize;
100
101        if len > self.buf.len() {
102            return None;
103        }
104
105        let mut data: [u8; MAX_PACKET_LENGTH] = [0; MAX_PACKET_LENGTH];
106        for c in data.iter_mut() {
107            *c = self.buf.pop_front().unwrap_or(0);
108        }
109
110        Some(Ok(RawPacket { data, len }))
111    }
112
113    /// Reads from the buffer the next packet
114    pub fn next_packet(&mut self) -> Option<Result<(PacketAddress, Packet), PacketError>> {
115        self.next_raw_packet().map(|raw_packet| match raw_packet {
116            Ok(raw_packet) => {
117                let destination = PacketAddress::from_u8(raw_packet.data[0]).unwrap();
118                let packet = Packet::from_raw(&raw_packet)?;
119                Ok((destination, packet))
120            }
121            Err(err) => Err(err),
122        })
123    }
124
125    fn sync(&mut self) {
126        while self
127            .buf
128            .peek_front(0)
129            .is_some_and(|val| PacketAddress::from_u8(val).is_none())
130        {
131            self.buf.pop_front();
132        }
133    }
134}
135
136/// Represents different kinds of packets
137#[non_exhaustive]
138#[derive(Clone, Debug)]
139pub enum Packet {
140    LinkStatistics(LinkStatistics),
141    RcChannels(RcChannels),
142}
143
144impl Packet {
145    /// Creates a packet from a raw packet
146    pub fn from_raw(raw_packet: &RawPacket) -> Result<Self, PacketError> {
147        let data = raw_packet.data();
148
149        let checksum_idx = data.len() - 1;
150        let checksum = Self::calculate_checksum(&data[2..checksum_idx]);
151        if checksum != data[checksum_idx] {
152            return Err(PacketError::ChecksumMismatch {
153                expected: checksum,
154                actual: data[checksum_idx],
155            });
156        }
157
158        let raw_type = data[2];
159        let payload_data = &data[3..];
160        let packet = match PacketType::from_u8(raw_type) {
161            Some(PacketType::RcChannelsPacked) => {
162                Packet::RcChannels(RcChannels::parse(payload_data))
163            }
164            Some(PacketType::LinkStatistics) => {
165                Packet::LinkStatistics(LinkStatistics::parse(payload_data))
166            }
167            _ => return Err(PacketError::UnknownType { raw_type }),
168        };
169        Ok(packet)
170    }
171
172    /// Creates a raw packet from a packet
173    pub fn into_raw(&self, addr: PacketAddress) -> RawPacket {
174        let mut data: [u8; MAX_PACKET_LENGTH] = [0; MAX_PACKET_LENGTH];
175
176        data[0] = addr as u8;
177
178        let payload = match self {
179            Packet::LinkStatistics(payload) => payload as &dyn Payload,
180            Packet::RcChannels(payload) => payload as &dyn Payload,
181        };
182
183        let len_byte = payload.len() + 2;
184        let len = PACKET_HEADER_LENGTH + len_byte as usize;
185        data[1] = len_byte;
186
187        data[2] = payload.packet_type() as u8;
188
189        payload.dump(&mut data[3..]);
190
191        let checksum_idx = len - 1;
192        data[checksum_idx] = Self::calculate_checksum(&data[2..checksum_idx]);
193
194        RawPacket { data, len }
195    }
196
197    fn calculate_checksum(data: &[u8]) -> u8 {
198        let crc8_alg = Crc::<u8>::new(&CRC_8_DVB_S2);
199        crc8_alg.checksum(data)
200    }
201}
202
203/// Stores raw packet data
204#[derive(Clone, Debug)]
205pub struct RawPacket {
206    data: [u8; MAX_PACKET_LENGTH],
207    len: usize,
208}
209
210impl RawPacket {
211    /// Returns the raw data
212    pub fn data(&self) -> &[u8] {
213        &self.data[..self.len]
214    }
215}
216
217/// Represents packet errors
218#[non_exhaustive]
219#[derive(Debug, PartialEq, Snafu)]
220#[cfg_attr(feature = "defmt", derive(defmt::Format))]
221pub enum PacketError {
222    #[snafu(display("Invalid length: {len}"))]
223    InvalidLength { len: u8 },
224    #[snafu(display("Unknown type: {raw_type:#04x}"))]
225    UnknownType { raw_type: u8 },
226    #[snafu(display("Checksum mismatch: expected {expected:#04x} but was {actual:#04x}"))]
227    ChecksumMismatch { expected: u8, actual: u8 },
228}
229
230/// Represents packet addresses
231#[non_exhaustive]
232#[derive(Debug, Clone, Copy, PartialEq, Eq)]
233#[repr(u8)]
234pub enum PacketAddress {
235    Transmitter = 0xEE,
236    Handset = 0xEA,
237    Controller = 0xC8,
238    Receiver = 0xEC,
239}
240
241impl PacketAddress {
242    pub fn from_u8(val: u8) -> Option<Self> {
243        match val {
244            0xEE => Some(PacketAddress::Transmitter),
245            0xEA => Some(PacketAddress::Handset),
246            0xC8 => Some(PacketAddress::Controller),
247            0xEC => Some(PacketAddress::Receiver),
248            _ => None,
249        }
250    }
251}
252
253/// Crossfire packet types
254#[non_exhaustive]
255#[derive(Debug, Clone, Copy, PartialEq, Eq)]
256#[repr(u8)]
257pub enum PacketType {
258    Gps = 0x02,
259    Vario = 0x07,
260    BatterySensor = 0x08,
261    BaroAltitude = 0x09,
262    LinkStatistics = 0x14,
263    OpenTxSync = 0x10,
264    RadioId = 0x3A,
265    RcChannelsPacked = 0x16,
266    Altitude = 0x1E,
267    FlightMode = 0x21,
268    DevicePing = 0x28,
269    DeviceInfo = 0x29,
270    ParameterSettingsEntry = 0x2B,
271    ParameterRead = 0x2C,
272    ParameterWrite = 0x2D,
273    Command = 0x32,
274    KissRequest = 0x78,
275    KissResponse = 0x79,
276    MspRequest = 0x7A,
277    MspResponse = 0x7B,
278    MspWrite = 0x7C,
279    ArdupilotResponse = 0x80,
280}
281
282impl PacketType {
283    pub fn from_u8(val: u8) -> Option<Self> {
284        match val {
285            0x02 => Some(PacketType::Gps),
286            0x07 => Some(PacketType::Vario),
287            0x08 => Some(PacketType::BatterySensor),
288            0x09 => Some(PacketType::BaroAltitude),
289            0x14 => Some(PacketType::LinkStatistics),
290            0x10 => Some(PacketType::OpenTxSync),
291            0x3A => Some(PacketType::RadioId),
292            0x16 => Some(PacketType::RcChannelsPacked),
293            0x1E => Some(PacketType::Altitude),
294            0x21 => Some(PacketType::FlightMode),
295            0x28 => Some(PacketType::DevicePing),
296            0x29 => Some(PacketType::DeviceInfo),
297            0x2B => Some(PacketType::ParameterSettingsEntry),
298            0x2C => Some(PacketType::ParameterRead),
299            0x2D => Some(PacketType::ParameterWrite),
300            0x32 => Some(PacketType::Command),
301            0x78 => Some(PacketType::KissRequest),
302            0x79 => Some(PacketType::KissResponse),
303            0x7A => Some(PacketType::MspRequest),
304            0x7B => Some(PacketType::MspResponse),
305            0x7C => Some(PacketType::MspWrite),
306            0x80 => Some(PacketType::ArdupilotResponse),
307            _ => None,
308        }
309    }
310}
311
312#[cfg(test)]
313mod tests {
314    use crate::LinkStatistics;
315    use crate::Packet;
316    use crate::PacketAddress;
317
318    use super::PacketError;
319    use super::PacketParser;
320    use super::PacketType;
321    use super::RcChannels;
322
323    #[test]
324    fn test_parse_next_packet() {
325        let mut parser = PacketParser::<1024>::new();
326
327        let addr = PacketAddress::Controller;
328        let typ = PacketType::RcChannelsPacked;
329
330        // Sync
331        parser.push_bytes(&[addr as u8]);
332        assert!(matches!(parser.next_packet(), None));
333        // Len
334        parser.push_bytes(&[24]);
335        assert!(matches!(parser.next_packet(), None));
336        // Type
337        parser.push_bytes(&[typ as u8]);
338        assert!(matches!(parser.next_packet(), None));
339        // Payload
340        parser.push_bytes(&[0; 22]);
341        assert!(matches!(parser.next_packet(), None));
342        // Checksum
343        parser.push_bytes(&[239]);
344
345        match parser.next_packet() {
346            None => panic!("Packet expected"),
347            Some(Ok((_, packet))) => {
348                if let Packet::RcChannels(RcChannels(channels)) = packet {
349                    assert_eq!(channels, [0; 16]);
350                } else {
351                    panic!("Packet was supposed to be of type rc channels");
352                }
353            }
354            Some(Err(e)) => panic!("Packet expected instead of an error: {}", e),
355        }
356        assert!(matches!(parser.next_packet(), None));
357    }
358
359    #[test]
360    fn test_parse_next_packet_with_validation_error() {
361        let mut parser = PacketParser::<1024>::new();
362
363        let addr = PacketAddress::Controller;
364
365        // Sync
366        parser.push_bytes(&[addr as u8]);
367        assert!(matches!(parser.next_packet(), None));
368        // Len
369        parser.push_bytes(&[24]);
370        assert!(matches!(parser.next_packet(), None));
371        // Type
372        parser.push_bytes(&[PacketType::RcChannelsPacked as u8]);
373        assert!(matches!(parser.next_packet(), None));
374        // Payload
375        parser.push_bytes(&[0; 22]);
376        assert!(matches!(parser.next_packet(), None));
377        // Checksum
378        parser.push_bytes(&[42]);
379
380        match parser.next_packet() {
381            None | Some(Ok(_)) => panic!("Validation error expected"),
382            Some(Err(e)) => {
383                assert_eq!(
384                    e,
385                    PacketError::ChecksumMismatch {
386                        expected: 239,
387                        actual: 42
388                    }
389                )
390            }
391        }
392        assert!(matches!(parser.next_packet(), None));
393    }
394
395    #[test]
396    fn test_parse_next_packet_with_zero_len() {
397        let mut parser = PacketParser::<1024>::new();
398
399        let addr = PacketAddress::Controller;
400
401        // Sync
402        parser.push_bytes(&[addr as u8]);
403        assert!(matches!(parser.next_packet(), None));
404        // Len
405        parser.push_bytes(&[0]);
406        match parser.next_packet() {
407            None | Some(Ok(_)) => panic!("Validation error expected"),
408            Some(Err(e)) => {
409                assert_eq!(e, PacketError::InvalidLength { len: 0 })
410            }
411        }
412        assert!(matches!(parser.next_packet(), None));
413    }
414
415    #[test]
416    fn test_parse_next_packet_with_max_len() {
417        let mut parser = PacketParser::<1024>::new();
418
419        let addr = PacketAddress::Controller;
420
421        // Sync
422        parser.push_bytes(&[addr as u8]);
423        assert!(matches!(parser.next_packet(), None));
424        // Len
425        parser.push_bytes(&[62]);
426        // Type
427        parser.push_bytes(&[0xff]);
428        // Payload
429        parser.push_bytes(&[0; 60]);
430        // Checksum
431        parser.push_bytes(&[33]);
432        match parser.next_packet() {
433            None | Some(Ok(_)) => panic!("Validation error expected"),
434            Some(Err(e)) => {
435                assert_eq!(e, PacketError::UnknownType { raw_type: 0xff })
436            }
437        }
438        assert!(matches!(parser.next_packet(), None));
439    }
440
441    #[test]
442    fn test_parse_next_packet_with_too_big_len() {
443        let mut parser = PacketParser::<1024>::new();
444
445        // Sync
446        parser.push_bytes(&[0xc8]);
447        assert!(matches!(parser.next_packet(), None));
448        // Len
449        parser.push_bytes(&[63]);
450        match parser.next_packet() {
451            None | Some(Ok(_)) => panic!("Validation error expected"),
452            Some(Err(e)) => {
453                assert_eq!(e, PacketError::InvalidLength { len: 63 })
454            }
455        }
456        assert!(matches!(parser.next_packet(), None));
457    }
458
459    #[test]
460    fn test_packet_construction() {
461        let addr = PacketAddress::Controller;
462
463        let channels: [u16; 16] = [0; 16];
464        let packet = Packet::RcChannels(RcChannels(channels));
465
466        let raw_packet = packet.into_raw(addr);
467
468        let packet2 = Packet::from_raw(&raw_packet).unwrap();
469        if let Packet::RcChannels(RcChannels(channels2)) = packet2 {
470            assert_eq!(channels, channels2);
471        }
472    }
473
474    #[test]
475    fn test_rc_channels_packet_into_raw() {
476        let channels: [u16; 16] = [0xffff; 16];
477        let packet = Packet::RcChannels(RcChannels(channels));
478
479        let raw_packet = packet.into_raw(PacketAddress::Transmitter);
480        let mut expected_data: [u8; 26] = [0xff; 26];
481        expected_data[0] = 0xee;
482        expected_data[1] = 24;
483        expected_data[2] = 0x16;
484        expected_data[25] = 143;
485        assert_eq!(raw_packet.data(), &expected_data)
486    }
487
488    #[test]
489    fn test_link_statistics_packet_into_raw() {
490        let packet = Packet::LinkStatistics(LinkStatistics {
491            uplink_rssi_1: 16,
492            uplink_rssi_2: 19,
493            uplink_link_quality: 99,
494            uplink_snr: -105,
495            active_antenna: 1,
496            rf_mode: 2,
497            uplink_tx_power: 3,
498            downlink_rssi: 8,
499            downlink_link_quality: 88,
500            downlink_snr: -108,
501        });
502
503        let raw_packet = packet.into_raw(PacketAddress::Controller);
504        let expected_data = [0xc8, 12, 0x14, 16, 19, 99, 151, 1, 2, 3, 8, 88, 148, 252];
505        assert_eq!(raw_packet.data(), &expected_data)
506    }
507}
crsf/lib.rs

crsf/
lib.rs
