Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Make address as a part of a packet #25

Merged
merged 1 commit into from
Apr 26, 2024
Merged

Make address as a part of a packet #25

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions examples/local_serial.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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");
Expand All @@ -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);
}
_ => {}
Expand Down
176 changes: 116 additions & 60 deletions src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,40 @@
//! # Usage
//! ### Packet Parsing
//! ```rust
//! use crsf::{PacketReader, PacketAddress, PacketType};
//! use crsf::{Packet, PacketReader, PacketAddress, PacketType};
//!
//! let mut reader = PacketReader::new();
//! let data: &[&[u8]] = &[&[0xc8, 24, 0x16], &[0; 22], &[239]];
//! for input_buf in data {
//! let mut buf: &[u8] = input_buf;
//! while !buf.is_empty() {
//! let consumed = match reader.push_bytes(buf) {
//! (Some(raw_packet), n) => {
//! let packet = Packet::parse(raw_packet).expect("valid packet");
//! n
//! }
//! (None, n) => n,
//! };
//! buf = &buf[consumed..];
//! }
//! }
//!
//! let addr = PacketAddress::FlightController;
//! let typ = PacketType::RcChannelsPacked;
//! ```
//! ### Packet Construction
//! ```rust
//! use crsf::{Packet, RcChannels, PacketAddress};
//! use crsf::{Packet, PacketAddress, PacketPayload, RcChannels};
//!
//! let addr = PacketAddress::FlightController;
//! let channels: [u16; 16] = [0xffff; 16];
//! let packet = Packet::RcChannels(RcChannels(channels));
//! let packet = Packet::new(
//! PacketAddress::FlightController,
//! PacketPayload::RcChannels(RcChannels(channels))
//! );
//!
//! let mut buf = [0u8; Packet::MAX_LENGTH];
//! let len = packet.dump(&mut buf, addr);
//! let packet_len = packet.dump(&mut buf).expect("dumped packet");
//! let packet_data = &buf[..packet_len];
//!
//! // ...
//! ```
Expand Down Expand Up @@ -79,10 +96,10 @@ impl PacketReader {
match self.state {
ReadState::WaitingForSync => {
while let Some(addr_byte) = reader.next() {
if PacketAddress::try_from(addr_byte).is_ok() {
if let Ok(addr) = PacketAddress::try_from(addr_byte) {
self.buf.clear();
self.buf.push(addr_byte);
self.state = ReadState::WaitingForLen;
self.state = ReadState::WaitingForLen { addr };
break;
}
}
Expand All @@ -92,12 +109,13 @@ impl PacketReader {
continue;
}
}
ReadState::WaitingForLen => {
ReadState::WaitingForLen { addr } => {
if let Some(len_byte) = reader.next() {
match len_byte {
Self::MIN_DATA_LENGTH..=Self::MAX_DATA_LENGTH => {
self.buf.push(len_byte);
self.state = ReadState::Reading {
addr,
len: Packet::HEADER_LENGTH + len_byte as usize,
};
}
Expand All @@ -106,13 +124,14 @@ impl PacketReader {
continue;
}
}
ReadState::Reading { len } => {
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 = ReadState::WaitingForSync;
break Some(
RawPacket {
addr,
buf: self.buf.data(),
len: self.buf.len(),
}
Expand All @@ -137,6 +156,7 @@ impl Default for PacketReader {
/// Represents a raw packet (not parsed)
#[derive(Clone, Copy, Debug)]
pub struct RawPacket<'a> {
addr: PacketAddress,
buf: &'a [u8; Packet::MAX_LENGTH],
len: usize,
}
Expand All @@ -147,23 +167,36 @@ impl<'a> RawPacket<'a> {
&self.buf[..self.len]
}

// TODO: maybe add methods for getting the addr etc
pub fn addr(&self) -> PacketAddress {
self.addr
}
}

/// Represents a parsed packet
/// Represents a packet payload data
#[non_exhaustive]
#[derive(Clone, Debug)]
pub enum Packet {
pub enum PacketPayload {
LinkStatistics(LinkStatistics),
RcChannels(RcChannels),
}

/// Represents a parsed packet
pub struct Packet {
pub addr: PacketAddress,
pub payload: PacketPayload,
}

impl Packet {
/// Max crsf packet length
pub const MAX_LENGTH: usize = 64;
/// Crsf packet header length
pub const HEADER_LENGTH: usize = 2;

/// Creates new packet with address and payload
pub fn new(addr: PacketAddress, payload: PacketPayload) -> Self {
Self { addr, payload }
}

/// Parses a raw packet (returned by the packet reader)
pub fn parse(raw_packet: RawPacket) -> Result<Packet, ParseError> {
// TODO: use more constants instead of literals
Expand All @@ -189,27 +222,31 @@ impl Packet {
&buf[3..len]
};

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 {
addr: raw_packet.addr,
payload,
})
} else {
Err(ParseError::InvalidType { typ: typ_byte })
}
}

/// Dumps the packet into a buffer
pub fn dump(&self, buf: &mut [u8], addr: PacketAddress) -> Result<usize, BufferLenError> {
pub fn dump(&self, buf: &mut [u8]) -> Result<usize, BufferLenError> {
// 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();
Expand All @@ -223,7 +260,7 @@ impl Packet {
});
}

buf[0] = addr as u8;
buf[0] = self.addr as u8;
buf[1] = len_byte;
buf[2] = typ as u8;

Expand Down Expand Up @@ -331,8 +368,8 @@ impl PacketType {
///
enum ReadState {
WaitingForSync,
WaitingForLen,
Reading { len: usize },
WaitingForLen { addr: PacketAddress },
Reading { addr: PacketAddress, len: usize },
}

#[cfg(test)]
Expand All @@ -342,11 +379,11 @@ mod tests {
use crate::LinkStatistics;
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() {
Expand Down Expand Up @@ -381,7 +418,10 @@ 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 {
addr: PacketAddress::Handset,
payload: PacketPayload::RcChannels(RcChannels(channels))
}) if channels == [0; 16]
));
}
}
Expand All @@ -404,7 +444,10 @@ 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 {
addr: PacketAddress::FlightController,
payload: PacketPayload::RcChannels(RcChannels(channels))
}) if channels == [0; 16]
));
}

Expand All @@ -429,9 +472,13 @@ 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 {
addr: PacketAddress::FlightController,
payload: PacketPayload::RcChannels(RcChannels(channels))
}) if channels == [0; 16]
));
}

#[test]
fn test_parse_next_packet_with_validation_error() {
let mut reader = PacketReader::new();
Expand Down Expand Up @@ -462,22 +509,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::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; 10];
assert_eq!(
packet.dump(&mut buf, PacketAddress::FlightController),
packet.dump(&mut buf),
Err(BufferLenError {
expected: 14,
actual: 10
Expand All @@ -488,10 +538,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 len = packet.dump(&mut buf).unwrap();
let mut expected_data: [u8; 26] = [0xff; 26];
expected_data[0] = 0xee;
expected_data[1] = 24;
Expand All @@ -502,21 +555,24 @@ mod tests {

#[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 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, PacketAddress::FlightController).unwrap();
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)
}
Expand Down