From 265e084d8e18d020591fda28231e9e2f6c3700d6 Mon Sep 17 00:00:00 2001 From: Alexander Koval Date: Wed, 24 Apr 2024 23:15:16 +0300 Subject: [PATCH] Optionally parse sync (address) byte --- Cargo.toml | 1 + examples/local_serial.rs | 10 +- src/buffer.rs | 40 +++ src/lib.rs | 547 +++++++++++++++++++++++++-------- src/packets/link_statistics.rs | 4 +- src/packets/rc_channels.rs | 4 +- 6 files changed, 474 insertions(+), 132 deletions(-) create mode 100644 src/buffer.rs diff --git a/Cargo.toml b/Cargo.toml index 5396d16..2cfc2cb 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,7 @@ repository = "https://github.com/tact1m4n3/crsf-rs" documentation = "https://docs.rs/crsf" [dependencies] +bitflags = "2.5.0" crc = "3.2" defmt = { version = "0.3.6", optional = true } num_enum = { version = "0.7.2", default-features = false } diff --git a/examples/local_serial.rs b/examples/local_serial.rs index 7c8bbe7..078738f 100644 --- a/examples/local_serial.rs +++ b/examples/local_serial.rs @@ -1,6 +1,6 @@ use std::{env, io, time::Duration}; -use crsf::{Packet, PacketReader}; +use crsf::{Packet, PacketPayload, PacketReader}; fn main() { let path = env::args().nth(1).expect("no serial port supplied"); @@ -10,7 +10,7 @@ fn main() { .expect("failed to open serial port"); let mut buf = [0; 1024]; - let mut reader = PacketReader::new(); + let mut reader = PacketReader::default(); loop { match port.read(buf.as_mut_slice()) { Ok(n) => { @@ -18,11 +18,11 @@ fn main() { let mut remaining = &buf[..n]; while let (Some(raw_packet), consumed) = reader.push_bytes(remaining) { match Packet::parse(raw_packet) { - Ok(packet) => match packet { - Packet::LinkStatistics(link_statistics) => { + Ok(packet) => match packet.payload { + PacketPayload::LinkStatistics(link_statistics) => { println!("{:?}", link_statistics); } - Packet::RcChannels(channels) => { + PacketPayload::RcChannels(channels) => { println!("{:?}", channels); } _ => {} diff --git a/src/buffer.rs b/src/buffer.rs new file mode 100644 index 0000000..6079ae1 --- /dev/null +++ b/src/buffer.rs @@ -0,0 +1,40 @@ +pub(crate) struct Buf { + buf: [u8; C], + len: usize, +} + +impl Buf { + pub(crate) const fn new() -> Self { + Self { + buf: [0; C], + len: 0, + } + } + + pub(crate) fn len(&self) -> usize { + self.len + } + + pub(crate) fn push(&mut self, c: u8) -> bool { + if let Some(v) = self.buf.get_mut(self.len) { + *v = c; + self.len += 1; + true + } else { + false + } + } + + pub(crate) fn push_bytes(&mut self, data: &[u8]) { + self.buf[self.len..self.len + data.len()].copy_from_slice(data); + self.len += data.len(); + } + + pub(crate) fn data(&self) -> &[u8; C] { + &self.buf + } + + pub(crate) fn clear(&mut self) { + self.len = 0; + } +} diff --git a/src/lib.rs b/src/lib.rs index 2c2d72a..e082166 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,23 +2,25 @@ //! # Usage //! ### Packet Parsing //! ```rust -//! use crsf::{PacketReader, PacketAddress, PacketType}; +//! use crsf::{PacketAddress, PacketReader, PacketType}; //! -//! let mut reader = PacketReader::new(); +//! let mut reader = PacketReader::default(); //! //! let addr = PacketAddress::FlightController; //! let typ = PacketType::RcChannelsPacked; //! ``` //! ### Packet Construction //! ```rust -//! use crsf::{Packet, RcChannels, PacketAddress}; +//! use crsf::{Packet, PacketAddress, PacketPayload, PACKET_MAX_LENGTH, RcChannels}; //! -//! let addr = PacketAddress::FlightController; //! let channels: [u16; 16] = [0xffff; 16]; -//! let packet = Packet::RcChannels(RcChannels(channels)); +//! let packet = Packet { +//! address: PacketAddress::FlightController, +//! payload: PacketPayload::RcChannels(RcChannels(channels)) +//! }; //! -//! let mut buf = [0u8; Packet::MAX_LENGTH]; -//! let len = packet.dump(&mut buf, addr); +//! let mut buf = [0u8; PACKET_MAX_LENGTH]; +//! let len = packet.dump(&mut buf); //! //! // ... //! ``` @@ -27,6 +29,7 @@ // TODO: top level crate packet reader examples redo +use bitflags::bitflags; use crc::{Crc, CRC_8_DVB_S2}; #[cfg(feature = "defmt")] use defmt; @@ -37,71 +40,131 @@ use snafu::prelude::*; #[link_section = ".data"] static CRC8: Crc = Crc::::new(&CRC_8_DVB_S2); -pub use packets::*; - +mod buffer; +use buffer::Buf; mod packets; +pub use packets::*; mod to_array; +/// Max crsf packet length +pub const PACKET_MAX_LENGTH: usize = 64; + /// Represents a packet reader -pub struct PacketReader { - buf: [u8; Packet::MAX_LENGTH], - state: ReadState, +pub struct PacketReader> { + buf: Buf, + initial_state: ReadState, + state: Option>, + address_decoder: A, +} + +impl PacketReader<(), NoPacketAddressDecoder> { + /// Creates a new PacketReader struct that does not expect a sync byte. + pub fn without_sync() -> PacketReader<(), NoPacketAddressDecoder> { + let initial_state = ReadState::WaitingForLen { addr: () }; + PacketReader::<(), NoPacketAddressDecoder> { + buf: Buf::new(), + initial_state, + state: Some(initial_state), + address_decoder: NoPacketAddressDecoder, + } + } } -impl PacketReader { +impl Default for PacketReader { + /// Creates a new PacketReader struct that expects default sync byte (0xC8) + /// as a packet marker + fn default() -> PacketReader { + let initial_state = ReadState::WaitingForSync; + PacketReader { + buf: Buf::new(), + initial_state, + state: Some(initial_state), + address_decoder: KnownPacketAddressDecoder::new(&[PacketAddress::FlightController]), + } + } +} + +impl PacketReader { + /// Creates a new PacketReader struct with custom addresses + pub fn with_addresses( + addresses: &[PacketAddress], + ) -> PacketReader { + let initial_state = ReadState::WaitingForSync; + PacketReader { + buf: Buf::new(), + initial_state, + state: Some(initial_state), + address_decoder: KnownPacketAddressDecoder::new(addresses), + } + } +} + +impl> PacketReader { // Packet type and checksum bytes are mandatory const MIN_DATA_LENGTH: u8 = 2; // Number of bytes of packet type, payload and checksum - const MAX_DATA_LENGTH: u8 = Packet::MAX_LENGTH as u8 - Self::MIN_DATA_LENGTH; - - /// Creates a new PacketReader struct - pub const fn new() -> Self { - Self { - buf: [0; Packet::MAX_LENGTH], - state: ReadState::WaitingForSync, - } + const MAX_DATA_LENGTH: u8 = PACKET_MAX_LENGTH as u8 - Self::MIN_DATA_LENGTH; + + /// Resets reader + /// + /// Useful in situations when timeout is triggered and a packet is not parsed + pub fn reset(&mut self) { + self.state = Some(self.initial_state); + self.buf.clear(); } /// Reads the first packet from the buffer - pub fn push_bytes(&mut self, bytes: &[u8]) -> (Option, usize) { + pub fn push_bytes(&mut self, bytes: &[u8]) -> (Option>, usize) { let mut reader = BytesReader::new(bytes); let packet = loop { match self.state { - ReadState::WaitingForSync => { + None => { + // We cannot clear buffer when constructing `RawPacket` as `RawPacket` borrows it. + // Owned version of `RawPacket` should solve this. + self.reset(); + } + Some(ReadState::WaitingForSync) => { while let Some(addr_byte) = reader.next() { - if PacketAddress::try_from(addr_byte).is_ok() { - self.buf[0] = addr_byte; - self.state = ReadState::WaitingForLen; + if let Some(addr) = self.address_decoder.decode(addr_byte) { + self.buf.push(addr_byte); + self.state = Some(ReadState::WaitingForLen { addr }); break; } } - continue; + if reader.is_empty() { + break None; + } else { + continue; + } } - ReadState::WaitingForLen => { + Some(ReadState::WaitingForLen { addr }) => { if let Some(len_byte) = reader.next() { match len_byte { Self::MIN_DATA_LENGTH..=Self::MAX_DATA_LENGTH => { - self.buf[1] = len_byte; - self.state = ReadState::Reading { - idx: Packet::HEADER_LENGTH, - len: Packet::HEADER_LENGTH + len_byte as usize, - }; + self.buf.push(len_byte); + self.state = Some(ReadState::Reading { + addr, + len: addr.header_len() + len_byte as usize, + }); + } + _ => { + self.state = Some(ReadState::WaitingForSync); + self.buf.clear(); } - _ => self.state = ReadState::WaitingForSync, } continue; } } - ReadState::Reading { ref mut idx, len } => { - let data = reader.next_n(len - *idx); - self.buf[*idx..*idx + data.len()].copy_from_slice(data); - *idx += data.len(); - if *idx >= len { - self.state = ReadState::WaitingForSync; + Some(ReadState::Reading { addr, len }) => { + let data = reader.next_n(len - self.buf.len()); + self.buf.push_bytes(data); + if self.buf.len() >= len { + self.state = None; break Some( RawPacket { - buf: &self.buf, - len, + address: addr, + buf: self.buf.data(), + len: self.buf.len(), } ); } @@ -115,20 +178,15 @@ impl PacketReader { } } -impl Default for PacketReader { - fn default() -> Self { - Self::new() - } -} - /// Represents a raw packet (not parsed) #[derive(Clone, Copy, Debug)] -pub struct RawPacket<'a> { - buf: &'a [u8; Packet::MAX_LENGTH], +pub struct RawPacket<'a, T> { + address: T, + buf: &'a [u8; PACKET_MAX_LENGTH], len: usize, } -impl<'a> RawPacket<'a> { +impl<'a, T> RawPacket<'a, T> { /// Returns the packet data as a slice pub fn as_slice(&self) -> &[u8] { &self.buf[..self.len] @@ -138,29 +196,49 @@ impl<'a> RawPacket<'a> { } /// Represents a parsed packet +#[derive(Clone, Debug)] +pub struct Packet { + pub address: T, + pub payload: PacketPayload, +} + #[non_exhaustive] #[derive(Clone, Debug)] -pub enum Packet { +pub enum PacketPayload { LinkStatistics(LinkStatistics), RcChannels(RcChannels), } -impl Packet { - /// Max crsf packet length - pub const MAX_LENGTH: usize = 64; - /// Crsf packet header length - pub const HEADER_LENGTH: usize = 2; +impl Packet<()> { + pub fn without_address(payload: PacketPayload) -> Self { + Self { + address: (), + payload, + } + } +} + +impl Packet { + pub fn new(address: PacketAddress, payload: PacketPayload) -> Self { + Self { + address, + payload, + } + } +} +impl Packet { /// Parses a raw packet (returned by the packet reader) - pub fn parse(raw_packet: RawPacket) -> Result { + pub fn parse(raw_packet: RawPacket) -> Result { // TODO: use more constants instead of literals // not using raw_packet.as_slice() for the compiler to remove out of bounds checking let buf = raw_packet.buf; let len = raw_packet.len; + let header_len = raw_packet.address.header_len(); let checksum_idx = len - 1; - let checksum = CRC8.checksum(&buf[2..checksum_idx]); + let checksum = CRC8.checksum(&buf[header_len..checksum_idx]); if checksum != buf[checksum_idx] { return Err(ParseError::ChecksumMismatch { expected: checksum, @@ -168,40 +246,45 @@ impl Packet { }); } - let typ_byte = buf[2]; + let typ_byte = buf[header_len]; if let Ok(typ) = PacketType::try_from(typ_byte) { let payload_data = if typ.is_extended() { - &buf[5..len] + &buf[header_len + 3..len - 1] } else { - &buf[3..len] + &buf[header_len + 1..len - 1] }; - match typ { + let payload = match typ { PacketType::RcChannelsPacked => { - Ok(Packet::RcChannels(RcChannels::parse(payload_data))) + PacketPayload::RcChannels(RcChannels::parse(payload_data)) } PacketType::LinkStatistics => { - Ok(Packet::LinkStatistics(LinkStatistics::parse(payload_data))) + PacketPayload::LinkStatistics(LinkStatistics::parse(payload_data)) } - _ => Err(ParseError::UnknownType { typ: typ_byte }), - } + _ => return Err(ParseError::UnknownType { typ: typ_byte }), + }; + Ok(Packet { + address: raw_packet.address, + payload, + }) } else { Err(ParseError::InvalidType { typ: typ_byte }) } } /// Dumps the packet into a buffer - pub fn dump(&self, buf: &mut [u8], addr: PacketAddress) -> Result { + pub fn dump(&self, buf: &mut [u8]) -> Result { // TODO: use more constants instead of literals - let payload = match self { - Packet::LinkStatistics(payload) => payload as &dyn Payload, - Packet::RcChannels(payload) => payload as &dyn Payload, + let payload = match &self.payload { + PacketPayload::LinkStatistics(payload) => payload as &dyn Payload, + PacketPayload::RcChannels(payload) => payload as &dyn Payload, }; let typ = payload.packet_type(); let len_byte = payload.len() + if typ.is_extended() { 4 } else { 2 }; - let len = Packet::HEADER_LENGTH + len_byte as usize; + let header_len = self.address.header_len(); + let len = header_len + len_byte as usize; if buf.len() < len { return Err(BufferLenError { @@ -210,14 +293,20 @@ impl Packet { }); } - buf[0] = addr as u8; - buf[1] = len_byte; - buf[2] = typ as u8; + let mut ix = 0; + if let Some(addr) = self.address.encode() { + buf[ix] = addr; + ix += 1; + } + buf[ix] = len_byte; + ix += 1; + buf[ix] = typ as u8; + ix += 1; - let payload_start = if typ.is_extended() { 5 } else { 3 }; + let payload_start = if typ.is_extended() { ix + 2 } else { ix }; let checksum_idx = len - 1; payload.dump(&mut buf[payload_start..checksum_idx]); - buf[checksum_idx] = CRC8.checksum(&buf[2..checksum_idx]); + buf[checksum_idx] = CRC8.checksum(&buf[header_len..checksum_idx]); Ok(len) } @@ -267,6 +356,105 @@ pub enum PacketAddress { Transmitter = 0xEE, } +pub trait PacketAddressEncoder { + fn header_len(&self) -> usize { + if self.encode().is_some() { + 2 + } else { + 1 + } + } + + fn encode(&self) -> Option; +} + +impl PacketAddressEncoder for () { + fn encode(&self) -> Option { + None + } +} + +impl PacketAddressEncoder for PacketAddress { + fn encode(&self) -> Option { + Some(*self as u8) + } +} + +pub trait PacketAddressDecoder { + fn decode(&self, c: u8) -> Option; +} + +pub struct NoPacketAddressDecoder; + +impl PacketAddressDecoder<()> for NoPacketAddressDecoder { + fn decode(&self, _: u8) -> Option<()> { + Some(()) + } +} + +pub struct KnownPacketAddressDecoder { + flags: PacketAddressFlags, +} + +impl KnownPacketAddressDecoder { + fn new(addresses: &[PacketAddress]) -> Self { + let flags = addresses.iter() + .map(|addr| PacketAddressFlags::from_address(*addr)) + .reduce(|flags, f| flags | f) + .expect("non-empty packet addresses"); + Self { flags } + } +} + +bitflags! { + struct PacketAddressFlags: u16 { + const BROADCAST = 1; + const USB = 1 << 1; + const BLUETOOTH = 1 << 2; + const TBS_CORE_PNP_PRO = 1 << 3; + const RESERVED1 = 1 << 4; + const CURRENT_SENSOR = 1 << 5; + const GPS = 1 << 6; + const TBS_BLACKBOX = 1 << 7; + const FLIGHT_CONTROLLER = 1 << 8; + const RESERVED2 = 1 << 9; + const RACE_TAG = 1 << 10; + const HANDSET = 1 << 11; + const RECEIVER = 1 << 12; + const TRANSMITTER = 1 << 13; + } +} + +impl PacketAddressFlags { + fn from_address(address: PacketAddress) -> Self { + use PacketAddress::*; + + match address { + Broadcast => PacketAddressFlags::BROADCAST, + Usb => PacketAddressFlags::USB, + Bluetooth => PacketAddressFlags::BLUETOOTH, + TbsCorePnpPro => PacketAddressFlags::TBS_CORE_PNP_PRO, + Reserved1 => PacketAddressFlags::RESERVED1, + CurrentSensor => PacketAddressFlags::CURRENT_SENSOR, + Gps => PacketAddressFlags::GPS, + TbsBlackbox => PacketAddressFlags::TBS_BLACKBOX, + FlightController => PacketAddressFlags::FLIGHT_CONTROLLER, + Reserved2 => PacketAddressFlags::RESERVED2, + RaceTag => PacketAddressFlags::RACE_TAG, + Handset => PacketAddressFlags::HANDSET, + Receiver => PacketAddressFlags::RECEIVER, + Transmitter => PacketAddressFlags::TRANSMITTER, + } + } +} + +impl PacketAddressDecoder for KnownPacketAddressDecoder { + fn decode(&self, c: u8) -> Option { + PacketAddress::try_from(c).ok() + .filter(|&addr| self.flags.contains(PacketAddressFlags::from_address(addr))) + } +} + /// Represents all CRSF packet types #[non_exhaustive] #[derive(Clone, Copy, Debug, PartialEq, Eq, TryFromPrimitive)] @@ -316,10 +504,11 @@ impl PacketType { /// +-------------------+ | /// +------------------------------------+ /// -enum ReadState { +#[derive(Clone, Copy, Debug)] +enum ReadState { WaitingForSync, - WaitingForLen, - Reading { idx: usize, len: usize }, + WaitingForLen { addr: T }, + Reading { addr: T, len: usize }, } struct BytesReader<'a> { @@ -336,6 +525,10 @@ impl<'a> BytesReader<'a> { self.idx } + fn is_empty(&self) -> bool { + self.idx == self.buf.len() + } + fn next(&mut self) -> Option { if self.idx < self.buf.len() { let val = self.buf[self.idx]; @@ -359,13 +552,14 @@ mod tests { use crate::BufferLenError; use crate::BytesReader; use crate::LinkStatistics; + use crate::PACKET_MAX_LENGTH; use crate::Packet; use crate::PacketAddress; - - use super::PacketReader; - use super::PacketType; - use super::ParseError; - use super::RcChannels; + use crate::PacketPayload; + use crate::PacketReader; + use crate::PacketType; + use crate::ParseError; + use crate::RcChannels; #[test] fn test_bytes_reader() { @@ -380,8 +574,8 @@ mod tests { } #[test] - fn test_parse_next_packet() { - let mut reader = PacketReader::new(); + fn test_packet_reader_parse_packet_parts() { + let mut reader = PacketReader::default(); let addr = PacketAddress::FlightController; let typ = PacketType::RcChannelsPacked; @@ -397,13 +591,16 @@ mod tests { // Checksum assert!(matches!( reader.push_bytes(&[239]).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), - Ok(Packet::RcChannels(RcChannels(channels))) if channels == [0; 16] + Ok(Packet { + address: PacketAddress::FlightController, + payload: PacketPayload::RcChannels(RcChannels(channels)), + }) if channels == [0; 16] )); } #[test] - fn test_parse_full_packet() { - let mut reader = PacketReader::new(); + fn test_packet_reader_parse_packet_full() { + let mut reader = PacketReader::default(); let addr = PacketAddress::FlightController; let typ = PacketType::RcChannelsPacked; @@ -422,12 +619,91 @@ mod tests { ]; assert!(matches!( reader.push_bytes(&data).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), - Ok(Packet::RcChannels(RcChannels(channels))) if channels == [0; 16] + Ok(Packet { + address: PacketAddress::FlightController, + payload: PacketPayload::RcChannels(RcChannels(channels)), + }) if channels == [0; 16] + )); + } + + #[test] + fn test_packet_reader_waiting_for_sync() { + let mut reader = PacketReader::with_addresses( + &[PacketAddress::Transmitter, PacketAddress::Handset] + ); + let typ = PacketType::RcChannelsPacked; + + // Garbage + assert!(reader.push_bytes(&[0, 1, 2, 3]).0.is_none()); + // More garbage + assert!(reader.push_bytes(&[PacketAddress::FlightController as u8, PacketAddress::Receiver as u8]).0.is_none()); + // Sync + assert!(reader.push_bytes(&[PacketAddress::Handset as u8]).0.is_none()); + // Len + assert!(reader.push_bytes(&[24]).0.is_none()); + // Type + assert!(reader.push_bytes(&[typ as u8]).0.is_none()); + // Payload + assert!(reader.push_bytes(&[0; 22]).0.is_none()); + // Checksum + assert!(matches!( + reader.push_bytes(&[239]).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), + Ok(Packet { + address: PacketAddress::Handset, + payload: PacketPayload::RcChannels(RcChannels(channels)), + }) if channels == [0; 16] + )); + + // Garbage again + assert!(reader.push_bytes(&[42, 34]).0.is_none()); + // Sync + assert!(reader.push_bytes(&[PacketAddress::Transmitter as u8]).0.is_none()); + // Len + assert!(reader.push_bytes(&[24]).0.is_none()); + // Type + assert!(reader.push_bytes(&[typ as u8]).0.is_none()); + // Payload + assert!(reader.push_bytes(&[0; 22]).0.is_none()); + // Checksum + assert!(matches!( + reader.push_bytes(&[239]).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), + Ok(Packet { + address: PacketAddress::Transmitter, + payload: PacketPayload::RcChannels(RcChannels(channels)), + }) if channels == [0; 16] )); } + + #[test] + fn test_packet_reader_parse_packet_without_sync_byte() { + let mut reader = PacketReader::without_sync(); + + let typ = PacketType::RcChannelsPacked; + + let data = [ + // Len + 24, + // Type + typ as u8, + // Payload + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + // Checksum + 239, + ]; + let res = reader.push_bytes(&data).0.map(|raw_packet| Packet::parse(raw_packet)); + assert!(matches!( + res.expect("packet expected"), + // reader.push_bytes(&data).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), + Ok(Packet { + address: (), + payload: PacketPayload::RcChannels(RcChannels(channels)), + }) if channels == [0; 16] + )); + } + #[test] fn test_parse_next_packet_with_validation_error() { - let mut reader = PacketReader::new(); + let mut reader = PacketReader::default(); let addr = PacketAddress::FlightController; @@ -455,22 +731,25 @@ mod tests { #[test] fn test_packet_dump_in_small_buffer() { - let packet = Packet::LinkStatistics(LinkStatistics { - uplink_rssi_1: 16, - uplink_rssi_2: 19, - uplink_link_quality: 99, - uplink_snr: -105, - active_antenna: 1, - rf_mode: 2, - uplink_tx_power: 3, - downlink_rssi: 8, - downlink_link_quality: 88, - downlink_snr: -108, - }); + let packet = Packet { + address: PacketAddress::FlightController, + payload: PacketPayload::LinkStatistics(LinkStatistics { + uplink_rssi_1: 16, + uplink_rssi_2: 19, + uplink_link_quality: 99, + uplink_snr: -105, + active_antenna: 1, + rf_mode: 2, + uplink_tx_power: 3, + downlink_rssi: 8, + downlink_link_quality: 88, + downlink_snr: -108, + }), + }; let mut buf = [0u8; 10]; assert_eq!( - packet.dump(&mut buf, PacketAddress::FlightController), + packet.dump(&mut buf), Err(BufferLenError { expected: 14, actual: 10 @@ -481,10 +760,13 @@ mod tests { #[test] fn test_rc_channels_packet_dump() { let channels: [u16; 16] = [0x7FF; 16]; - let packet = Packet::RcChannels(RcChannels(channels)); + let packet = Packet::new( + PacketAddress::Transmitter, + PacketPayload::RcChannels(RcChannels(channels)), + ); - let mut buf = [0u8; Packet::MAX_LENGTH]; - let len = packet.dump(&mut buf, PacketAddress::Transmitter).unwrap(); + let mut buf = [0u8; PACKET_MAX_LENGTH]; + let len = packet.dump(&mut buf).unwrap(); let mut expected_data: [u8; 26] = [0xff; 26]; expected_data[0] = 0xee; expected_data[1] = 24; @@ -493,23 +775,42 @@ mod tests { assert_eq!(&buf[..len], &expected_data) } + #[test] + fn test_rc_channels_packet_dump_no_address() { + let channels: [u16; 16] = [0xffff; 16]; + let packet = Packet::without_address( + PacketPayload::RcChannels(RcChannels(channels)), + ); + + let mut buf = [0u8; PACKET_MAX_LENGTH]; + let len = packet.dump(&mut buf).unwrap(); + let mut expected_data: [u8; 25] = [0xff; 25]; + expected_data[0] = 24; + expected_data[1] = 0x16; + expected_data[24] = 143; + assert_eq!(&buf[..len], &expected_data) + } + #[test] fn test_link_statistics_packet_dump() { - let packet = Packet::LinkStatistics(LinkStatistics { - uplink_rssi_1: 16, - uplink_rssi_2: 19, - uplink_link_quality: 99, - uplink_snr: -105, - active_antenna: 1, - rf_mode: 2, - uplink_tx_power: 3, - downlink_rssi: 8, - downlink_link_quality: 88, - downlink_snr: -108, - }); - - let mut buf = [0u8; Packet::MAX_LENGTH]; - let len = packet.dump(&mut buf, PacketAddress::FlightController).unwrap(); + let packet = Packet::new( + PacketAddress::FlightController, + PacketPayload::LinkStatistics(LinkStatistics { + uplink_rssi_1: 16, + uplink_rssi_2: 19, + uplink_link_quality: 99, + uplink_snr: -105, + active_antenna: 1, + rf_mode: 2, + uplink_tx_power: 3, + downlink_rssi: 8, + downlink_link_quality: 88, + downlink_snr: -108, + }) + ); + + let mut buf = [0u8; PACKET_MAX_LENGTH]; + let len = packet.dump(&mut buf).unwrap(); let expected_data = [0xc8, 12, 0x14, 16, 19, 99, 151, 1, 2, 3, 8, 88, 148, 252]; assert_eq!(&buf[..len], &expected_data) } diff --git a/src/packets/link_statistics.rs b/src/packets/link_statistics.rs index 8f7d8f1..0bec7b9 100644 --- a/src/packets/link_statistics.rs +++ b/src/packets/link_statistics.rs @@ -55,7 +55,7 @@ impl Payload for LinkStatistics { #[cfg(test)] mod tests { - use crate::{LinkStatistics, Packet}; + use crate::{LinkStatistics, PACKET_MAX_LENGTH}; use crate::packets::Payload; #[test] @@ -73,7 +73,7 @@ mod tests { downlink_snr: -68, }; - let mut data = [0u8; Packet::MAX_LENGTH]; + let mut data = [0u8; PACKET_MAX_LENGTH]; original.dump(&mut data); assert_eq!(data[0], 100_u8.to_le()); diff --git a/src/packets/rc_channels.rs b/src/packets/rc_channels.rs index ba928aa..f947860 100644 --- a/src/packets/rc_channels.rs +++ b/src/packets/rc_channels.rs @@ -121,7 +121,7 @@ impl core::ops::DerefMut for RcChannels { #[cfg(test)] mod tests { use crate::packets::Payload; - use crate::{Packet, RcChannels}; + use crate::{RcChannels, PACKET_MAX_LENGTH}; #[test] fn test_rc_channels_write_and_parse() { @@ -130,7 +130,7 @@ mod tests { original[i] = i as u16 * 10; } - let mut data = [0u8; Packet::MAX_LENGTH]; + let mut data = [0u8; PACKET_MAX_LENGTH]; original.dump(&mut data); let parsed = RcChannels::parse(&data);