diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 17940867b..f5e7d34ca 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -1,7 +1,14 @@ [package] name = "mavlink-core" version = "0.12.2" -authors = ["Todd Stellanova", "Michal Podhradsky", "Kevin Mehall", "Tim Ryan", "Patrick José Pereira", "Ibiyemi Abiodun"] +authors = [ + "Todd Stellanova", + "Michal Podhradsky", + "Kevin Mehall", + "Tim Ryan", + "Patrick José Pereira", + "Ibiyemi Abiodun", +] description = "Implements the MAVLink data interchange format for UAVs." readme = "README.md" license = "MIT/Apache-2.0" diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index c8d226e13..341d075ef 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -1,10 +1,12 @@ use crate::connection::MavConnection; +use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; use std::io; use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; -use serial::prelude::*; +use serial::{prelude::*, SystemPort}; /// Serial MAVLINK connection @@ -40,14 +42,14 @@ pub fn open(settings: &str) -> io::Result { port.configure(&settings)?; Ok(SerialConnection { - port: Mutex::new(port), + port: Mutex::new(PeekReader::new(port)), sequence: Mutex::new(0), protocol_version: MavlinkVersion::V2, }) } pub struct SerialConnection { - port: Mutex, + port: Mutex>, sequence: Mutex, protocol_version: MavlinkVersion, } @@ -55,9 +57,8 @@ pub struct SerialConnection { impl MavConnection for SerialConnection { fn recv(&self) -> Result<(MavHeader, M), MessageReadError> { let mut port = self.port.lock().unwrap(); - loop { - match read_versioned_msg(&mut *port, self.protocol_version) { + match read_versioned_msg(port.deref_mut(), self.protocol_version) { ok @ Ok(..) => { return ok; } @@ -83,7 +84,7 @@ impl MavConnection for SerialConnection { *sequence = sequence.wrapping_add(1); - write_versioned_msg(&mut *port, self.protocol_version, header, data) + write_versioned_msg(port.reader_mut(), self.protocol_version, header, data) } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index 5972c4446..d0fcdc987 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,8 +1,10 @@ use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; +use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message}; +use core::ops::DerefMut; use std::fs::File; -use std::io::{self}; +use std::io; use std::sync::Mutex; /// File MAVLINK connection @@ -11,13 +13,13 @@ pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; Ok(FileConnection { - file: Mutex::new(file), + file: Mutex::new(PeekReader::new(file)), protocol_version: MavlinkVersion::V2, }) } pub struct FileConnection { - file: Mutex, + file: Mutex>, protocol_version: MavlinkVersion, } @@ -28,7 +30,7 @@ impl MavConnection for FileConnection { let mut file = self.file.lock().unwrap(); loop { - match read_versioned_msg(&mut *file, self.protocol_version) { + match read_versioned_msg(file.deref_mut(), self.protocol_version) { ok @ Ok(..) => { return ok; } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index b34eb5f09..44d377877 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -1,6 +1,8 @@ use crate::connection::MavConnection; +use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::{self}; +use core::ops::DerefMut; +use std::io; use std::net::ToSocketAddrs; use std::net::{TcpListener, TcpStream}; use std::sync::Mutex; @@ -34,7 +36,7 @@ pub fn tcpout(address: T) -> io::Result { socket.set_read_timeout(Some(Duration::from_millis(100)))?; Ok(TcpConnection { - reader: Mutex::new(socket.try_clone()?), + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -52,7 +54,7 @@ pub fn tcpin(address: T) -> io::Result { match incoming { Ok(socket) => { return Ok(TcpConnection { - reader: Mutex::new(socket.try_clone()?), + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -73,7 +75,7 @@ pub fn tcpin(address: T) -> io::Result { } pub struct TcpConnection { - reader: Mutex, + reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, } @@ -85,8 +87,8 @@ struct TcpWrite { impl MavConnection for TcpConnection { fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { - let mut lock = self.reader.lock().expect("tcp read failure"); - read_versioned_msg(&mut *lock, self.protocol_version) + let mut reader = self.reader.lock().unwrap(); + read_versioned_msg(reader.deref_mut(), self.protocol_version) } fn send(&self, header: &MavHeader, data: &M) -> Result { diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 1522ed20d..c0db512da 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,7 +1,8 @@ use crate::connection::MavConnection; +use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::Read; -use std::io::{self}; +use core::ops::DerefMut; +use std::io::{self, Read}; use std::net::ToSocketAddrs; use std::net::{SocketAddr, UdpSocket}; use std::sync::Mutex; @@ -31,7 +32,7 @@ pub fn select_protocol( pub fn udpbcast(address: T) -> io::Result { let addr = get_socket_addr(address)?; - let socket = UdpSocket::bind("0.0.0.0:0")?; + let socket = UdpSocket::bind("0.0.0.0:0").unwrap(); socket .set_broadcast(true) .expect("Couldn't bind to broadcast address."); @@ -45,67 +46,37 @@ pub fn udpout(address: T) -> io::Result { } pub fn udpin(address: T) -> io::Result { - let addr = get_socket_addr(address)?; + let addr = address + .to_socket_addrs() + .unwrap() + .next() + .expect("Invalid address"); let socket = UdpSocket::bind(addr)?; UdpConnection::new(socket, true, None) } -struct UdpWrite { +struct UdpRead { socket: UdpSocket, - dest: Option, - sequence: u8, -} - -struct PacketBuf { - buf: Vec, - start: usize, - end: usize, -} - -impl PacketBuf { - pub fn new() -> Self { - let v = vec![0; 65536]; - Self { - buf: v, - start: 0, - end: 0, - } - } - - pub fn reset(&mut self) -> &mut [u8] { - self.start = 0; - self.end = 0; - &mut self.buf - } - - pub fn set_len(&mut self, size: usize) { - self.end = size; - } - - pub fn slice(&self) -> &[u8] { - &self.buf[self.start..self.end] - } - - pub fn len(&self) -> usize { - self.slice().len() - } + last_recv_address: Option, } -impl Read for PacketBuf { +impl Read for UdpRead { fn read(&mut self, buf: &mut [u8]) -> io::Result { - let n = Read::read(&mut self.slice(), buf)?; - self.start += n; - Ok(n) + self.socket.recv_from(buf).map(|(n, addr)| { + self.last_recv_address = Some(addr); + n + }) } } -struct UdpRead { +struct UdpWrite { socket: UdpSocket, - recv_buf: PacketBuf, + dest: Option, + sequence: u8, } pub struct UdpConnection { - reader: Mutex, + reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, server: bool, @@ -115,10 +86,10 @@ impl UdpConnection { fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { Ok(Self { server, - reader: Mutex::new(UdpRead { + reader: Mutex::new(PeekReader::new(UdpRead { socket: socket.try_clone()?, - recv_buf: PacketBuf::new(), - }), + last_recv_address: None, + })), writer: Mutex::new(UdpWrite { socket, dest, @@ -131,19 +102,16 @@ impl UdpConnection { impl MavConnection for UdpConnection { fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { - let mut guard = self.reader.lock().unwrap(); - let state = &mut *guard; - loop { - if state.recv_buf.len() == 0 { - let (len, src) = state.socket.recv_from(state.recv_buf.reset())?; - state.recv_buf.set_len(len); + let mut reader = self.reader.lock().unwrap(); - if self.server { - self.writer.lock().unwrap().dest = Some(src); + loop { + let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + if self.server { + if let addr @ Some(_) = reader.reader_ref().last_recv_address { + self.writer.lock().unwrap().dest = addr; } } - - if let ok @ Ok(..) = read_versioned_msg(&mut state.recv_buf, self.protocol_version) { + if let ok @ Ok(..) = result { return ok; } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index c5f86e4ac..2bcfc6859 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -27,9 +27,6 @@ use core::result::Result; #[cfg(feature = "std")] use std::io::{Read, Write}; -#[cfg(feature = "std")] -use byteorder::ReadBytesExt; - pub mod utils; #[allow(unused_imports)] use utils::{remove_trailing_zeroes, RustDefault}; @@ -37,6 +34,11 @@ use utils::{remove_trailing_zeroes, RustDefault}; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +#[cfg(feature = "std")] +pub mod peek_reader; +#[cfg(feature = "std")] +use peek_reader::PeekReader; + use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; @@ -215,7 +217,7 @@ fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { } pub fn read_versioned_msg( - r: &mut R, + r: &mut PeekReader, version: MavlinkVersion, ) -> Result<(MavHeader, M), error::MessageReadError> { match version { @@ -362,52 +364,61 @@ impl MAVLinkV1MessageRaw { /// Return a raw buffer with the mavlink message /// V1 maximum size is 263 bytes: `` -pub fn read_v1_raw_message( - reader: &mut R, -) -> Result { +pub fn read_v1_raw_message( + reader: &mut PeekReader, +) -> Result { loop { - // search for the magic framing value indicating start of mavlink message - if reader.read_u8()? == MAV_STX { - break; + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX { + break; + } } - } - - let mut message = MAVLinkV1MessageRaw::new(); - - message.0[0] = MAV_STX; - reader.read_exact(message.mut_header())?; - reader.read_exact(message.mut_payload_and_checksum())?; - Ok(message) + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE)? + [..MAVLinkV1MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum = + &reader.peek_exact(packet_length)?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum() + .copy_from_slice(payload_and_checksum); + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + reader.consume(message.raw_bytes().len() - 1); + return Ok(message); + } + } } /// Read a MAVLink v1 message from a Read stream. pub fn read_v1_msg( - r: &mut R, + r: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { - loop { - let message = read_v1_raw_message(r)?; - if !message.has_valid_crc::() { - continue; - } - - return M::parse( - MavlinkVersion::V1, - u32::from(message.message_id()), - message.payload(), + let message = read_v1_raw_message::(r)?; + + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + ) + .map(|msg| { + ( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + msg, ) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()); - } + }) + .map_err(|err| err.into()) } const MAVLINK_IFLAG_SIGNED: u8 = 0x01; @@ -581,49 +592,55 @@ impl MAVLinkV2MessageRaw { /// Return a raw buffer with the mavlink message /// V2 maximum size is 280 bytes: `` -pub fn read_v2_raw_message( - reader: &mut R, -) -> Result { +pub fn read_v2_raw_message( + reader: &mut PeekReader, +) -> Result { loop { - // search for the magic framing value indicating start of mavlink message - if reader.read_u8()? == MAV_STX_V2 { - break; + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX_V2 { + break; + } } - } - - let mut message = MAVLinkV2MessageRaw::new(); - message.0[0] = MAV_STX_V2; - reader.read_exact(message.mut_header())?; - reader.read_exact(message.mut_payload_and_checksum_and_sign())?; - - Ok(message) + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX_V2; + let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE)? + [..MAVLinkV2MessageRaw::HEADER_SIZE]; + message.mut_header().copy_from_slice(header); + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum_and_sign = + &reader.peek_exact(packet_length)?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length]; + message + .mut_payload_and_checksum_and_sign() + .copy_from_slice(payload_and_checksum_and_sign); + + if message.has_valid_crc::() { + reader.consume(message.raw_bytes().len() - 1); + return Ok(message); + } + } } /// Read a MAVLink v2 message from a Read stream. pub fn read_v2_msg( - read: &mut R, + read: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { - loop { - let message = read_v2_raw_message(read)?; - if !message.has_valid_crc::() { - // bad crc: ignore message - continue; - } + let message = read_v2_raw_message::(read)?; - return M::parse(MavlinkVersion::V2, message.message_id(), message.payload()) - .map(|msg| { - ( - MavHeader { - sequence: message.sequence(), - system_id: message.system_id(), - component_id: message.component_id(), - }, - msg, - ) - }) - .map_err(|err| err.into()); - } + M::parse(MavlinkVersion::V2, message.message_id(), message.payload()) + .map(|msg| { + ( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + msg, + ) + }) + .map_err(|err| err.into()) } /// Write a message using the given mavlink version diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs new file mode 100644 index 000000000..854188176 --- /dev/null +++ b/mavlink-core/src/peek_reader.rs @@ -0,0 +1,218 @@ +//! This module implements a buffered/peekable reader. +//! +//! The purpose of the buffered/peekable reader is to allow for backtracking parsers. +//! +//! A reader implementing the standard librairy's [`std::io::BufRead`] trait seems like a good fit, but +//! it does not allow for peeking a specific number of bytes, so it provides no way to request +//! more data from the underlying reader without consuming the existing data. +//! +//! This API still tries to adhere to the [`std::io::BufRead`]'s trait philosophy. +//! +//! The main type `PeekReader`does not implement [`std::io::Read`] itself, as there is no added benefit +//! in doing so. +//! + +use std::io::{self, ErrorKind, Read}; + +// The default chunk size to read from the underlying reader +const DEFAULT_CHUNK_SIZE: usize = 1024; + +/// A buffered/peekable reader +/// +/// This reader wraps a type implementing [`std::io::Read`] and adds buffering via an internal buffer. +/// +/// It allows the user to `peek` a specified number of bytes (without consuming them), +/// to `read` bytes (consuming them), or to `consume` them after `peek`ing. +/// +pub struct PeekReader { + // Internal buffer + buffer: Vec, + // The position of the next byte to read in the buffer. + cursor: usize, + // The preferred chunk size. This is just a hint. + preferred_chunk_size: usize, + // The wrapped reader. + reader: R, + // Stashed error, if any. + error: Option, + // Whether we hit EOF on the underlying reader. + eof: bool, +} + +impl PeekReader { + /// Instanciates a new [`PeekReader`], wrapping the provided [`std::io::Read`]er and using the default chunk size + pub fn new(reader: R) -> Self { + Self::with_chunk_size(reader, DEFAULT_CHUNK_SIZE) + } + + /// Instanciates a new [`PeekReader`], wrapping the provided [`std::io::Read`]er and using the supplied chunk size + pub fn with_chunk_size(reader: R, preferred_chunk_size: usize) -> Self { + Self { + buffer: Vec::with_capacity(preferred_chunk_size), + cursor: 0, + preferred_chunk_size, + reader, + error: None, + eof: false, + } + } + + /// Peeks a specified amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// This function does not consume data from the buffer, so subsequent calls to `peek` or `read` functions + /// will still return the peeked data. + /// + pub fn peek(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, false, false) + } + + /// Peeks an exact amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function does not consume data from the buffer, so subsequent calls to `peek` or `read` functions + /// will still return the peeked data. + /// + pub fn peek_exact(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, true, false) + } + + /// Consumes a specified amount of bytes from the buffer + /// + /// If the internal buffer does not contain enough data, this function will consume as much data as is buffered. + /// + pub fn consume(&mut self, amount: usize) -> usize { + let amount = amount.min(self.buffer.len() - self.cursor); + self.cursor += amount; + amount + } + + /// Reads a specified amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub fn read(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, false, true) + } + + /// Reads a specified amount of bytes from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub fn read_exact(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, true, true) + } + + /// Reads a byte from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`std::io::Read`]er until it does, an error occurs or no more data can be read (EOF). + /// + /// If an EOF occurs and the specified amount could not be read, this function will return an [`ErrorKind::UnexpectedEof`]. + /// + /// This function consumes the data from the buffer, unless an error occurs, in which case no data is consumed. + /// + pub fn read_u8(&mut self) -> io::Result { + let buf = self.read_exact(1)?; + Ok(buf[0]) + } + + /// Returns an immutable reference to the underlying [`std::io::Read`]er + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_ref(&mut self) -> &R { + &self.reader + } + + /// Returns a mutable reference to the underlying [`std::io::Read`]er + /// + /// Reading directly from the underlying stream will cause data loss + pub fn reader_mut(&mut self) -> &mut R { + &mut self.reader + } + + /// Internal function to fetch data from the internal buffer and/or reader + fn fetch(&mut self, amount: usize, exact: bool, consume: bool) -> io::Result<&[u8]> { + let previous_len = self.buffer.len(); + let mut buffered = previous_len - self.cursor; + + // the caller requested more bytes tha we have buffered, fetch them from the reader + if buffered < amount { + // if we got an earlier EOF, return it + if self.eof { + return Err(io::Error::new( + ErrorKind::UnexpectedEof, + "Unexpected EOF already returned in previous call to reader", + )); + } + // if we have a stashed error, return it (and clear it) + if let Some(e) = self.error.take() { + if e.kind() == ErrorKind::UnexpectedEof { + self.eof = true; + } + return Err(e); + } + + let needed = amount - buffered; + let chunk_size = self.preferred_chunk_size.max(needed); + let mut buf = vec![0u8; chunk_size]; + + // read needed bytes from reader + let mut read = 0; + while read < needed { + match self.reader.read(&mut buf[read..]) { + Ok(n) => { + if n == 0 { + break; + } + read += n; + } + Err(e) => { + self.error = Some(e); + break; + } + } + } + // if some bytes were read, add them to the buffer + if read > 0 { + if self.buffer.capacity() - previous_len < read { + // reallocate + self.buffer + .copy_within(self.cursor..self.cursor + buffered, 0); + self.buffer.truncate(buffered); + self.cursor = 0; + } + self.buffer.extend_from_slice(&buf[..read]); + buffered += read; + } + + if buffered == 0 && self.error.is_some() { + return Err(self.error.take().unwrap()); + } + } + if exact && buffered < amount { + return Err(io::Error::new(ErrorKind::UnexpectedEof, "Unexpected EOF")); + } + + let result_len = amount.min(buffered); + let result = &self.buffer[self.cursor..self.cursor + result_len]; + if consume { + self.cursor += result_len; + } + Ok(result) + } +} diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 728e630a3..08cf4e1ab 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -2,7 +2,14 @@ [package] name = "mavlink" version = "0.12.2" -authors = ["Todd Stellanova", "Michal Podhradsky", "Kevin Mehall", "Tim Ryan", "Patrick José Pereira", "Ibiyemi Abiodun"] +authors = [ + "Todd Stellanova", + "Michal Podhradsky", + "Kevin Mehall", + "Tim Ryan", + "Patrick José Pereira", + "Ibiyemi Abiodun", +] build = "build/main.rs" description = "Implements the MAVLink data interchange format for UAVs." readme = "README.md" @@ -12,7 +19,7 @@ edition = "2018" rust-version = "1.65.0" [build-dependencies] -mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false} +mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false } [[example]] name = "mavlink-dump" @@ -20,7 +27,7 @@ path = "examples/mavlink-dump/src/main.rs" required-features = ["ardupilotmega"] [dependencies] -mavlink-core = { path = "../mavlink-core", default-features = false} +mavlink-core = { path = "../mavlink-core", default-features = false } num-traits = { workspace = true, default-features = false } num-derive = { workspace = true } bitflags = { workspace = true } @@ -93,4 +100,10 @@ default = ["std", "tcp", "udp", "direct-serial", "serde", "ardupilotmega"] # build with all features on docs.rs so that users viewing documentation # can see everything [package.metadata.docs.rs] -features = ["default", "all-dialects", "emit-description", "emit-extensions", "format-generated-code"] +features = [ + "default", + "all-dialects", + "emit-description", + "emit-extensions", + "format-generated-code", +] diff --git a/mavlink/tests/encode_decode_tests.rs b/mavlink/tests/encode_decode_tests.rs index f4b380074..0cc290eb3 100644 --- a/mavlink/tests/encode_decode_tests.rs +++ b/mavlink/tests/encode_decode_tests.rs @@ -3,6 +3,7 @@ mod test_shared; #[cfg(feature = "common")] mod test_encode_decode { use mavlink::{common, Message}; + use mavlink_core::peek_reader::PeekReader; #[test] pub fn test_echo_heartbeat() { @@ -16,7 +17,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); assert_eq!(recv_msg.message_id(), 0); @@ -34,7 +35,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let common::MavMessage::COMMAND_INT(recv_msg) = recv_msg { @@ -56,7 +57,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(recv_msg) = recv_msg { assert_eq!( @@ -85,9 +86,8 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); - let (_header, recv_msg) = mavlink::read_v2_msg::(&mut c) - .expect("Failed to read"); + let mut c = PeekReader::new(v.as_slice()); + let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); match &recv_msg { ardupilotmega::MavMessage::HEARTBEAT(_data) => { @@ -115,7 +115,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); if let ardupilotmega::MavMessage::MOUNT_STATUS(recv_msg) = recv_msg { assert_eq!(4, recv_msg.pointing_b); @@ -139,9 +139,8 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); - let (_header, recv_msg) = mavlink::read_v2_msg::(&mut c) - .expect("Failed to read"); + let mut c = PeekReader::new(v.as_slice()); + let (_header, recv_msg) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); match &recv_msg { ardupilotmega::MavMessage::COMMAND_INT(data) => { diff --git a/mavlink/tests/process_log_files.rs b/mavlink/tests/process_log_files.rs index 0bf71544f..5d60db9e3 100644 --- a/mavlink/tests/process_log_files.rs +++ b/mavlink/tests/process_log_files.rs @@ -49,7 +49,7 @@ mod process_files { println!("Number of parsed messages: {counter}"); assert!( - counter == 1374, + counter == 1426, "Unable to hit the necessary amount of matches" ); } diff --git a/mavlink/tests/v1_encode_decode_tests.rs b/mavlink/tests/v1_encode_decode_tests.rs index db7ea6fca..048e97c8e 100644 --- a/mavlink/tests/v1_encode_decode_tests.rs +++ b/mavlink/tests/v1_encode_decode_tests.rs @@ -2,6 +2,7 @@ pub mod test_shared; #[cfg(all(feature = "std", feature = "common"))] mod test_v1_encode_decode { + use mavlink_core::peek_reader::PeekReader; pub const HEARTBEAT_V1: &[u8] = &[ mavlink::MAV_STX, @@ -25,7 +26,7 @@ mod test_v1_encode_decode { #[test] pub fn test_read_heartbeat() { - let mut r = HEARTBEAT_V1; + let mut r = PeekReader::new(HEARTBEAT_V1); let (header, msg) = mavlink::read_v1_msg(&mut r).expect("Failed to parse message"); //println!("{:?}, {:?}", header, msg); @@ -73,7 +74,7 @@ mod test_v1_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, mavlink::common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read"); diff --git a/mavlink/tests/v2_encode_decode_tests.rs b/mavlink/tests/v2_encode_decode_tests.rs index 0aae35bf5..f83eb127a 100644 --- a/mavlink/tests/v2_encode_decode_tests.rs +++ b/mavlink/tests/v2_encode_decode_tests.rs @@ -2,6 +2,8 @@ mod test_shared; #[cfg(all(feature = "std", feature = "common"))] mod test_v2_encode_decode { + use mavlink_core::peek_reader::PeekReader; + pub const HEARTBEAT_V2: &[u8] = &[ mavlink::MAV_STX_V2, //magic 0x09, //payload len @@ -28,7 +30,7 @@ mod test_v2_encode_decode { #[test] pub fn test_read_v2_heartbeat() { - let mut r = HEARTBEAT_V2; + let mut r = PeekReader::new(HEARTBEAT_V2); let (header, msg) = mavlink::read_v2_msg(&mut r).expect("Failed to parse message"); assert_eq!(header, crate::test_shared::COMMON_MSG_HEADER); @@ -110,7 +112,7 @@ mod test_v2_encode_decode { #[test] pub fn test_read_truncated_command_long() { - let mut r = COMMAND_LONG_TRUNCATED_V2; + let mut r = PeekReader::new(COMMAND_LONG_TRUNCATED_V2); let (_header, recv_msg) = mavlink::read_v2_msg(&mut r).expect("Failed to parse COMMAND_LONG_TRUNCATED_V2"); @@ -139,7 +141,7 @@ mod test_v2_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = PeekReader::new(v.as_slice()); let (_header, recv_msg): (mavlink::MavHeader, mavlink::common::MavMessage) = mavlink::read_v2_msg(&mut c).expect("Failed to read");