From 6fbbfd2ba3d98526eb5c78aaa784eb176f9c1291 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 10 Aug 2022 11:28:24 -0300 Subject: [PATCH 001/159] src: connection: udp: Remove usage of unnecessary MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/connection/udp.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/connection/udp.rs b/src/connection/udp.rs index e9418e9a06..5ba92364d1 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -32,7 +32,7 @@ pub fn udpbcast(address: T) -> io::Result { .unwrap() .next() .expect("Invalid address"); - let socket = UdpSocket::bind(&SocketAddr::from_str("0.0.0.0:0").unwrap()).unwrap(); + let socket = UdpSocket::bind("0.0.0.0:0").unwrap(); socket .set_broadcast(true) .expect("Couldn't bind to broadcast address."); @@ -45,7 +45,7 @@ pub fn udpout(address: T) -> io::Result { .unwrap() .next() .expect("Invalid address"); - let socket = UdpSocket::bind(&SocketAddr::from_str("0.0.0.0:0").unwrap())?; + let socket = UdpSocket::bind("0.0.0.0:0")?; UdpConnection::new(socket, false, Some(addr)) } From 4fc603ebdb487434ce2d4aa1941df7fe055755a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 10 Aug 2022 23:26:32 -0300 Subject: [PATCH 002/159] lib: Add read_v2_raw_message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 157 +++++++++++++++++++++++++++++++---------------------- 1 file changed, 92 insertions(+), 65 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 55e6095662..3e96517b4f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -259,86 +259,113 @@ pub fn read_v1_msg( const MAVLINK_IFLAG_SIGNED: u8 = 0x01; -/// Read a MAVLink v2 message from a Read stream. -pub fn read_v2_msg( - r: &mut R, -) -> Result<(MavHeader, M), error::MessageReadError> { - loop { - // search for the magic framing value indicating start of mavlink message - if r.read_u8()? != MAV_STX_V2 { - continue; - } - - // println!("Got STX2"); - let payload_len = r.read_u8()? as usize; - // println!("Got payload_len: {}", payload_len); - let incompat_flags = r.read_u8()?; - // println!("Got incompat flags: {}", incompat_flags); - let compat_flags = r.read_u8()?; - // println!("Got compat flags: {}", compat_flags); - - let seq = r.read_u8()?; - // println!("Got seq: {}", seq); +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +// Follow protocol definition: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format +pub struct MAVLinkV2MessageRaw { + pub start: u8, // = MAV_STX_V2 + pub payload_length: u8, + pub incompatibility_flags: u8, + pub compatibility_flags: u8, + pub sequence: u8, + pub system_id: u8, + pub component_id: u8, + pub message_id: u32, + pub payload_buffer: [u8; 255], + pub checksum: u16, +} - let sysid = r.read_u8()?; - // println!("Got sysid: {}", sysid); +impl MAVLinkV2MessageRaw { + pub fn payload(&self) -> &[u8] { + return &self.payload_buffer[..self.payload_length as usize]; + } - let compid = r.read_u8()?; - // println!("Got compid: {}", compid); - - let mut msgid_buf = [0; 4]; - msgid_buf[0] = r.read_u8()?; - msgid_buf[1] = r.read_u8()?; - msgid_buf[2] = r.read_u8()?; - - let header_buf = &[ - payload_len as u8, - incompat_flags, - compat_flags, - seq, - sysid, - compid, - msgid_buf[0], - msgid_buf[1], - msgid_buf[2], - ]; - - let msgid: u32 = u32::from_le_bytes(msgid_buf); - // println!("Got msgid: {}", msgid); - - //provide a buffer that is the maximum payload size - let mut payload_buf = [0; 255]; - let payload = &mut payload_buf[..payload_len]; + pub fn mut_payload(&mut self) -> &mut [u8] { + return &mut self.payload_buffer[..self.payload_length as usize]; + } - r.read_exact(payload)?; + pub fn calculate_crc(&self) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(&[ + self.payload_length, + self.incompatibility_flags, + self.compatibility_flags, + self.sequence, + self.system_id, + self.component_id, + ]); + let payload = self.payload(); + crc_calculator.digest(&self.message_id.to_le_bytes()[..3]); + crc_calculator.digest(payload); + let extra_crc = M::extra_crc(self.message_id); + + crc_calculator.digest(&[extra_crc]); + return crc_calculator.get_crc(); + } - let crc = r.read_u16::()?; + pub fn has_valid_crc(&self) -> bool { + return self.checksum == self.calculate_crc::(); + } +} - if (incompat_flags & 0x01) == MAVLINK_IFLAG_SIGNED { - let mut sign = [0; 13]; - r.read_exact(&mut sign)?; +/// Return a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: https://mavlink.io/en/guide/serialization.html +pub fn read_v2_raw_message( + reader: &mut R, +) -> Result { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX_V2 { + break; } + } - let mut crc_calc = CRCu16::crc16mcrf4cc(); - crc_calc.digest(header_buf); - crc_calc.digest(payload); - let extra_crc = M::extra_crc(msgid); + let mut message = MAVLinkV2MessageRaw { + start: MAV_STX_V2, + payload_length: reader.read_u8()?, + incompatibility_flags: reader.read_u8()?, + compatibility_flags: reader.read_u8()?, + sequence: reader.read_u8()?, + system_id: reader.read_u8()?, + component_id: reader.read_u8()?, + message_id: 0, + payload_buffer: [0; 255], + checksum: 0, + }; + + message.message_id = + u32::from_le_bytes([reader.read_u8()?, reader.read_u8()?, reader.read_u8()?, 0]); + + reader.read_exact(message.mut_payload())?; + + message.checksum = reader.read_u16::()?; + + // Signature to ensure the link is tamper-proof. + if (message.incompatibility_flags & 0x01) == MAVLINK_IFLAG_SIGNED { + let mut sign = [0; 13]; + reader.read_exact(&mut sign)?; + } - crc_calc.digest(&[extra_crc]); - let recvd_crc = crc_calc.get_crc(); - if recvd_crc != crc { + return Ok(message); +} + +/// Read a MAVLink v2 message from a Read stream. +pub fn read_v2_msg( + read: &mut R, +) -> Result<(MavHeader, M), error::MessageReadError> { + loop { + let message = read_v2_raw_message(read)?; + if !message.has_valid_crc::() { // bad crc: ignore message - // println!("msg id {} payload_len {} , crc got {} expected {}", msgid, payload_len, crc, recvd_crc ); continue; } - return M::parse(MavlinkVersion::V2, msgid, payload) + return M::parse(MavlinkVersion::V2, message.message_id, message.payload()) .map(|msg| { ( MavHeader { - sequence: seq, - system_id: sysid, - component_id: compid, + sequence: message.sequence, + system_id: message.system_id, + component_id: message.component_id, }, msg, ) From 9084a459193f8e32d826e320adbc48ded5bae922 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 10 Aug 2022 23:49:43 -0300 Subject: [PATCH 003/159] lib: Add read_v1_raw_message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 127 ++++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 92 insertions(+), 35 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 3e96517b4f..731c8072f7 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -210,50 +210,107 @@ pub fn read_versioned_msg( } } -/// Read a MAVLink v1 message from a Read stream. -pub fn read_v1_msg( - r: &mut R, -) -> Result<(MavHeader, M), error::MessageReadError> { +#[derive(Debug, Copy, Clone, PartialEq, Eq)] +// Follow protocol definition: https://mavlink.io/en/guide/serialization.html#v1_packet_format +pub struct MAVLinkV1MessageRaw { + pub start: u8, // = MAV_STX + pub payload_length: u8, + pub sequence: u8, + pub system_id: u8, + pub component_id: u8, + pub message_id: u8, + pub payload_buffer: [u8; 255], + pub checksum: u16, +} + +impl MAVLinkV1MessageRaw { + pub fn payload(&self) -> &[u8] { + return &self.payload_buffer[..self.payload_length as usize]; + } + + pub fn mut_payload(&mut self) -> &mut [u8] { + return &mut self.payload_buffer[..self.payload_length as usize]; + } + + pub fn calculate_crc(&self) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(&[ + self.payload_length, + self.sequence, + self.system_id, + self.component_id, + self.message_id, + ]); + let payload = self.payload(); + crc_calculator.digest(payload); + let extra_crc = M::extra_crc(self.message_id.into()); + + crc_calculator.digest(&[extra_crc]); + return crc_calculator.get_crc(); + } + + pub fn has_valid_crc(&self) -> bool { + return self.checksum == self.calculate_crc::(); + } +} + +/// Return a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: https://mavlink.io/en/guide/serialization.html +pub fn read_v1_raw_message( + reader: &mut R, +) -> Result { loop { - if r.read_u8()? != MAV_STX { - continue; + // search for the magic framing value indicating start of mavlink message + if reader.read_u8()? == MAV_STX { + break; } - let len = r.read_u8()? as usize; - let seq = r.read_u8()?; - let sysid = r.read_u8()?; - let compid = r.read_u8()?; - let msgid = r.read_u8()?; + } - let mut payload_buf = [0; 255]; - let payload = &mut payload_buf[..len]; + let mut message = MAVLinkV1MessageRaw { + start: MAV_STX, + payload_length: reader.read_u8()?, + sequence: reader.read_u8()?, + system_id: reader.read_u8()?, + component_id: reader.read_u8()?, + message_id: reader.read_u8()?, + payload_buffer: [0; 255], + checksum: 0, + }; - r.read_exact(payload)?; + let payload = &mut message.payload_buffer[..message.payload_length as usize]; + reader.read_exact(payload)?; - let crc = r.read_u16::()?; + message.checksum = reader.read_u16::()?; - let mut crc_calc = CRCu16::crc16mcrf4cc(); - crc_calc.digest(&[len as u8, seq, sysid, compid, msgid]); - crc_calc.digest(payload); - crc_calc.digest(&[M::extra_crc(msgid.into())]); - let recvd_crc = crc_calc.get_crc(); - if recvd_crc != crc { - // bad crc: ignore message - //println!("msg id {} len {} , crc got {} expected {}", msgid, len, crc, recvd_crc ); + return Ok(message); +} + +/// Read a MAVLink v1 message from a Read stream. +pub fn read_v1_msg( + r: &mut R, +) -> Result<(MavHeader, M), error::MessageReadError> { + loop { + let message = read_v1_raw_message(r)?; + if !message.has_valid_crc::() { continue; } - return M::parse(MavlinkVersion::V1, msgid as u32, payload) - .map(|msg| { - ( - MavHeader { - sequence: seq, - system_id: sysid, - component_id: compid, - }, - msg, - ) - }) - .map_err(|err| err.into()); + return M::parse( + MavlinkVersion::V1, + message.message_id as u32, + message.payload(), + ) + .map(|msg| { + ( + MavHeader { + sequence: message.sequence, + system_id: message.system_id, + component_id: message.component_id, + }, + msg, + ) + }) + .map_err(|err| err.into()); } } From 21fc5cf2f43c74840035595e3caead87d544f5de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 17 Aug 2022 08:41:18 -0300 Subject: [PATCH 004/159] Cargo: Update doc features to have description and extensions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index d542f56f8a..ca1db9a6ad 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -79,4 +79,4 @@ default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "common"] # build with all features on docs.rs so that users viewing documentation # can see everything [package.metadata.docs.rs] -features = ["default", "all-dialects"] +features = ["default", "all-dialects", "emit-description", "emit-extensions"] From 0eb4c91f3285d68d8050612f903a1d6c03ba5e63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 17 Aug 2022 08:42:49 -0300 Subject: [PATCH 005/159] src: connection: udp: Remove unused FromStr MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/connection/udp.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/connection/udp.rs b/src/connection/udp.rs index 5ba92364d1..ef41b78485 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -4,7 +4,6 @@ use std::io::Read; use std::io::{self}; use std::net::ToSocketAddrs; use std::net::{SocketAddr, UdpSocket}; -use std::str::FromStr; use std::sync::Mutex; /// UDP MAVLink connection From 17c4eef400487d7a5c656c2187bcfb828f4a3874 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 17 Aug 2022 08:43:04 -0300 Subject: [PATCH 006/159] src: lib: Remove unused WriteBytesExt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 731c8072f7..79f74c2cb6 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -35,7 +35,7 @@ use std::io::{Read, Write}; extern crate byteorder; use byteorder::LittleEndian; #[cfg(feature = "std")] -use byteorder::{ReadBytesExt, WriteBytesExt}; +use byteorder::ReadBytesExt; #[cfg(feature = "std")] mod connection; From 9a7a51d251cb563c73af0357fdded81a453cc698 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Tue, 16 Aug 2022 15:58:44 -0300 Subject: [PATCH 007/159] github: test: Add emit-description MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 20df235010..5c389ece7c 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -44,7 +44,7 @@ jobs: - os: ubuntu-latest TARGET: x86_64-unknown-linux-musl - FLAGS: --features all-dialects,emit-extensions + FLAGS: --features all-dialects,emit-description,emit-extensions - os: ubuntu-latest TARGET: thumbv7m-none-eabi From 4854089f21fb3bf63b16e3e7a3e62518859344d5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 17 Aug 2022 11:01:54 -0300 Subject: [PATCH 008/159] build: parser: Add documentation for enums MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- build/parser.rs | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/build/parser.rs b/build/parser.rs index b621bcf3f0..7ee81a4e44 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -448,6 +448,17 @@ impl MavEnum { .map(|enum_entry| { let name = Ident::from(enum_entry.name.clone()); let value; + + #[cfg(feature = "emit-description")] + let description = if let Some(description) = enum_entry.description.as_ref() { + Ident::from(format!("// {description}\n")) + } else { + Ident::from("") + }; + + #[cfg(not(feature = "emit-description"))] + let description = Ident::from(""); + if !self.has_enum_values() { value = Ident::from(cnt.to_string()); cnt += 1; @@ -457,7 +468,7 @@ impl MavEnum { if self.bitfield.is_some() { quote!(const #name = #value;) } else { - quote!(#name = #value,) + quote!(#name = #value, #description) } }) .collect::>() @@ -473,12 +484,23 @@ impl MavEnum { let default = Ident::from(self.entries[0].name.clone()); let enum_name = self.emit_name(); + #[cfg(feature = "emit-description")] + let description = if let Some(description) = self.description.as_ref() { + Ident::from(format!("/// {description}\n")) + } else { + Ident::from("") + }; + + #[cfg(not(feature = "emit-description"))] + let description = Ident::from(""); + let enum_def; if let Some(width) = self.bitfield.clone() { let width = Ident::from(width); enum_def = quote! { bitflags!{ #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #description pub struct #enum_name: #width { #(#defs)* } @@ -489,6 +511,7 @@ impl MavEnum { #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] + #description pub enum #enum_name { #(#defs)* } From 710692f6d4bd415e93e5a6305eb5b47186ece302 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 17 Aug 2022 11:19:24 -0300 Subject: [PATCH 009/159] build: parser: Add documentation for bitfields MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit They do not appears in the docs, but we can check in the code. It's probably a problem with bitflags! Signed-off-by: Patrick José Pereira --- build/parser.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build/parser.rs b/build/parser.rs index 7ee81a4e44..abefe25607 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -466,7 +466,7 @@ impl MavEnum { value = Ident::from(enum_entry.value.unwrap().to_string()); }; if self.bitfield.is_some() { - quote!(const #name = #value;) + quote!(const #name = #value; #description) } else { quote!(#name = #value, #description) } From 9803e32e767827b01605367ffa18f6476a2c23b3 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Thu, 18 Aug 2022 08:51:28 +0200 Subject: [PATCH 010/159] Use u8 instead of chars in Rust This change will cause structures to fill less in memory since the `char` type in rust is actually a unicode scalar value and is 4 bytes large[^1] compared to C where a c where it is most often a single byte. [^1]: `std::mem::size_of::() = 4` --- build/parser.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index abefe25607..dc6cc75cbe 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -873,7 +873,7 @@ impl MavType { pub fn rust_reader(&self, val: Ident, buf: Ident) -> Tokens { use self::MavType::*; match self.clone() { - Char => quote! {#val = #buf.get_u8() as char;}, + Char => quote! {#val = #buf.get_u8();}, UInt8 => quote! {#val = #buf.get_u8();}, UInt16 => quote! {#val = #buf.get_u16_le();}, UInt32 => quote! {#val = #buf.get_u32_le();}, @@ -915,7 +915,7 @@ impl MavType { match self.clone() { UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, UInt8 => quote! {#buf.put_u8(#val);}, - Char => quote! {#buf.put_u8(#val as u8);}, + Char => quote! {#buf.put_u8(#val);}, UInt16 => quote! {#buf.put_u16_le(#val);}, UInt32 => quote! {#buf.put_u32_le(#val);}, Int8 => quote! {#buf.put_i8(#val);}, @@ -987,7 +987,7 @@ impl MavType { match self.clone() { UInt8 | UInt8MavlinkVersion => "u8".into(), Int8 => "i8".into(), - Char => "char".into(), + Char => "u8".into(), UInt16 => "u16".into(), Int16 => "i16".into(), UInt32 => "u32".into(), From 3ea24349187a793ef2fdf4b947d5989903d17c46 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Thu, 18 Aug 2022 14:39:28 +0200 Subject: [PATCH 011/159] use quick-xml instead of xml-rs Since XML-rs is for the time being unmaintained[^1], and have been marked as such in RUSTSET[^2]. I have made ported the XML parsing to quick-xml. [^1]: https://github.com/netvl/xml-rs/issues/221 [^2]: https://rustsec.org/advisories/RUSTSEC-2022-0048 --- Cargo.toml | 2 +- build/main.rs | 2 +- build/parser.rs | 237 ++++++++++++++++++++++++++++++------------------ 3 files changed, 152 insertions(+), 89 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index ca1db9a6ad..374f299c3a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ edition = "2018" [build-dependencies] crc-any = "2.3.0" bytes = { version = "1.0", default-features = false } -xml-rs = "0.2" +quick-xml = "0.23" quote = "0.3" lazy_static = "1.2.0" serde = { version = "1.0.115", optional = true, features = ["derive"] } diff --git a/build/main.rs b/build/main.rs index a4750ebead..2feffdaadd 100644 --- a/build/main.rs +++ b/build/main.rs @@ -2,7 +2,7 @@ #[macro_use] extern crate quote; -extern crate xml; +extern crate quick_xml; mod binder; mod parser; diff --git a/build/parser.rs b/build/parser.rs index dc6cc75cbe..285b797690 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -1,10 +1,11 @@ use crc_any::CRCu16; use std::cmp::Ordering; use std::default::Default; -use std::io::{Read, Write}; +use std::io::{BufRead, BufReader, Read, Write}; use std::u32; -use xml::reader::{EventReader, XmlEvent}; +//use xml::reader::{EventReader, XmlEvent}; +use quick_xml::{events::Event, Reader}; use quote::{Ident, Tokens}; @@ -1036,24 +1037,24 @@ pub enum MavXmlElement { Extensions, } -fn identify_element(s: &str) -> Option { +fn identify_element(s: &[u8]) -> Option { use self::MavXmlElement::*; match s { - "version" => Some(Version), - "mavlink" => Some(Mavlink), - "dialect" => Some(Dialect), - "include" => Some(Include), - "enums" => Some(Enums), - "enum" => Some(Enum), - "entry" => Some(Entry), - "description" => Some(Description), - "param" => Some(Param), - "messages" => Some(Messages), - "message" => Some(Message), - "field" => Some(Field), - "deprecated" => Some(Deprecated), - "wip" => Some(Wip), - "extensions" => Some(Extensions), + b"version" => Some(Version), + b"mavlink" => Some(Mavlink), + b"dialect" => Some(Dialect), + b"include" => Some(Include), + b"enums" => Some(Enums), + b"enum" => Some(Enum), + b"entry" => Some(Entry), + b"description" => Some(Description), + b"param" => Some(Param), + b"messages" => Some(Messages), + b"message" => Some(Message), + b"field" => Some(Field), + b"deprecated" => Some(Deprecated), + b"wip" => Some(Wip), + b"extensions" => Some(Extensions), _ => None, } } @@ -1079,7 +1080,7 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { } } -pub fn parse_profile(file: &mut dyn Read) -> MavProfile { +pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { let mut stack: Vec = vec![]; let mut profile = MavProfile { @@ -1096,20 +1097,34 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { let mut paramid: Option = None; let mut xml_filter = MavXmlFilter::default(); - let mut parser: Vec> = - EventReader::new(file).into_iter().collect(); - xml_filter.filter(&mut parser); + let mut events: Vec> = Vec::new(); + let mut reader = Reader::from_reader(file); + reader.trim_text(true); + reader.trim_text_end(true); + + let mut buf = Vec::new(); + loop { + match reader.read_event(&mut buf) { + Ok(Event::Eof) => { + events.push(Ok(Event::Eof)); + break; + } + Ok(event) => events.push(Ok(event.into_owned())), + Err(why) => events.push(Err(why)), + } + buf.clear(); + } + xml_filter.filter(&mut events); let mut is_in_extension = false; - for e in parser { + for e in events { match e { - Ok(XmlEvent::StartElement { - name, - attributes: attrs, - .. - }) => { - let id = match identify_element(&name.to_string()) { + Ok(Event::Start(bytes)) => { + let id = match identify_element(&bytes.name()) { None => { - panic!("unexpected element {:?}", name); + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name()) + ); } Some(kind) => kind, }; @@ -1152,19 +1167,19 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { stack.push(id); - for attr in attrs { + for attr in bytes.attributes() { + let attr = attr.unwrap(); match stack.last() { - Some(&MavXmlElement::Enum) => match attr.name.local_name.clone().as_ref() { - "name" => { + Some(&MavXmlElement::Enum) => match attr.key { + b"name" => { mavenum.name = attr .value .clone() - .split("_") - .map(|x| x.to_lowercase()) - .map(|x| { - let mut v: Vec = x.chars().collect(); - v[0] = v[0].to_uppercase().nth(0).unwrap(); - v.into_iter().collect() + .split(|b| *b == b'_') + .map(|x| x.to_ascii_lowercase()) + .map(|mut v| { + v[0] = v[0].to_ascii_uppercase(); + String::from_utf8(v).unwrap() }) .collect::>() .join(""); @@ -1173,30 +1188,32 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { _ => (), }, Some(&MavXmlElement::Entry) => { - match attr.name.local_name.clone().as_ref() { - "name" => { - entry.name = attr.value.clone(); + match attr.key { + b"name" => { + let name = String::from_utf8(attr.value.to_vec()).unwrap(); + entry.name = name; } - "value" => { + b"value" => { // Deal with hexadecimal numbers - if attr.value.starts_with("0x") { + if attr.value.starts_with(b"0x") { entry.value = Some( u32::from_str_radix( - attr.value.trim_start_matches("0x"), + std::str::from_utf8(&attr.value[2..]).unwrap(), 16, ) .unwrap(), ); } else { - entry.value = Some(attr.value.parse::().unwrap()); + let s = std::str::from_utf8(&attr.value[..]).unwrap(); + entry.value = Some(s.parse::().unwrap()); } } _ => (), } } Some(&MavXmlElement::Message) => { - match attr.name.local_name.clone().as_ref() { - "name" => { + match attr.key { + b"name" => { /*message.name = attr .value .clone() @@ -1210,44 +1227,46 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { .collect::>() .join(""); */ - message.name = attr.value.clone(); + message.name = String::from_utf8(attr.value.to_vec()).unwrap(); } - "id" => { - //message.id = attr.value.parse::().unwrap(); - message.id = attr.value.parse::().unwrap(); + b"id" => { + let s = std::str::from_utf8(&attr.value).unwrap(); + message.id = s.parse::().unwrap(); } _ => (), } } Some(&MavXmlElement::Field) => { - match attr.name.local_name.clone().as_ref() { - "name" => { - field.name = attr.value.clone(); + match attr.key { + b"name" => { + let name = String::from_utf8(attr.value.to_vec()).unwrap(); + field.name = name; if field.name == "type" { field.name = "mavtype".to_string(); } } - "type" => { - field.mavtype = MavType::parse_type(&attr.value).unwrap(); + b"type" => { + let s = std::str::from_utf8(&attr.value).unwrap(); + field.mavtype = MavType::parse_type(&s).unwrap(); } - "enum" => { + b"enum" => { field.enumtype = Some( attr.value .clone() - .split("_") - .map(|x| x.to_lowercase()) - .map(|x| { - let mut v: Vec = x.chars().collect(); - v[0] = v[0].to_uppercase().nth(0).unwrap(); - v.into_iter().collect() + .split(|b| *b == b'_') + .map(|x| x.to_ascii_lowercase()) + .map(|mut v| { + v[0] = v[0].to_ascii_uppercase(); + String::from_utf8(v).unwrap() }) .collect::>() .join(""), ); //field.enumtype = Some(attr.value.clone()); } - "display" => { - field.display = Some(attr.value); + b"display" => { + field.display = + Some(String::from_utf8(attr.value.to_vec()).unwrap()); } _ => (), } @@ -1256,9 +1275,10 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { if let None = entry.params { entry.params = Some(vec![]); } - match attr.name.local_name.clone().as_ref() { - "index" => { - paramid = Some(attr.value.parse::().unwrap()); + match attr.key { + b"index" => { + let s = std::str::from_utf8(&attr.value).unwrap(); + paramid = Some(s.parse::().unwrap()); } _ => (), } @@ -1267,7 +1287,32 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { } } } - Ok(XmlEvent::Characters(s)) => { + Ok(Event::Empty(bytes)) => match bytes.name() { + b"extensions" => { + is_in_extension = true; + } + b"entry" => { + entry = Default::default(); + for attr in bytes.attributes() { + let attr = attr.unwrap(); + match attr.key { + b"name" => { + entry.name = String::from_utf8(attr.value.to_vec()).unwrap(); + } + b"value" => { + let s = std::str::from_utf8(&attr.value).unwrap(); + entry.value = Some(s.parse().unwrap()); + } + _ => (), + } + } + mavenum.entries.push(entry.clone()); + } + _ => (), + }, + Ok(Event::Text(bytes)) => { + let s = String::from_utf8(bytes.to_vec()).unwrap(); + use self::MavXmlElement::*; match (stack.last(), stack.get(stack.len() - 2)) { (Some(&Description), Some(&Message)) => { @@ -1311,7 +1356,7 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { } } } - Ok(XmlEvent::EndElement { .. }) => { + Ok(Event::End(_)) => { match stack.last() { Some(&MavXmlElement::Field) => message.fields.push(field.clone()), Some(&MavXmlElement::Entry) => { @@ -1363,7 +1408,8 @@ pub fn parse_profile(file: &mut dyn Read) -> MavProfile { /// Generate protobuf represenation of mavlink message set /// Generate rust representation of mavlink message set with appropriate conversion methods pub fn generate(input: &mut R, output_rust: &mut W) { - let profile = parse_profile(input); + let mut br = BufReader::new(input); + let profile = parse_profile(&mut br); // rust file let rust_tokens = profile.emit_rust(); @@ -1425,32 +1471,46 @@ impl Default for MavXmlFilter { } impl MavXmlFilter { - pub fn filter(&mut self, elements: &mut Vec>) { + pub fn filter(&mut self, elements: &mut Vec>) { // List of filters elements.retain(|x| self.filter_extension(x)); } #[cfg(feature = "emit-extensions")] - pub fn filter_extension( - &mut self, - _element: &Result, - ) -> bool { + pub fn filter_extension(&mut self, _element: &Result) -> bool { return true; } /// Ignore extension fields #[cfg(not(feature = "emit-extensions"))] - pub fn filter_extension( - &mut self, - element: &Result, - ) -> bool { + pub fn filter_extension(&mut self, element: &Result) -> bool { match element { Ok(content) => { match content { - XmlEvent::StartElement { name, .. } => { - let id = match identify_element(&name.to_string()) { + Event::Start(bytes) => { + let id = match identify_element(&bytes.name()) { + None => { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name()) + ); + } + Some(kind) => kind, + }; + match id { + MavXmlElement::Extensions => { + self.extension_filter.is_in = true; + } + _ => {} + } + } + Event::Empty(bytes) => { + let id = match identify_element(&bytes.name()) { None => { - panic!("unexpected element {:?}", name); + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name()) + ); } Some(kind) => kind, }; @@ -1461,10 +1521,13 @@ impl MavXmlFilter { _ => {} } } - XmlEvent::EndElement { name } => { - let id = match identify_element(&name.to_string()) { + Event::End(bytes) => { + let id = match identify_element(&bytes.name()) { None => { - panic!("unexpected element {:?}", name); + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name()) + ); } Some(kind) => kind, }; From df7d4b00e1ac7a64da7dd1945183d8308d4c0062 Mon Sep 17 00:00:00 2001 From: "daniel.eades" Date: Mon, 22 Aug 2022 09:53:01 +0100 Subject: [PATCH 012/159] add dependabot config --- .github/dependabot.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 .github/dependabot.yml diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000000..8ef5278d08 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,10 @@ +version: 2 +updates: +- package-ecosystem: cargo + directory: "/" + schedule: + interval: daily +- package-ecosystem: github-actions + directory: "/" + schedule: + interval: daily From 0e33839c4896917dbe444165b30906675f740c32 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 22 Aug 2022 11:56:55 +0000 Subject: [PATCH 013/159] build(deps): update nb requirement from 0.1 to 1.0 Updates the requirements on [nb](https://github.com/rust-embedded/nb) to permit the latest version. - [Release notes](https://github.com/rust-embedded/nb/releases) - [Changelog](https://github.com/rust-embedded/nb/blob/master/CHANGELOG.md) - [Commits](https://github.com/rust-embedded/nb/compare/v0.1.0...v1.0.0) --- updated-dependencies: - dependency-name: nb dependency-type: direct:production ... Signed-off-by: dependabot[bot] --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 374f299c3a..615593bc19 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -32,7 +32,7 @@ serial = { version = "0.4", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } embedded-hal = { version = "0.2", optional = true } -nb = { version = "0.1", optional = true } +nb = { version = "1.0", optional = true } [features] "ardupilotmega" = ["common", "icarous", "uavionix"] From b347918b9b42f38828e154365c371f0fec8cbd39 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 22 Aug 2022 11:56:34 +0000 Subject: [PATCH 014/159] build(deps): bump actions-rs/toolchain from 1.0.1 to 1.0.7 Bumps [actions-rs/toolchain](https://github.com/actions-rs/toolchain) from 1.0.1 to 1.0.7. - [Release notes](https://github.com/actions-rs/toolchain/releases) - [Changelog](https://github.com/actions-rs/toolchain/blob/master/CHANGELOG.md) - [Commits](https://github.com/actions-rs/toolchain/compare/v1.0.1...v1.0.7) --- updated-dependencies: - dependency-name: actions-rs/toolchain dependency-type: direct:production update-type: version-update:semver-patch ... Signed-off-by: dependabot[bot] --- .github/workflows/deploy.yml | 2 +- .github/workflows/test.yml | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 98154469f0..177e7ee22a 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable override: true diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 5c389ece7c..1a0bafea63 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -7,7 +7,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable target: ${{ matrix.TARGET }} @@ -58,7 +58,7 @@ jobs: - name: Building ${{ matrix.TARGET }} run: echo "${{ matrix.TARGET }}" - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable target: ${{ matrix.TARGET }} @@ -74,7 +74,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.1 + - uses: actions-rs/toolchain@v1.0.7 with: toolchain: nightly target: thumbv7em-none-eabihf From d59cd046e0071c2919792f6ea3cfccefd8f0dca5 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Thu, 18 Aug 2022 16:33:36 +0200 Subject: [PATCH 015/159] Update the parser to quote 1.0 This patch updates the parser from using quote 0.3 to using quote 1.0 and proc_macro2. --- Cargo.toml | 3 +- build/binder.rs | 4 +- build/parser.rs | 278 ++++++++++++++++++++++++++---------------------- 3 files changed, 153 insertions(+), 132 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 615593bc19..90fe4ce3cd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -14,7 +14,8 @@ edition = "2018" crc-any = "2.3.0" bytes = { version = "1.0", default-features = false } quick-xml = "0.23" -quote = "0.3" +quote = "1" +proc-macro2 = "1.0.43" lazy_static = "1.2.0" serde = { version = "1.0.115", optional = true, features = ["derive"] } diff --git a/build/binder.rs b/build/binder.rs index d3b663c510..a31c36653e 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -1,9 +1,9 @@ -use quote::Ident; +use quote::format_ident; use std::io::Write; pub fn generate(modules: Vec, out: &mut W) { let modules_tokens = modules.into_iter().map(|module| { - let module_ident = Ident::from(module.clone()); + let module_ident = format_ident!("{}", module); quote! { #[allow(non_camel_case_types)] diff --git a/build/parser.rs b/build/parser.rs index 285b797690..cf275dfeea 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -2,12 +2,14 @@ use crc_any::CRCu16; use std::cmp::Ordering; use std::default::Default; use std::io::{BufRead, BufReader, Read, Write}; +use std::str::FromStr; use std::u32; //use xml::reader::{EventReader, XmlEvent}; use quick_xml::{events::Event, Reader}; -use quote::{Ident, Tokens}; +use proc_macro2::{Ident, TokenStream}; +use quote::{format_ident, quote}; use crate::util::to_module_name; #[cfg(feature = "serde")] @@ -59,80 +61,78 @@ impl MavProfile { // } /// Simple header comment - fn emit_comments(&self) -> Ident { - Ident::from(format!( - "// This file was automatically generated, do not edit \n" - )) + fn emit_comments(&self) -> TokenStream { + quote!(#![doc = "This file was automatically generated, do not edit"]) } /// Emit includes fn emit_includes(&self) -> Vec { self.includes .iter() - .map(|i| Ident::from(to_module_name(i))) + .map(|i| format_ident!("{}", to_module_name(i))) .collect::>() } /// Emit rust messages - fn emit_msgs(&self) -> Vec { + fn emit_msgs(&self) -> Vec { self.messages .iter() .map(|d| d.emit_rust()) - .collect::>() + .collect::>() } /// Emit rust enums - fn emit_enums(&self) -> Vec { + fn emit_enums(&self) -> Vec { self.enums .iter() .map(|d| d.emit_rust()) - .collect::>() + .collect::>() } /// Get list of original message names - fn emit_enum_names(&self) -> Vec { + fn emit_enum_names(&self) -> Vec { self.messages .iter() .map(|msg| { - let name = Ident::from(msg.name.clone()); + let name = format_ident!("{}", msg.name); quote!(#name) }) - .collect::>() + .collect::>() } /// Emit message names with "_DATA" at the end - fn emit_struct_names(&self) -> Vec { + fn emit_struct_names(&self) -> Vec { self.messages .iter() .map(|msg| msg.emit_struct_name()) - .collect::>() + .collect::>() } /// A list of message IDs - fn emit_msg_ids(&self) -> Vec { + fn emit_msg_ids(&self) -> Vec { self.messages .iter() .map(|msg| { - let id = Ident::from(msg.id.to_string()); - quote!(#id) + let msg_id = msg.id; + quote!(#msg_id) }) - .collect::>() + .collect::>() } /// CRC values needed for mavlink parsing - fn emit_msg_crc(&self) -> Vec { + fn emit_msg_crc(&self) -> Vec { self.messages .iter() .map(|msg| { - let crc = Ident::from(extra_crc(&msg).to_string()); - quote!(#crc) + let ec = extra_crc(&msg); + quote!(#ec) }) - .collect::>() + .collect::>() } - fn emit_rust(&self) -> Tokens { + fn emit_rust(&self) -> TokenStream { //TODO verify that id_width of u8 is OK even in mavlink v1 - let id_width = Ident::from("u32"); + let id_width = format_ident!("u32"); let comment = self.emit_comments(); let msgs = self.emit_msgs(); @@ -208,10 +208,10 @@ impl MavProfile { fn emit_mav_message( &self, - enums: &Vec, - structs: &Vec, + enums: &Vec, + structs: &Vec, includes: &Vec, - ) -> Tokens { + ) -> TokenStream { let includes = includes.into_iter().map(|include| { quote! { #include(crate::#include::MavMessage) @@ -228,7 +228,7 @@ impl MavProfile { } } - fn emit_mav_message_from_includes(&self, includes: &Vec) -> Tokens { + fn emit_mav_message_from_includes(&self, includes: &Vec) -> TokenStream { let froms = includes.into_iter().map(|include| { quote! { impl From for MavMessage { @@ -246,12 +246,12 @@ impl MavProfile { fn emit_mav_message_parse( &self, - enums: &Vec, - structs: &Vec, - ids: &Vec, + enums: &Vec, + structs: &Vec, + ids: &Vec, includes: &Vec, - ) -> Tokens { - let id_width = Ident::from("u32"); + ) -> TokenStream { + let id_width = format_ident!("u32"); // try parsing all included message variants if it doesn't land in the id // range for this message @@ -279,10 +279,10 @@ impl MavProfile { fn emit_mav_message_crc( &self, id_width: &Ident, - ids: &Vec, - crc: &Vec, + ids: &Vec, + crc: &Vec, includes: &Vec, - ) -> Tokens { + ) -> TokenStream { let includes_branch = includes.into_iter().map(|include| { quote! { match crate::#include::MavMessage::extra_crc(id) { @@ -306,14 +306,18 @@ impl MavProfile { } } - fn emit_mav_message_name(&self, enums: &Vec, includes: &Vec) -> Tokens { + fn emit_mav_message_name( + &self, + enums: &Vec, + includes: &Vec, + ) -> TokenStream { let enum_names = enums .iter() .map(|enum_name| { - let name = Ident::from(format!("\"{}\"", enum_name)); + let name = enum_name.to_string(); quote!(#name) }) - .collect::>(); + .collect::>(); quote! { fn message_name(&self) -> &'static str { @@ -327,11 +331,11 @@ impl MavProfile { fn emit_mav_message_id( &self, - enums: &Vec, - ids: &Vec, + enums: &Vec, + ids: &Vec, includes: &Vec, - ) -> Tokens { - let id_width = Ident::from("u32"); + ) -> TokenStream { + let id_width = format_ident!("u32"); quote! { fn message_id(&self) -> #id_width { match self { @@ -344,10 +348,10 @@ impl MavProfile { fn emit_mav_message_id_from_name( &self, - enums: &Vec, - ids: &Vec, + enums: &Vec, + ids: &Vec, includes: &Vec, - ) -> Tokens { + ) -> TokenStream { let includes_branch = includes.into_iter().map(|include| { quote! { match crate::#include::MavMessage::message_id_from_name(name) { @@ -360,10 +364,10 @@ impl MavProfile { let enum_names = enums .iter() .map(|enum_name| { - let name = Ident::from(format!("\"{}\"", enum_name)); + let name = enum_name.to_string(); quote!(#name) }) - .collect::>(); + .collect::>(); quote! { fn message_id_from_name(name: &str) -> Result { @@ -381,17 +385,17 @@ impl MavProfile { fn emit_mav_message_default_from_id( &self, - enums: &Vec, - ids: &Vec, + enums: &Vec, + ids: &Vec, includes: &Vec, - ) -> Tokens { + ) -> TokenStream { let data_name = enums .iter() .map(|enum_name| { - let name = Ident::from(format!("{}_DATA", enum_name)); + let name = format_ident!("{}_DATA", enum_name.to_string()); quote!(#name) }) - .collect::>(); + .collect::>(); let includes_branches = includes.into_iter().map(|include| { quote! { @@ -415,7 +419,11 @@ impl MavProfile { } } - fn emit_mav_message_serialize(&self, enums: &Vec, includes: &Vec) -> Tokens { + fn emit_mav_message_serialize( + &self, + enums: &Vec, + includes: &Vec, + ) -> TokenStream { quote! { fn ser(&self) -> Vec { match self { @@ -442,62 +450,71 @@ impl MavEnum { self.entries.iter().all(|x| x.value.is_some()) } - fn emit_defs(&self) -> Vec { - let mut cnt = 0; + fn emit_defs(&self) -> Vec { + let mut cnt = 0isize; self.entries .iter() .map(|enum_entry| { - let name = Ident::from(enum_entry.name.clone()); + let name = format_ident!("{}", enum_entry.name.clone()); let value; #[cfg(feature = "emit-description")] let description = if let Some(description) = enum_entry.description.as_ref() { - Ident::from(format!("// {description}\n")) + quote!(#[doc = #description]) } else { - Ident::from("") + quote!() }; #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); if !self.has_enum_values() { - value = Ident::from(cnt.to_string()); + value = quote!(#cnt); cnt += 1; } else { - value = Ident::from(enum_entry.value.unwrap().to_string()); + let tmp = + TokenStream::from_str(&enum_entry.value.unwrap().to_string()).unwrap(); + value = quote!(#tmp); }; if self.bitfield.is_some() { - quote!(const #name = #value; #description) + quote! { + #description + const #name = #value; + } } else { - quote!(#name = #value, #description) + quote! { + #description + #name = #value, + } } }) - .collect::>() + .collect::>() } - fn emit_name(&self) -> Tokens { - let name = Ident::from(self.name.clone()); + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); quote!(#name) } - fn emit_rust(&self) -> Tokens { + fn emit_rust(&self) -> TokenStream { let defs = self.emit_defs(); - let default = Ident::from(self.entries[0].name.clone()); + let default = format_ident!("{}", self.entries[0].name); let enum_name = self.emit_name(); #[cfg(feature = "emit-description")] let description = if let Some(description) = self.description.as_ref() { - Ident::from(format!("/// {description}\n")) + let desc = format!("{}", description); + quote!(#[doc = #desc]) } else { - Ident::from("") + quote!() }; #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); let enum_def; if let Some(width) = self.bitfield.clone() { - let width = Ident::from(width); + let width = format_ident!("{}", width); enum_def = quote! { bitflags!{ #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -519,7 +536,7 @@ impl MavEnum { }; } - quote! { + dbg!(quote! { #enum_def impl Default for #enum_name { @@ -527,7 +544,7 @@ impl MavEnum { #enum_name::#default } } - } + }) } } @@ -552,12 +569,12 @@ pub struct MavMessage { impl MavMessage { /// Return Token of "MESSAGE_NAME_DATA /// for mavlink struct data - fn emit_struct_name(&self) -> Tokens { - let name = Ident::from(format!("{}_DATA", self.name)); + fn emit_struct_name(&self) -> TokenStream { + let name = format_ident!("{}", format!("{}_DATA", self.name)); quote!(#name) } - fn emit_name_types(&self) -> (Vec, usize) { + fn emit_name_types(&self) -> (Vec, usize) { let mut encoded_payload_len: usize = 0; let field_toks = self .fields @@ -570,15 +587,15 @@ impl MavMessage { let description = field.emit_description(); #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); // From MAVLink specification: // If sent by an implementation that doesn't have the extensions fields // then the recipient will see zero values for the extensions fields. let serde_default = if field.is_extension { - Ident::from(r#"#[cfg_attr(feature = "serde", serde(default))]"#) + quote!(#[cfg_attr(feature = "serde", serde(default))]) } else { - Ident::from("") + quote!() }; quote! { @@ -587,27 +604,29 @@ impl MavMessage { #nametype } }) - .collect::>(); + .collect::>(); (field_toks, encoded_payload_len) } /// Generate description for the given message #[cfg(feature = "emit-description")] - fn emit_description(&self) -> Tokens { - let mut desc = String::from(format!("\n/// id: {}\n", self.id)); + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); + let desc = format!("id: {}", self.id); + ts.extend(quote!(#[doc = #desc])); if let Some(val) = self.description.clone() { - desc = desc + &format!("/// {}.\n", val); + let doc = &format!("{}.", val); + ts.extend(quote!(#[doc = #doc])); } - let desc = Ident::from(desc); - quote!(#desc) + ts } - fn emit_serialize_vars(&self) -> Tokens { + fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self .fields .iter() .map(|f| f.rust_writer()) - .collect::>(); + .collect::>(); quote! { let mut _tmp = Vec::new(); #(#ser_vars)* @@ -615,14 +634,15 @@ impl MavMessage { } } - fn emit_deserialize_vars(&self) -> Tokens { + fn emit_deserialize_vars(&self) -> TokenStream { let deser_vars = self .fields .iter() .map(|f| f.rust_reader()) - .collect::>(); + .collect::>(); - let encoded_len_name = Ident::from(format!("{}_DATA::ENCODED_LEN", self.name)); + let i = format_ident!("{}_DATA", self.name); + let encoded_len_name = quote!(#i::ENCODED_LEN); if deser_vars.is_empty() { // struct has no fields @@ -651,7 +671,7 @@ impl MavMessage { } } - fn emit_rust(&self) -> Tokens { + fn emit_rust(&self) -> TokenStream { let msg_name = self.emit_struct_name(); let (name_types, msg_encoded_len) = self.emit_name_types(); @@ -662,7 +682,7 @@ impl MavMessage { let description = self.emit_description(); #[cfg(not(feature = "emit-description"))] - let description = Ident::from(""); + let description = quote!(); quote! { #description @@ -700,52 +720,53 @@ pub struct MavField { impl MavField { /// Emit rust name of a given field - fn emit_name(&self) -> Tokens { - let name = Ident::from(self.name.clone()); + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); quote!(#name) } /// Emit rust type of the field - fn emit_type(&self) -> Tokens { + fn emit_type(&self) -> TokenStream { let mavtype; match self.mavtype { MavType::Array(_, _) => { - mavtype = Ident::from(self.mavtype.rust_type()); + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); } _ => match self.enumtype { Some(ref enumname) => { - mavtype = Ident::from(enumname.clone()); + let en = TokenStream::from_str(enumname).unwrap(); + mavtype = quote!(#en); } _ => { - mavtype = Ident::from(self.mavtype.rust_type()); + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); } }, } - quote!(#mavtype) + mavtype } /// Generate description for the given field #[cfg(feature = "emit-description")] - fn emit_description(&self) -> Tokens { - let mut desc = Vec::new(); + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); if let Some(val) = self.description.clone() { - desc.push(format!("\n/// {}.", val)); + let desc = format!("{}.", val); + ts.extend(quote!(#[doc = #desc])); } - desc.push("\n".to_string()); - let desc: String = desc.iter().map(|s| s.to_string()).collect(); - let desc = Ident::from(desc); - quote!(#desc) + ts } /// Combine rust name and type of a given field - fn emit_name_type(&self) -> Tokens { + fn emit_name_type(&self) -> TokenStream { let name = self.emit_name(); let fieldtype = self.emit_type(); quote!(pub #name: #fieldtype,) } /// Emit writer - fn rust_writer(&self) -> Tokens { + fn rust_writer(&self) -> TokenStream { let mut name = "self.".to_string() + &self.name.clone(); if let Some(_) = &self.enumtype { if let Some(dsp) = &self.display { @@ -767,23 +788,24 @@ impl MavField { } } } - let name = Ident::from(name); - let buf = Ident::from("_tmp"); + let ts = TokenStream::from_str(&name).unwrap(); + let name = quote!(#ts); + let buf = format_ident!("_tmp"); self.mavtype.rust_writer(name, buf) } /// Emit reader - fn rust_reader(&self) -> Tokens { - let name = Ident::from("_struct.".to_string() + &self.name.clone()); - let buf = Ident::from("buf"); + fn rust_reader(&self) -> TokenStream { + let _name = TokenStream::from_str(&self.name).unwrap(); + + let name = quote!(_struct.#_name); + let buf = format_ident!("buf"); if let Some(enum_name) = &self.enumtype { if let Some(dsp) = &self.display { if dsp == "bitmask" { // bitflags - let tmp = self - .mavtype - .rust_reader(Ident::from("let tmp"), buf.clone()); - let enum_name_ident = Ident::from(enum_name.clone()); + let tmp = self.mavtype.rust_reader(quote!(let tmp), buf.clone()); + let enum_name_ident = format_ident!("{}", enum_name); quote! { #tmp #name = #enum_name_ident::from_bits(tmp & #enum_name_ident::all().bits()) @@ -800,10 +822,8 @@ impl MavField { _ => {} } // handle enum by FromPrimitive - let tmp = self - .mavtype - .rust_reader(Ident::from("let tmp"), buf.clone()); - let val = Ident::from("from_".to_string() + &self.mavtype.rust_type()); + let tmp = self.mavtype.rust_reader(quote!(let tmp), buf.clone()); + let val = format_ident!("from_{}", &self.mavtype.rust_type()); quote!( #tmp #name = FromPrimitive::#val(tmp) @@ -871,7 +891,7 @@ impl MavType { } /// Emit reader of a given type - pub fn rust_reader(&self, val: Ident, buf: Ident) -> Tokens { + pub fn rust_reader(&self, val: TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; match self.clone() { Char => quote! {#val = #buf.get_u8();}, @@ -889,7 +909,7 @@ impl MavType { Array(t, size) => { if size > 32 { // it is a vector - let r = t.rust_reader(Ident::from("let val"), buf.clone()); + let r = t.rust_reader(quote!(let val), buf.clone()); quote! { for _ in 0..#size { #r @@ -898,7 +918,7 @@ impl MavType { } } else { // handle as a slice - let r = t.rust_reader(Ident::from("let val"), buf.clone()); + let r = t.rust_reader(quote!(let val), buf.clone()); quote! { for idx in 0..#size { #r @@ -911,7 +931,7 @@ impl MavType { } /// Emit writer of a given type - pub fn rust_writer(&self, val: Ident, buf: Ident) -> Tokens { + pub fn rust_writer(&self, val: TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; match self.clone() { UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, @@ -927,7 +947,7 @@ impl MavType { Int64 => quote! {#buf.put_i64_le(#val);}, Double => quote! {#buf.put_f64_le(#val);}, Array(t, _size) => { - let w = t.rust_writer(Ident::from("*val"), buf.clone()); + let w = t.rust_writer(quote!(*val), buf.clone()); quote! { for val in &#val { #w From bc64cd60c5198b6d6d2acf5f71bdd234674a5610 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Thu, 18 Aug 2022 09:10:34 +0200 Subject: [PATCH 016/159] Truncate MAVLink v2 messages The MAVLink documentation states the following[^1]: > *MAVLink 2* implementations *must* truncate any empty (zero-filled) > bytes at the end of the serialized payload before it is sent. So this PR implements that. [^1]: https://mavlink.io/en/guide/serialization.html#payload_truncation --- build/parser.rs | 11 +++++++---- src/lib.rs | 13 ++++++++----- src/utils.rs | 18 ++++++++++++++++++ 3 files changed, 33 insertions(+), 9 deletions(-) create mode 100644 src/utils.rs diff --git a/build/parser.rs b/build/parser.rs index cf275dfeea..4f5694ccd2 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -425,10 +425,10 @@ impl MavProfile { includes: &Vec, ) -> TokenStream { quote! { - fn ser(&self) -> Vec { + fn ser(&self, version: MavlinkVersion) -> Vec { match self { - #(&MavMessage::#enums(ref body) => body.ser(),)* - #(&MavMessage::#includes(ref msg) => msg.ser(),)* + #(&MavMessage::#enums(ref body) => body.ser(version),)* + #(&MavMessage::#includes(ref msg) => msg.ser(version),)* } } } @@ -630,6 +630,9 @@ impl MavMessage { quote! { let mut _tmp = Vec::new(); #(#ser_vars)* + if matches!(version, MavlinkVersion::V2) { + crate::remove_trailing_zeroes(&mut _tmp); + } _tmp } } @@ -699,7 +702,7 @@ impl MavMessage { #deser_vars } - pub fn ser(&self) -> Vec { + pub fn ser(&self, version: MavlinkVersion) -> Vec { #serialize_vars } } diff --git a/src/lib.rs b/src/lib.rs index 79f74c2cb6..87f41f5f8c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -42,6 +42,10 @@ mod connection; #[cfg(feature = "std")] pub use self::connection::{connect, MavConnection}; +mod utils; +#[allow(unused_imports)] +use utils::remove_trailing_zeroes; + #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; @@ -71,7 +75,7 @@ where { fn message_id(&self) -> u32; fn message_name(&self) -> &'static str; - fn ser(&self) -> Vec; + fn ser(&self, version: MavlinkVersion) -> Vec; fn parse( version: MavlinkVersion, @@ -159,9 +163,8 @@ impl MavFrame { v.push(self.msg.message_id() as u8); //TODO check } } - // serialize message - v.append(&mut self.msg.ser()); + v.append(&mut self.msg.ser(self.protocol_version)); v } @@ -451,7 +454,7 @@ pub fn write_v2_msg( data: &M, ) -> Result { let msgid = data.message_id(); - let payload = data.ser(); + let payload = data.ser(MavlinkVersion::V2); // println!("write payload_len : {}", payload.len()); let header = &[ @@ -497,7 +500,7 @@ pub fn write_v1_msg( data: &M, ) -> Result { let msgid = data.message_id(); - let payload = data.ser(); + let payload = data.ser(MavlinkVersion::V1); let header = &[ MAV_STX, diff --git a/src/utils.rs b/src/utils.rs new file mode 100644 index 0000000000..3de872dd98 --- /dev/null +++ b/src/utils.rs @@ -0,0 +1,18 @@ +#[cfg(not(feature = "std"))] +use alloc::vec::Vec; + +/// Removes the trailing zeroes in the payload +/// +/// # Note: +/// +/// There must always be at least one remaining byte even if it is a +/// zero byte. +#[allow(dead_code)] +pub(crate) fn remove_trailing_zeroes(buf: &mut Vec) { + while let Some(&0) = buf.last() { + if buf.len() <= 1 { + break; + } + buf.pop(); + } +} From 00f654b84c28754c8f018faf626e95286d8135df Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Thu, 21 Oct 2021 15:47:49 +0100 Subject: [PATCH 017/159] fix lints --- build/binder.rs | 2 +- build/main.rs | 55 ++++-------- build/parser.rs | 144 ++++++++++++++------------------ src/connection/direct_serial.rs | 9 +- src/connection/tcp.rs | 16 ++-- src/connection/udp.rs | 27 +++--- src/lib.rs | 36 ++++---- tests/direct_serial_tests.rs | 3 - tests/encode_decode_tests.rs | 9 +- tests/helper_tests.rs | 5 +- tests/process_log_files.rs | 3 - tests/tcp_loopback_tests.rs | 3 - tests/test_shared/mod.rs | 62 +++++++------- tests/udp_loopback_tests.rs | 3 - tests/v1_encode_decode_tests.rs | 17 ++-- tests/v2_encode_decode_tests.rs | 9 +- 16 files changed, 165 insertions(+), 238 deletions(-) diff --git a/build/binder.rs b/build/binder.rs index a31c36653e..1037b1e78a 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -1,4 +1,4 @@ -use quote::format_ident; +use quote::{format_ident, quote}; use std::io::Write; pub fn generate(modules: Vec, out: &mut W) { diff --git a/build/main.rs b/build/main.rs index 2feffdaadd..ac7a7568f0 100644 --- a/build/main.rs +++ b/build/main.rs @@ -1,8 +1,4 @@ #![recursion_limit = "256"] -#[macro_use] -extern crate quote; - -extern crate quick_xml; mod binder; mod parser; @@ -10,6 +6,7 @@ mod util; use crate::util::to_module_name; use std::env; +use std::ffi::OsStr; use std::fs::{read_dir, File}; use std::path::{Path, PathBuf}; use std::process::Command; @@ -18,15 +15,14 @@ pub fn main() { let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); // Update and init submodule - match Command::new("git") + if let Err(error) = Command::new("git") .arg("submodule") .arg("update") .arg("--init") .current_dir(&src_dir) .status() { - Ok(_) => {} - Err(error) => eprintln!("{}", error), + eprintln!("{}", error); } // find & apply patches to XML definitions to avoid crashes @@ -36,17 +32,14 @@ pub fn main() { mavlink_dir.push("mavlink"); if let Ok(dir) = read_dir(patch_dir) { - for entry in dir { - if let Ok(entry) = entry { - match Command::new("git") - .arg("apply") - .arg(entry.path().as_os_str()) - .current_dir(&mavlink_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } + for entry in dir.flatten() { + if let Err(error) = Command::new("git") + .arg("apply") + .arg(entry.path().as_os_str()) + .current_dir(&mavlink_dir) + .status() + { + eprintln!("{}", error); } } } @@ -77,16 +70,7 @@ pub fn main() { // generate code parser::generate(&mut inf, &mut outf); - - // format code - match Command::new("rustfmt") - .arg(dest_path.as_os_str()) - .current_dir(&out_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } + format_code(&out_dir, &dest_path); // Re-run build if definition file changes println!("cargo:rerun-if-changed={}", entry.path().to_string_lossy()); @@ -99,15 +83,12 @@ pub fn main() { // generate code binder::generate(modules, &mut outf); + format_code(out_dir, dest_path); + } +} - // format code - match Command::new("rustfmt") - .arg(dest_path.as_os_str()) - .current_dir(&out_dir) - .status() - { - Ok(_) => (), - Err(error) => eprintln!("{}", error), - } +fn format_code(cwd: impl AsRef, path: impl AsRef) { + if let Err(error) = Command::new("rustfmt").arg(path).current_dir(cwd).status() { + eprintln!("{}", error); } } diff --git a/build/parser.rs b/build/parser.rs index 4f5694ccd2..f7ee65b859 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -124,7 +124,7 @@ impl MavProfile { self.messages .iter() .map(|msg| { - let ec = extra_crc(&msg); + let ec = extra_crc(msg); quote!(#ec) }) .collect::>() @@ -210,9 +210,9 @@ impl MavProfile { &self, enums: &Vec, structs: &Vec, - includes: &Vec, + includes: &[Ident], ) -> TokenStream { - let includes = includes.into_iter().map(|include| { + let includes = includes.iter().map(|include| { quote! { #include(crate::#include::MavMessage) } @@ -228,8 +228,8 @@ impl MavProfile { } } - fn emit_mav_message_from_includes(&self, includes: &Vec) -> TokenStream { - let froms = includes.into_iter().map(|include| { + fn emit_mav_message_from_includes(&self, includes: &[Ident]) -> TokenStream { + let froms = includes.iter().map(|include| { quote! { impl From for MavMessage { fn from(message: crate::#include::MavMessage) -> Self { @@ -246,16 +246,16 @@ impl MavProfile { fn emit_mav_message_parse( &self, - enums: &Vec, - structs: &Vec, - ids: &Vec, - includes: &Vec, + enums: &[TokenStream], + structs: &[TokenStream], + ids: &[TokenStream], + includes: &[Ident], ) -> TokenStream { let id_width = format_ident!("u32"); // try parsing all included message variants if it doesn't land in the id // range for this message - let includes_branches = includes.into_iter().map(|i| { + let includes_branches = includes.iter().map(|i| { quote! { if let Ok(msg) = crate::#i::MavMessage::parse(version, id, payload) { return Ok(MavMessage::#i(msg)) @@ -279,11 +279,11 @@ impl MavProfile { fn emit_mav_message_crc( &self, id_width: &Ident, - ids: &Vec, - crc: &Vec, - includes: &Vec, + ids: &[TokenStream], + crc: &[TokenStream], + includes: &[Ident], ) -> TokenStream { - let includes_branch = includes.into_iter().map(|include| { + let includes_branch = includes.iter().map(|include| { quote! { match crate::#include::MavMessage::extra_crc(id) { 0 => {}, @@ -348,11 +348,11 @@ impl MavProfile { fn emit_mav_message_id_from_name( &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, + enums: &[TokenStream], + ids: &[TokenStream], + includes: &[Ident], ) -> TokenStream { - let includes_branch = includes.into_iter().map(|include| { + let includes_branch = includes.iter().map(|include| { quote! { match crate::#include::MavMessage::message_id_from_name(name) { Ok(name) => return Ok(name), @@ -385,9 +385,9 @@ impl MavProfile { fn emit_mav_message_default_from_id( &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, + enums: &[TokenStream], + ids: &[TokenStream], + includes: &[Ident], ) -> TokenStream { let data_name = enums .iter() @@ -397,7 +397,7 @@ impl MavProfile { }) .collect::>(); - let includes_branches = includes.into_iter().map(|include| { + let includes_branches = includes.iter().map(|include| { quote! { if let Ok(msg) = crate::#include::MavMessage::default_message_from_id(id) { return Ok(MavMessage::#include(msg)); @@ -435,7 +435,7 @@ impl MavProfile { } } -#[derive(Debug, PartialEq, Clone, Default)] +#[derive(Debug, PartialEq, Eq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavEnum { pub name: String, @@ -548,7 +548,7 @@ impl MavEnum { } } -#[derive(Debug, PartialEq, Clone, Default)] +#[derive(Debug, PartialEq, Eq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavEnumEntry { pub value: Option, @@ -771,7 +771,7 @@ impl MavField { /// Emit writer fn rust_writer(&self) -> TokenStream { let mut name = "self.".to_string() + &self.name.clone(); - if let Some(_) = &self.enumtype { + if self.enumtype.is_some() { if let Some(dsp) = &self.display { // potentially a bitflag if dsp == "bitmask" { @@ -807,7 +807,7 @@ impl MavField { if let Some(dsp) = &self.display { if dsp == "bitmask" { // bitflags - let tmp = self.mavtype.rust_reader(quote!(let tmp), buf.clone()); + let tmp = self.mavtype.rust_reader(quote!(let tmp), buf); let enum_name_ident = format_ident!("{}", enum_name); quote! { #tmp @@ -818,14 +818,11 @@ impl MavField { panic!("Display option not implemented"); } } else { - match &self.mavtype { - MavType::Array(_t, _size) => { - return self.mavtype.rust_reader(name, buf); - } - _ => {} + if let MavType::Array(_t, _size) = &self.mavtype { + return self.mavtype.rust_reader(name, buf); } // handle enum by FromPrimitive - let tmp = self.mavtype.rust_reader(quote!(let tmp), buf.clone()); + let tmp = self.mavtype.rust_reader(quote!(let tmp), buf); let val = format_ident!("from_{}", &self.mavtype.rust_type()); quote!( #tmp @@ -881,8 +878,8 @@ impl MavType { "Double" => Some(Double), "double" => Some(Double), _ => { - if s.ends_with("]") { - let start = s.find("[")?; + if s.ends_with(']') { + let start = s.find('[')?; let size = s[start + 1..(s.len() - 1)].parse::().ok()?; let mtype = MavType::parse_type(&s[0..start])?; Some(Array(Box::new(mtype), size)) @@ -912,7 +909,7 @@ impl MavType { Array(t, size) => { if size > 32 { // it is a vector - let r = t.rust_reader(quote!(let val), buf.clone()); + let r = t.rust_reader(quote!(let val), buf); quote! { for _ in 0..#size { #r @@ -921,7 +918,7 @@ impl MavType { } } else { // handle as a slice - let r = t.rust_reader(quote!(let val), buf.clone()); + let r = t.rust_reader(quote!(let val), buf); quote! { for idx in 0..#size { #r @@ -950,7 +947,7 @@ impl MavType { Int64 => quote! {#buf.put_i64_le(#val);}, Double => quote! {#buf.put_f64_le(#val);}, Array(t, _size) => { - let w = t.rust_writer(quote!(*val), buf.clone()); + let w = t.rust_writer(quote!(*val), buf); quote! { for val in &#val { #w @@ -1039,7 +1036,7 @@ impl MavType { } } -#[derive(Debug, PartialEq, Clone, Copy)] +#[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] pub enum MavXmlElement { @@ -1086,7 +1083,7 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { use self::MavXmlElement::*; match s { Version => p == Some(Mavlink), - Mavlink => p == None, + Mavlink => p.is_none(), Dialect => p == Some(Mavlink), Include => p == Some(Mavlink), Enums => p == Some(Mavlink), @@ -1142,7 +1139,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { for e in events { match e { Ok(Event::Start(bytes)) => { - let id = match identify_element(&bytes.name()) { + let id = match identify_element(bytes.name()) { None => { panic!( "unexpected element {:?}", @@ -1152,13 +1149,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { Some(kind) => kind, }; - if !is_valid_parent( - match stack.last().clone() { - Some(arg) => Some(arg.clone()), - None => None, - }, - id.clone(), - ) { + if !is_valid_parent(stack.last().copied(), id) { panic!("not valid parent {:?} of {:?}", stack.last(), id); } @@ -1193,8 +1184,8 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { for attr in bytes.attributes() { let attr = attr.unwrap(); match stack.last() { - Some(&MavXmlElement::Enum) => match attr.key { - b"name" => { + Some(&MavXmlElement::Enum) => { + if let b"name" = attr.key { mavenum.name = attr .value .clone() @@ -1208,8 +1199,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { .join(""); //mavenum.name = attr.value.clone(); } - _ => (), - }, + } Some(&MavXmlElement::Entry) => { match attr.key { b"name" => { @@ -1270,7 +1260,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } b"type" => { let s = std::str::from_utf8(&attr.value).unwrap(); - field.mavtype = MavType::parse_type(&s).unwrap(); + field.mavtype = MavType::parse_type(s).unwrap(); } b"enum" => { field.enumtype = Some( @@ -1295,15 +1285,12 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } Some(&MavXmlElement::Param) => { - if let None = entry.params { + if entry.params.is_none() { entry.params = Some(vec![]); } - match attr.key { - b"index" => { - let s = std::str::from_utf8(&attr.value).unwrap(); - paramid = Some(s.parse::().unwrap()); - } - _ => (), + if let b"index" = attr.key { + let s = std::str::from_utf8(&attr.value).unwrap(); + paramid = Some(s.parse::().unwrap()); } } _ => (), @@ -1339,16 +1326,16 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { use self::MavXmlElement::*; match (stack.last(), stack.get(stack.len() - 2)) { (Some(&Description), Some(&Message)) => { - message.description = Some(s.replace("\n", " ")); + message.description = Some(s.replace('\n', " ")); } (Some(&Field), Some(&Message)) => { - field.description = Some(s.replace("\n", " ")); + field.description = Some(s.replace('\n', " ")); } (Some(&Description), Some(&Enum)) => { - mavenum.description = Some(s.replace("\n", " ")); + mavenum.description = Some(s.replace('\n', " ")); } (Some(&Description), Some(&Entry)) => { - entry.description = Some(s.replace("\n", " ")); + entry.description = Some(s.replace('\n', " ")); } (Some(&Param), Some(&Entry)) => { if let Some(ref mut params) = entry.params { @@ -1363,7 +1350,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } (Some(&Include), Some(&Mavlink)) => { - include = s.replace("\n", ""); + include = s.replace('\n', ""); } (Some(&Version), Some(&Mavlink)) => { eprintln!("TODO: version {:?}", s); @@ -1511,7 +1498,7 @@ impl MavXmlFilter { Ok(content) => { match content { Event::Start(bytes) => { - let id = match identify_element(&bytes.name()) { + let id = match identify_element(bytes.name()) { None => { panic!( "unexpected element {:?}", @@ -1520,15 +1507,12 @@ impl MavXmlFilter { } Some(kind) => kind, }; - match id { - MavXmlElement::Extensions => { - self.extension_filter.is_in = true; - } - _ => {} + if let MavXmlElement::Extensions = id { + self.extension_filter.is_in = true; } } Event::Empty(bytes) => { - let id = match identify_element(&bytes.name()) { + let id = match identify_element(bytes.name()) { None => { panic!( "unexpected element {:?}", @@ -1537,15 +1521,12 @@ impl MavXmlFilter { } Some(kind) => kind, }; - match id { - MavXmlElement::Extensions => { - self.extension_filter.is_in = true; - } - _ => {} + if let MavXmlElement::Extensions = id { + self.extension_filter.is_in = true; } } Event::End(bytes) => { - let id = match identify_element(&bytes.name()) { + let id = match identify_element(bytes.name()) { None => { panic!( "unexpected element {:?}", @@ -1555,16 +1536,13 @@ impl MavXmlFilter { Some(kind) => kind, }; - match id { - MavXmlElement::Message => { - self.extension_filter.is_in = false; - } - _ => {} + if let MavXmlElement::Message = id { + self.extension_filter.is_in = false; } } _ => {} } - return !self.extension_filter.is_in; + !self.extension_filter.is_in } Err(error) => panic!("Failed to filter XML: {}", error), } diff --git a/src/connection/direct_serial.rs b/src/connection/direct_serial.rs index ab20df006d..c8d226e131 100644 --- a/src/connection/direct_serial.rs +++ b/src/connection/direct_serial.rs @@ -1,18 +1,15 @@ -extern crate serial; - use crate::connection::MavConnection; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io::{self}; +use std::io; use std::sync::Mutex; -//TODO why is this import so hairy? -use crate::connection::direct_serial::serial::prelude::*; use crate::error::{MessageReadError, MessageWriteError}; +use serial::prelude::*; /// Serial MAVLINK connection pub fn open(settings: &str) -> io::Result { - let settings_toks: Vec<&str> = settings.split(":").collect(); + let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { return Err(io::Error::new( io::ErrorKind::AddrNotAvailable, diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs index 89a109e86e..000c7300cc 100644 --- a/src/connection/tcp.rs +++ b/src/connection/tcp.rs @@ -11,16 +11,18 @@ use std::time::Duration; pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { - if address.starts_with("tcpout:") { - Ok(Box::new(tcpout(&address["tcpout:".len()..])?)) - } else if address.starts_with("tcpin:") { - Ok(Box::new(tcpin(&address["tcpin:".len()..])?)) + let connection = if let Some(address) = address.strip_prefix("tcpout:") { + tcpout(address) + } else if let Some(address) = address.strip_prefix("tcpin:") { + tcpin(address) } else { Err(io::Error::new( io::ErrorKind::AddrNotAvailable, "Protocol unsupported", )) - } + }; + + Ok(Box::new(connection?)) } pub fn tcpout(address: T) -> io::Result { @@ -35,7 +37,7 @@ pub fn tcpout(address: T) -> io::Result { Ok(TcpConnection { reader: Mutex::new(socket.try_clone()?), writer: Mutex::new(TcpWrite { - socket: socket, + socket, sequence: 0, }), protocol_version: MavlinkVersion::V2, @@ -57,7 +59,7 @@ pub fn tcpin(address: T) -> io::Result { return Ok(TcpConnection { reader: Mutex::new(socket.try_clone()?), writer: Mutex::new(TcpWrite { - socket: socket, + socket, sequence: 0, }), protocol_version: MavlinkVersion::V2, diff --git a/src/connection/udp.rs b/src/connection/udp.rs index ef41b78485..0fbe88f7c0 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -11,18 +11,20 @@ use std::sync::Mutex; pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { - if address.starts_with("udpin:") { - Ok(Box::new(udpin(&address["udpin:".len()..])?)) - } else if address.starts_with("udpout:") { - Ok(Box::new(udpout(&address["udpout:".len()..])?)) - } else if address.starts_with("udpbcast:") { - Ok(Box::new(udpbcast(&address["udpbcast:".len()..])?)) + let connection = if let Some(address) = address.strip_prefix("udpin:") { + udpin(address) + } else if let Some(address) = address.strip_prefix("udpout:") { + udpout(address) + } else if let Some(address) = address.strip_prefix("udpbcast:") { + udpbcast(address) } else { Err(io::Error::new( io::ErrorKind::AddrNotAvailable, "Protocol unsupported", )) - } + }; + + Ok(Box::new(connection?)) } pub fn udpbcast(address: T) -> io::Result { @@ -123,14 +125,14 @@ pub struct UdpConnection { impl UdpConnection { fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { Ok(UdpConnection { - server: server, + server, reader: Mutex::new(UdpRead { socket: socket.try_clone()?, recv_buf: PacketBuf::new(), }), writer: Mutex::new(UdpWrite { - socket: socket, - dest: dest, + socket, + dest, sequence: 0, }), protocol_version: MavlinkVersion::V2, @@ -152,9 +154,8 @@ impl MavConnection for UdpConnection { } } - match read_versioned_msg(&mut state.recv_buf, self.protocol_version) { - ok @ Ok(..) => return ok, - _ => (), + if let ok @ Ok(..) = read_versioned_msg(&mut state.recv_buf, self.protocol_version) { + return ok; } } } diff --git a/src/lib.rs b/src/lib.rs index 87f41f5f8c..cd309b1bcc 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -32,7 +32,6 @@ use core::result::Result; #[cfg(feature = "std")] use std::io::{Read, Write}; -extern crate byteorder; use byteorder::LittleEndian; #[cfg(feature = "std")] use byteorder::ReadBytesExt; @@ -49,14 +48,9 @@ use utils::remove_trailing_zeroes; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -extern crate bytes; use crate::error::ParserError; use bytes::{Buf, BytesMut}; -extern crate bitflags; -extern crate num_derive; -extern crate num_traits; - use crc_any::CRCu16; // include generate definitions @@ -146,12 +140,12 @@ impl MavFrame { /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. pub fn ser(&self) -> Vec { - let mut v = vec![]; - // serialize header - v.push(self.header.system_id); - v.push(self.header.component_id); - v.push(self.header.sequence); + let mut v = vec![ + self.header.system_id, + self.header.component_id, + self.header.sequence, + ]; // message id match self.protocol_version { @@ -228,11 +222,11 @@ pub struct MAVLinkV1MessageRaw { impl MAVLinkV1MessageRaw { pub fn payload(&self) -> &[u8] { - return &self.payload_buffer[..self.payload_length as usize]; + &self.payload_buffer[..self.payload_length as usize] } pub fn mut_payload(&mut self) -> &mut [u8] { - return &mut self.payload_buffer[..self.payload_length as usize]; + &mut self.payload_buffer[..self.payload_length as usize] } pub fn calculate_crc(&self) -> u16 { @@ -249,11 +243,11 @@ impl MAVLinkV1MessageRaw { let extra_crc = M::extra_crc(self.message_id.into()); crc_calculator.digest(&[extra_crc]); - return crc_calculator.get_crc(); + crc_calculator.get_crc() } pub fn has_valid_crc(&self) -> bool { - return self.checksum == self.calculate_crc::(); + self.checksum == self.calculate_crc::() } } @@ -285,7 +279,7 @@ pub fn read_v1_raw_message( message.checksum = reader.read_u16::()?; - return Ok(message); + Ok(message) } /// Read a MAVLink v1 message from a Read stream. @@ -336,11 +330,11 @@ pub struct MAVLinkV2MessageRaw { impl MAVLinkV2MessageRaw { pub fn payload(&self) -> &[u8] { - return &self.payload_buffer[..self.payload_length as usize]; + &self.payload_buffer[..self.payload_length as usize] } pub fn mut_payload(&mut self) -> &mut [u8] { - return &mut self.payload_buffer[..self.payload_length as usize]; + &mut self.payload_buffer[..self.payload_length as usize] } pub fn calculate_crc(&self) -> u16 { @@ -359,11 +353,11 @@ impl MAVLinkV2MessageRaw { let extra_crc = M::extra_crc(self.message_id); crc_calculator.digest(&[extra_crc]); - return crc_calculator.get_crc(); + crc_calculator.get_crc() } pub fn has_valid_crc(&self) -> bool { - return self.checksum == self.calculate_crc::(); + self.checksum == self.calculate_crc::() } } @@ -405,7 +399,7 @@ pub fn read_v2_raw_message( reader.read_exact(&mut sign)?; } - return Ok(message); + Ok(message) } /// Read a MAVLink v2 message from a Read stream. diff --git a/tests/direct_serial_tests.rs b/tests/direct_serial_tests.rs index 98aa617508..f52a25bf8d 100644 --- a/tests/direct_serial_tests.rs +++ b/tests/direct_serial_tests.rs @@ -1,6 +1,3 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "std", feature = "direct-serial", feature = "common"))] mod test_direct_serial { use mavlink::common::MavMessage; diff --git a/tests/encode_decode_tests.rs b/tests/encode_decode_tests.rs index 81adb22286..a8ffd928b3 100644 --- a/tests/encode_decode_tests.rs +++ b/tests/encode_decode_tests.rs @@ -1,8 +1,5 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(feature = "common")] mod test_encode_decode { use mavlink::{common, Message}; @@ -15,7 +12,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &common::MavMessage::HEARTBEAT(send_msg.clone()), + &common::MavMessage::HEARTBEAT(send_msg), ) .expect("Failed to write message"); @@ -33,7 +30,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::COMMAND_INT(send_msg.clone()), + &mavlink::common::MavMessage::COMMAND_INT(send_msg), ) .expect("Failed to write message"); @@ -55,7 +52,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(send_msg.clone()), + &mavlink::common::MavMessage::HIL_ACTUATOR_CONTROLS(send_msg), ) .expect("Failed to write message"); diff --git a/tests/helper_tests.rs b/tests/helper_tests.rs index e44de326de..6f37c08ff5 100644 --- a/tests/helper_tests.rs +++ b/tests/helper_tests.rs @@ -1,9 +1,6 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod helper_tests { - use crate::mavlink::{common::MavMessage, Message}; + use mavlink::{common::MavMessage, Message}; #[test] fn test_get_default_message_from_id() { diff --git a/tests/process_log_files.rs b/tests/process_log_files.rs index 876762b4b8..f0785761e6 100644 --- a/tests/process_log_files.rs +++ b/tests/process_log_files.rs @@ -1,6 +1,3 @@ -extern crate mavlink; - -#[cfg(test)] #[cfg(all(feature = "default", feature = "ardupilotmega"))] mod process_files { use mavlink::ardupilotmega::MavMessage; diff --git a/tests/tcp_loopback_tests.rs b/tests/tcp_loopback_tests.rs index cf4d92942f..606aa2aaa3 100644 --- a/tests/tcp_loopback_tests.rs +++ b/tests/tcp_loopback_tests.rs @@ -1,8 +1,5 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "tcp", feature = "common"))] mod test_tcp_connections { use std::thread; diff --git a/tests/test_shared/mod.rs b/tests/test_shared/mod.rs index 35ba39a52e..49fda509c5 100644 --- a/tests/test_shared/mod.rs +++ b/tests/test_shared/mod.rs @@ -1,5 +1,3 @@ -extern crate mavlink; - pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { sequence: 239, system_id: 1, @@ -43,8 +41,8 @@ pub fn get_cmd_nav_takeoff_msg() -> mavlink::common::COMMAND_INT_DATA { #[cfg(feature = "common")] pub fn get_hil_actuator_controls_msg() -> mavlink::common::HIL_ACTUATOR_CONTROLS_DATA { mavlink::common::HIL_ACTUATOR_CONTROLS_DATA { - time_usec: 1234567 as u64, - flags: 0 as u64, + time_usec: 1234567_u64, + flags: 0_u64, controls: [ 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0, ], @@ -57,40 +55,40 @@ pub fn get_hil_actuator_controls_msg() -> mavlink::common::HIL_ACTUATOR_CONTROLS #[cfg(all(feature = "common", not(feature = "emit-extensions")))] pub fn get_servo_output_raw_v1() -> mavlink::common::SERVO_OUTPUT_RAW_DATA { mavlink::common::SERVO_OUTPUT_RAW_DATA { - time_usec: 1234567 as u32, - servo1_raw: 1100 as u16, - servo2_raw: 1200 as u16, - servo3_raw: 1300 as u16, - servo4_raw: 1400 as u16, - servo5_raw: 1500 as u16, - servo6_raw: 1600 as u16, - servo7_raw: 1700 as u16, - servo8_raw: 1800 as u16, - port: 123 as u8, + time_usec: 1234567_u32, + servo1_raw: 1100_u16, + servo2_raw: 1200_u16, + servo3_raw: 1300_u16, + servo4_raw: 1400_u16, + servo5_raw: 1500_u16, + servo6_raw: 1600_u16, + servo7_raw: 1700_u16, + servo8_raw: 1800_u16, + port: 123_u8, } } #[cfg(all(feature = "common", feature = "emit-extensions"))] pub fn get_servo_output_raw_v2() -> mavlink::common::SERVO_OUTPUT_RAW_DATA { mavlink::common::SERVO_OUTPUT_RAW_DATA { - time_usec: 1234567 as u32, - servo1_raw: 1100 as u16, - servo2_raw: 1200 as u16, - servo3_raw: 1300 as u16, - servo4_raw: 1400 as u16, - servo5_raw: 1500 as u16, - servo6_raw: 1600 as u16, - servo7_raw: 1700 as u16, - servo8_raw: 1800 as u16, - port: 123 as u8, - servo9_raw: 1110 as u16, - servo10_raw: 1220 as u16, - servo11_raw: 1330 as u16, - servo12_raw: 1440 as u16, - servo13_raw: 1550 as u16, - servo14_raw: 1660 as u16, - servo15_raw: 1770 as u16, - servo16_raw: 1880 as u16, + time_usec: 1234567_u32, + servo1_raw: 1100_u16, + servo2_raw: 1200_u16, + servo3_raw: 1300_u16, + servo4_raw: 1400_u16, + servo5_raw: 1500_u16, + servo6_raw: 1600_u16, + servo7_raw: 1700_u16, + servo8_raw: 1800_u16, + port: 123_u8, + servo9_raw: 1110_u16, + servo10_raw: 1220_u16, + servo11_raw: 1330_u16, + servo12_raw: 1440_u16, + servo13_raw: 1550_u16, + servo14_raw: 1660_u16, + servo15_raw: 1770_u16, + servo16_raw: 1880_u16, } } diff --git a/tests/udp_loopback_tests.rs b/tests/udp_loopback_tests.rs index 0d1b191cfa..a884014f5c 100644 --- a/tests/udp_loopback_tests.rs +++ b/tests/udp_loopback_tests.rs @@ -1,8 +1,5 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "udp", feature = "common"))] mod test_udp_connections { use std::thread; diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index ad3addc167..83fab1d2e7 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -1,12 +1,9 @@ -extern crate mavlink; - pub mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod test_v1_encode_decode { - pub const HEARTBEAT_V1: &'static [u8] = &[ + pub const HEARTBEAT_V1: &[u8] = &[ mavlink::MAV_STX, 0x09, 0xef, @@ -54,7 +51,7 @@ mod test_v1_encode_decode { mavlink::write_v1_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg.clone()), + &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg), ) .expect("Failed to write message"); @@ -64,7 +61,7 @@ mod test_v1_encode_decode { #[test] #[cfg(not(feature = "emit-extensions"))] pub fn test_echo_servo_output_raw() { - use mavlink::{common, Message}; + use mavlink::Message; let mut v = vec![]; let send_msg = crate::test_shared::get_servo_output_raw_v1(); @@ -72,7 +69,7 @@ mod test_v1_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::SERVO_OUTPUT_RAW(send_msg.clone()), + &mavlink::common::MavMessage::SERVO_OUTPUT_RAW(send_msg), ) .expect("Failed to write message"); @@ -82,12 +79,12 @@ mod test_v1_encode_decode { assert_eq!( mavlink::common::MavMessage::extra_crc(recv_msg.message_id()), - 222 as u8 + 222_u8 ); if let mavlink::common::MavMessage::SERVO_OUTPUT_RAW(recv_msg) = recv_msg { - assert_eq!(recv_msg.port, 123 as u8); - assert_eq!(recv_msg.servo4_raw, 1400 as u16); + assert_eq!(recv_msg.port, 123_u8); + assert_eq!(recv_msg.servo4_raw, 1400_u16); } else { panic!("Decoded wrong message type") } diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index b6cfd8b2fa..109fdc9694 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -1,11 +1,8 @@ -extern crate mavlink; - mod test_shared; -#[cfg(test)] #[cfg(all(feature = "std", feature = "common"))] mod test_v2_encode_decode { - pub const HEARTBEAT_V2: &'static [u8] = &[ + pub const HEARTBEAT_V2: &[u8] = &[ mavlink::MAV_STX_V2, //magic 0x09, //payload len 0, //incompat flags @@ -56,7 +53,7 @@ mod test_v2_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg.clone()), + &mavlink::common::MavMessage::HEARTBEAT(heartbeat_msg), ) .expect("Failed to write message"); @@ -64,7 +61,7 @@ mod test_v2_encode_decode { } /// A COMMAND_LONG message with a truncated payload (allowed for empty fields) - pub const COMMAND_LONG_TRUNCATED_V2: &'static [u8] = &[ + pub const COMMAND_LONG_TRUNCATED_V2: &[u8] = &[ mavlink::MAV_STX_V2, 30, 0, From 86ac9f9fead980cfed7bb7aa0e1ae0f4511e79f3 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Tue, 30 Aug 2022 12:32:52 +0200 Subject: [PATCH 018/159] update quick-xml to 0.24 supersedes #130 --- Cargo.toml | 2 +- build/parser.rs | 32 ++++++++++++++++---------------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 90fe4ce3cd..32e79bf5d0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ edition = "2018" [build-dependencies] crc-any = "2.3.0" bytes = { version = "1.0", default-features = false } -quick-xml = "0.23" +quick-xml = "0.24" quote = "1" proc-macro2 = "1.0.43" lazy_static = "1.2.0" diff --git a/build/parser.rs b/build/parser.rs index f7ee65b859..dd99564cd6 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -1124,7 +1124,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { let mut buf = Vec::new(); loop { - match reader.read_event(&mut buf) { + match reader.read_event_into(&mut buf) { Ok(Event::Eof) => { events.push(Ok(Event::Eof)); break; @@ -1139,11 +1139,11 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { for e in events { match e { Ok(Event::Start(bytes)) => { - let id = match identify_element(bytes.name()) { + let id = match identify_element(&bytes.name().into_inner()) { None => { panic!( "unexpected element {:?}", - String::from_utf8_lossy(bytes.name()) + String::from_utf8_lossy(bytes.name().into_inner()) ); } Some(kind) => kind, @@ -1185,7 +1185,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { let attr = attr.unwrap(); match stack.last() { Some(&MavXmlElement::Enum) => { - if let b"name" = attr.key { + if let b"name" = attr.key.into_inner() { mavenum.name = attr .value .clone() @@ -1201,7 +1201,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } Some(&MavXmlElement::Entry) => { - match attr.key { + match attr.key.into_inner() { b"name" => { let name = String::from_utf8(attr.value.to_vec()).unwrap(); entry.name = name; @@ -1225,7 +1225,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } Some(&MavXmlElement::Message) => { - match attr.key { + match attr.key.into_inner() { b"name" => { /*message.name = attr .value @@ -1250,7 +1250,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } Some(&MavXmlElement::Field) => { - match attr.key { + match attr.key.into_inner() { b"name" => { let name = String::from_utf8(attr.value.to_vec()).unwrap(); field.name = name; @@ -1288,7 +1288,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { if entry.params.is_none() { entry.params = Some(vec![]); } - if let b"index" = attr.key { + if let b"index" = attr.key.into_inner() { let s = std::str::from_utf8(&attr.value).unwrap(); paramid = Some(s.parse::().unwrap()); } @@ -1297,7 +1297,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { } } } - Ok(Event::Empty(bytes)) => match bytes.name() { + Ok(Event::Empty(bytes)) => match bytes.name().into_inner() { b"extensions" => { is_in_extension = true; } @@ -1305,7 +1305,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { entry = Default::default(); for attr in bytes.attributes() { let attr = attr.unwrap(); - match attr.key { + match attr.key.into_inner() { b"name" => { entry.name = String::from_utf8(attr.value.to_vec()).unwrap(); } @@ -1498,11 +1498,11 @@ impl MavXmlFilter { Ok(content) => { match content { Event::Start(bytes) => { - let id = match identify_element(bytes.name()) { + let id = match identify_element(bytes.name().into_inner()) { None => { panic!( "unexpected element {:?}", - String::from_utf8_lossy(bytes.name()) + String::from_utf8_lossy(bytes.name().into_inner()) ); } Some(kind) => kind, @@ -1512,11 +1512,11 @@ impl MavXmlFilter { } } Event::Empty(bytes) => { - let id = match identify_element(bytes.name()) { + let id = match identify_element(bytes.name().into_inner()) { None => { panic!( "unexpected element {:?}", - String::from_utf8_lossy(bytes.name()) + String::from_utf8_lossy(bytes.name().into_inner()) ); } Some(kind) => kind, @@ -1526,11 +1526,11 @@ impl MavXmlFilter { } } Event::End(bytes) => { - let id = match identify_element(bytes.name()) { + let id = match identify_element(bytes.name().into_inner()) { None => { panic!( "unexpected element {:?}", - String::from_utf8_lossy(bytes.name()) + String::from_utf8_lossy(bytes.name().into_inner()) ); } Some(kind) => kind, From 9c24ab395dff46370de5d2d53bc68fb8c398801f Mon Sep 17 00:00:00 2001 From: Marcus Loo Vergara Date: Wed, 7 Sep 2022 13:16:29 +0200 Subject: [PATCH 019/159] Flattened mavlink definitions in generated dialect files --- build/main.rs | 9 +- build/parser.rs | 235 +++++++++++++++++++----------------------------- 2 files changed, 98 insertions(+), 146 deletions(-) diff --git a/build/main.rs b/build/main.rs index ac7a7568f0..095ba1632d 100644 --- a/build/main.rs +++ b/build/main.rs @@ -62,14 +62,15 @@ pub fn main() { modules.push(module_name); - let in_path = Path::new(&definitions_dir).join(&definition_file); - let mut inf = File::open(&in_path).unwrap(); - let dest_path = Path::new(&out_dir).join(definition_rs); let mut outf = File::create(&dest_path).unwrap(); // generate code - parser::generate(&mut inf, &mut outf); + parser::generate( + &definitions_dir, + &definition_file.into_string().unwrap(), + &mut outf, + ); format_code(&out_dir, &dest_path); // Re-run build if definition file changes diff --git a/build/parser.rs b/build/parser.rs index dd99564cd6..addb7c8422 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -1,34 +1,62 @@ use crc_any::CRCu16; use std::cmp::Ordering; +use std::collections::hash_map::Entry; +use std::collections::{HashMap, HashSet}; use std::default::Default; -use std::io::{BufRead, BufReader, Read, Write}; +use std::fs::File; +use std::io::{BufReader, Write}; +use std::path::{Path, PathBuf}; use std::str::FromStr; use std::u32; -//use xml::reader::{EventReader, XmlEvent}; use quick_xml::{events::Event, Reader}; use proc_macro2::{Ident, TokenStream}; use quote::{format_ident, quote}; -use crate::util::to_module_name; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; #[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavProfile { - pub includes: Vec, - pub messages: Vec, - pub enums: Vec, + pub messages: HashMap, + pub enums: HashMap, } impl MavProfile { + fn add_message(&mut self, message: &MavMessage) { + match self.messages.entry(message.name.clone()) { + Entry::Occupied(entry) => { + if entry.get() != message { + panic!( + "Message '{}' defined twice but definitions are different", + message.name + ); + } + } + Entry::Vacant(entry) => { + entry.insert(message.clone()); + } + } + } + + fn add_enum(&mut self, enm: &MavEnum) { + match self.enums.entry(enm.name.clone()) { + Entry::Occupied(entry) => { + entry.into_mut().try_combine(enm); + } + Entry::Vacant(entry) => { + entry.insert(enm.clone()); + } + } + } + /// Go over all fields in the messages, and if you encounter an enum, /// update this enum with information about whether it is a bitmask, and what /// is the desired width of such. fn update_enums(mut self) -> Self { - for msg in &self.messages { + for msg in self.messages.values() { for field in &msg.fields { if let Some(ref enum_name) = field.enumtype { // it is an enum @@ -36,7 +64,7 @@ impl MavProfile { // it is a bitmask if dsp == "bitmask" { // find the corresponding enum - for mut enm in &mut self.enums { + for mut enm in self.enums.values_mut() { if enm.name == *enum_name { // this is the right enum enm.bitfield = Some(field.mavtype.rust_type()); @@ -65,19 +93,11 @@ impl MavProfile { quote!(#![doc = "This file was automatically generated, do not edit"]) } - /// Emit includes - fn emit_includes(&self) -> Vec { - self.includes - .iter() - .map(|i| format_ident!("{}", to_module_name(i))) - .collect::>() - } - /// Emit rust messages fn emit_msgs(&self) -> Vec { self.messages .iter() - .map(|d| d.emit_rust()) + .map(|(_, d)| d.emit_rust()) .collect::>() } @@ -85,7 +105,7 @@ impl MavProfile { fn emit_enums(&self) -> Vec { self.enums .iter() - .map(|d| d.emit_rust()) + .map(|(_, d)| d.emit_rust()) .collect::>() } @@ -93,7 +113,7 @@ impl MavProfile { fn emit_enum_names(&self) -> Vec { self.messages .iter() - .map(|msg| { + .map(|(_, msg)| { let name = format_ident!("{}", msg.name); quote!(#name) }) @@ -104,7 +124,7 @@ impl MavProfile { fn emit_struct_names(&self) -> Vec { self.messages .iter() - .map(|msg| msg.emit_struct_name()) + .map(|(_, msg)| msg.emit_struct_name()) .collect::>() } @@ -112,7 +132,7 @@ impl MavProfile { fn emit_msg_ids(&self) -> Vec { self.messages .iter() - .map(|msg| { + .map(|(_, msg)| { let msg_id = msg.id; quote!(#msg_id) }) @@ -123,7 +143,7 @@ impl MavProfile { fn emit_msg_crc(&self) -> Vec { self.messages .iter() - .map(|msg| { + .map(|(_, msg)| { let ec = extra_crc(msg); quote!(#ec) }) @@ -136,25 +156,21 @@ impl MavProfile { let comment = self.emit_comments(); let msgs = self.emit_msgs(); - let includes = self.emit_includes(); let enum_names = self.emit_enum_names(); let struct_names = self.emit_struct_names(); let enums = self.emit_enums(); let msg_ids = self.emit_msg_ids(); let msg_crc = self.emit_msg_crc(); - let mav_message = self.emit_mav_message(&enum_names, &struct_names, &includes); - let mav_message_from_includes = self.emit_mav_message_from_includes(&includes); - let mav_message_parse = - self.emit_mav_message_parse(&enum_names, &struct_names, &msg_ids, &includes); - let mav_message_crc = self.emit_mav_message_crc(&id_width, &msg_ids, &msg_crc, &includes); - let mav_message_name = self.emit_mav_message_name(&enum_names, &includes); - let mav_message_id = self.emit_mav_message_id(&enum_names, &msg_ids, &includes); - let mav_message_id_from_name = - self.emit_mav_message_id_from_name(&enum_names, &msg_ids, &includes); + let mav_message = self.emit_mav_message(&enum_names, &struct_names); + let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names, &msg_ids); + let mav_message_crc = self.emit_mav_message_crc(&id_width, &msg_ids, &msg_crc); + let mav_message_name = self.emit_mav_message_name(&enum_names); + let mav_message_id = self.emit_mav_message_id(&enum_names, &msg_ids); + let mav_message_id_from_name = self.emit_mav_message_id_from_name(&enum_names, &msg_ids); let mav_message_default_from_id = - self.emit_mav_message_default_from_id(&enum_names, &msg_ids, &includes); - let mav_message_serialize = self.emit_mav_message_serialize(&enum_names, &includes); + self.emit_mav_message_default_from_id(&enum_names, &msg_ids); + let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); quote! { #comment @@ -173,8 +189,6 @@ impl MavProfile { use bitflags::bitflags; use crate::{Message, error::*}; - #[allow(unused_imports)] - use crate::{#(#includes::*),*}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -192,8 +206,6 @@ impl MavProfile { #[derive(Clone, PartialEq, Debug)] #mav_message - #mav_message_from_includes - impl Message for MavMessage { #mav_message_parse #mav_message_name @@ -210,37 +222,13 @@ impl MavProfile { &self, enums: &Vec, structs: &Vec, - includes: &[Ident], ) -> TokenStream { - let includes = includes.iter().map(|include| { - quote! { - #include(crate::#include::MavMessage) - } - }); - quote! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] pub enum MavMessage { #(#enums(#structs),)* - #(#includes,)* - } - } - } - - fn emit_mav_message_from_includes(&self, includes: &[Ident]) -> TokenStream { - let froms = includes.iter().map(|include| { - quote! { - impl From for MavMessage { - fn from(message: crate::#include::MavMessage) -> Self { - MavMessage::#include(message) - } - } } - }); - - quote! { - #(#froms)* } } @@ -249,26 +237,14 @@ impl MavProfile { enums: &[TokenStream], structs: &[TokenStream], ids: &[TokenStream], - includes: &[Ident], ) -> TokenStream { let id_width = format_ident!("u32"); - // try parsing all included message variants if it doesn't land in the id - // range for this message - let includes_branches = includes.iter().map(|i| { - quote! { - if let Ok(msg) = crate::#i::MavMessage::parse(version, id, payload) { - return Ok(MavMessage::#i(msg)) - } - } - }); - quote! { fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { #(#ids => #structs::deser(version, payload).map(|s| MavMessage::#enums(s)),)* _ => { - #(#includes_branches)* Err(ParserError::UnknownMessage { id }) }, } @@ -281,24 +257,12 @@ impl MavProfile { id_width: &Ident, ids: &[TokenStream], crc: &[TokenStream], - includes: &[Ident], ) -> TokenStream { - let includes_branch = includes.iter().map(|include| { - quote! { - match crate::#include::MavMessage::extra_crc(id) { - 0 => {}, - any => return any - } - } - }); - quote! { fn extra_crc(id: #id_width) -> u8 { match id { #(#ids => #crc,)* _ => { - #(#includes_branch)* - 0 }, } @@ -306,11 +270,7 @@ impl MavProfile { } } - fn emit_mav_message_name( - &self, - enums: &Vec, - includes: &Vec, - ) -> TokenStream { + fn emit_mav_message_name(&self, enums: &Vec) -> TokenStream { let enum_names = enums .iter() .map(|enum_name| { @@ -323,24 +283,17 @@ impl MavProfile { fn message_name(&self) -> &'static str { match self { #(MavMessage::#enums(..) => #enum_names,)* - #(MavMessage::#includes(msg) => msg.message_name(),)* } } } } - fn emit_mav_message_id( - &self, - enums: &Vec, - ids: &Vec, - includes: &Vec, - ) -> TokenStream { + fn emit_mav_message_id(&self, enums: &Vec, ids: &Vec) -> TokenStream { let id_width = format_ident!("u32"); quote! { fn message_id(&self) -> #id_width { match self { #(MavMessage::#enums(..) => #ids,)* - #(MavMessage::#includes(msg) => msg.message_id(),)* } } } @@ -350,17 +303,7 @@ impl MavProfile { &self, enums: &[TokenStream], ids: &[TokenStream], - includes: &[Ident], ) -> TokenStream { - let includes_branch = includes.iter().map(|include| { - quote! { - match crate::#include::MavMessage::message_id_from_name(name) { - Ok(name) => return Ok(name), - Err(..) => {} - } - } - }); - let enum_names = enums .iter() .map(|enum_name| { @@ -374,8 +317,6 @@ impl MavProfile { match name { #(#enum_names => Ok(#ids),)* _ => { - #(#includes_branch)* - Err("Invalid message name.") } } @@ -387,7 +328,6 @@ impl MavProfile { &self, enums: &[TokenStream], ids: &[TokenStream], - includes: &[Ident], ) -> TokenStream { let data_name = enums .iter() @@ -397,21 +337,11 @@ impl MavProfile { }) .collect::>(); - let includes_branches = includes.iter().map(|include| { - quote! { - if let Ok(msg) = crate::#include::MavMessage::default_message_from_id(id) { - return Ok(MavMessage::#include(msg)); - } - } - }); - quote! { fn default_message_from_id(id: u32) -> Result { match id { #(#ids => Ok(MavMessage::#enums(#data_name::default())),)* _ => { - #(#includes_branches)* - return Err("Invalid message id."); } } @@ -419,16 +349,11 @@ impl MavProfile { } } - fn emit_mav_message_serialize( - &self, - enums: &Vec, - includes: &Vec, - ) -> TokenStream { + fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { fn ser(&self, version: MavlinkVersion) -> Vec { match self { #(&MavMessage::#enums(ref body) => body.ser(version),)* - #(&MavMessage::#includes(ref msg) => msg.ser(version),)* } } } @@ -446,6 +371,20 @@ pub struct MavEnum { } impl MavEnum { + fn try_combine(&mut self, enm: &MavEnum) { + if self.name == enm.name { + for enum_entry in &enm.entries { + let found_entry = self.entries.iter().find(|elem| { + elem.name == enum_entry.name && elem.value.unwrap() == enum_entry.value.unwrap() + }); + match found_entry { + Some(entry) => panic!("Enum entry {} already exists", entry.name), + None => self.entries.push(enum_entry.clone()), + } + } + } + } + fn has_enum_values(&self) -> bool { self.entries.iter().all(|x| x.value.is_some()) } @@ -1100,15 +1039,17 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { } } -pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { - let mut stack: Vec = vec![]; +pub fn parse_profile( + definitions_dir: &Path, + definition_file: &String, + parsed_files: &mut HashSet, +) -> MavProfile { + let in_path = Path::new(&definitions_dir).join(&definition_file); + parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed - let mut profile = MavProfile { - includes: vec![], - messages: vec![], - enums: vec![], - }; + let mut stack: Vec = vec![]; + let mut profile = MavProfile::default(); let mut field = MavField::default(); let mut message = MavMessage::default(); let mut mavenum = MavEnum::default(); @@ -1118,7 +1059,7 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { let mut xml_filter = MavXmlFilter::default(); let mut events: Vec> = Vec::new(); - let mut reader = Reader::from_reader(file); + let mut reader = Reader::from_reader(BufReader::new(File::open(in_path).unwrap())); reader.trim_text(true); reader.trim_text_end(true); @@ -1390,13 +1331,23 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { msg.fields.extend(not_extension_fields); msg.fields.extend(extension_fields); - profile.messages.push(msg); + profile.add_message(&msg); } Some(&MavXmlElement::Enum) => { - profile.enums.push(mavenum.clone()); + profile.add_enum(&mavenum); } Some(&MavXmlElement::Include) => { - profile.includes.push(include.clone()); + let include_file = Path::new(&definitions_dir).join(include.clone()); + if !parsed_files.contains(&include_file) { + let included_profile = + parse_profile(definitions_dir, &include, parsed_files); + for message in included_profile.messages.values() { + profile.add_message(message); + } + for enm in included_profile.enums.values() { + profile.add_enum(enm); + } + } } _ => (), } @@ -1417,9 +1368,9 @@ pub fn parse_profile(file: &mut dyn BufRead) -> MavProfile { /// Generate protobuf represenation of mavlink message set /// Generate rust representation of mavlink message set with appropriate conversion methods -pub fn generate(input: &mut R, output_rust: &mut W) { - let mut br = BufReader::new(input); - let profile = parse_profile(&mut br); +pub fn generate(definitions_dir: &Path, definition_file: &String, output_rust: &mut W) { + let mut parsed_files: HashSet = HashSet::new(); + let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files); // rust file let rust_tokens = profile.emit_rust(); From d042708716d0cec1f430b530c67a510777b336f3 Mon Sep 17 00:00:00 2001 From: Marcus Loo Vergara Date: Wed, 7 Sep 2022 13:29:37 +0200 Subject: [PATCH 020/159] Updated example and test code to use flattened API --- src/bin/mavlink-dump.rs | 52 +++++++++++++++++++----------------- tests/encode_decode_tests.rs | 34 ++++++++++------------- 2 files changed, 41 insertions(+), 45 deletions(-) diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index badb18b1bd..ec68c572c8 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -31,16 +31,16 @@ fn main() { let vehicle = Arc::new(mavconn); vehicle - .send(&mavlink::MavHeader::default(), &request_parameters().into()) + .send(&mavlink::MavHeader::default(), &request_parameters()) .unwrap(); vehicle - .send(&mavlink::MavHeader::default(), &request_stream().into()) + .send(&mavlink::MavHeader::default(), &request_stream()) .unwrap(); thread::spawn({ let vehicle = vehicle.clone(); move || loop { - let res = vehicle.send_default(&heartbeat_message().into()); + let res = vehicle.send_default(&heartbeat_message()); if res.is_ok() { thread::sleep(Duration::from_secs(1)); } else { @@ -73,38 +73,40 @@ fn main() { } } -/// Create a heartbeat message using ardupilotmega dialect -/// If only common dialect is used, the `ardupilotmega::MavMessage::common` is not necessary, -/// and the function could return only a simple `mavlink::common::MavMessage` type +/// Create a heartbeat message using 'ardupilotmega' dialect #[cfg(feature = "std")] -pub fn heartbeat_message() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::HEARTBEAT(mavlink::common::HEARTBEAT_DATA { +pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::HEARTBEAT(mavlink::ardupilotmega::HEARTBEAT_DATA { custom_mode: 0, - mavtype: mavlink::common::MavType::MAV_TYPE_QUADROTOR, - autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, - base_mode: mavlink::common::MavModeFlag::empty(), - system_status: mavlink::common::MavState::MAV_STATE_STANDBY, + mavtype: mavlink::ardupilotmega::MavType::MAV_TYPE_QUADROTOR, + autopilot: mavlink::ardupilotmega::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode: mavlink::ardupilotmega::MavModeFlag::empty(), + system_status: mavlink::ardupilotmega::MavState::MAV_STATE_STANDBY, mavlink_version: 0x3, }) } /// Create a message requesting the parameters list #[cfg(feature = "std")] -pub fn request_parameters() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::PARAM_REQUEST_LIST(mavlink::common::PARAM_REQUEST_LIST_DATA { - target_system: 0, - target_component: 0, - }) +pub fn request_parameters() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::PARAM_REQUEST_LIST( + mavlink::ardupilotmega::PARAM_REQUEST_LIST_DATA { + target_system: 0, + target_component: 0, + }, + ) } /// Create a message enabling data streaming #[cfg(feature = "std")] -pub fn request_stream() -> mavlink::common::MavMessage { - mavlink::common::MavMessage::REQUEST_DATA_STREAM(mavlink::common::REQUEST_DATA_STREAM_DATA { - target_system: 0, - target_component: 0, - req_stream_id: 0, - req_message_rate: 10, - start_stop: 1, - }) +pub fn request_stream() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::REQUEST_DATA_STREAM( + mavlink::ardupilotmega::REQUEST_DATA_STREAM_DATA { + target_system: 0, + target_component: 0, + req_stream_id: 0, + req_message_rate: 10, + start_stop: 1, + }, + ) } diff --git a/tests/encode_decode_tests.rs b/tests/encode_decode_tests.rs index a8ffd928b3..12bac4a396 100644 --- a/tests/encode_decode_tests.rs +++ b/tests/encode_decode_tests.rs @@ -81,22 +81,19 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::common(common::MavMessage::HEARTBEAT(send_msg.clone())), + &mavlink::common::MavMessage::HEARTBEAT(send_msg.clone()), ) .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 (_header, recv_msg) = mavlink::read_v2_msg::(&mut c) + .expect("Failed to read"); - if let ardupilotmega::MavMessage::common(recv_msg) = recv_msg { - match &recv_msg { - common::MavMessage::HEARTBEAT(data) => { - assert_eq!(recv_msg.message_id(), 0); - } - _ => panic!("Decoded wrong message type"), + match &recv_msg { + ardupilotmega::MavMessage::HEARTBEAT(_data) => { + assert_eq!(recv_msg.message_id(), 0); } - } else { - panic!("Decoded wrong message type") + _ => panic!("Decoded wrong message type"), } } @@ -138,22 +135,19 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::common(common::MavMessage::COMMAND_INT(send_msg.clone())), + &common::MavMessage::COMMAND_INT(send_msg.clone()), ) .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 (_header, recv_msg) = mavlink::read_v2_msg::(&mut c) + .expect("Failed to read"); - if let ardupilotmega::MavMessage::common(recv_msg) = recv_msg { - match &recv_msg { - common::MavMessage::COMMAND_INT(data) => { - assert_eq!(data.command, common::MavCmd::MAV_CMD_NAV_TAKEOFF); - } - _ => panic!("Decoded wrong message type"), + match &recv_msg { + ardupilotmega::MavMessage::COMMAND_INT(data) => { + assert_eq!(data.command, ardupilotmega::MavCmd::MAV_CMD_NAV_TAKEOFF); } - } else { - panic!("Decoded wrong message type") + _ => panic!("Decoded wrong message type"), } } } From e4d8a4bd0144c4b81df330940f7d190f7aa0ffe7 Mon Sep 17 00:00:00 2001 From: Marcus Loo Vergara Date: Wed, 7 Sep 2022 13:33:09 +0200 Subject: [PATCH 021/159] Updated CI to run `cargo test` on each dialect individually --- .github/workflows/test.yml | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 1a0bafea63..e59808c90e 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -15,7 +15,15 @@ jobs: - name: Check Type run: cargo fmt -- --check - name: Run internal tests - run: cargo test --verbose --features all-dialects -- --nocapture + run: | + dialects=("ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common") + for dialect in "${dialects[@]}"; do + echo "::group::Testing $dialect" + if ! cargo test --verbose --features "$dialect" -- --nocapture; then + echo "::error::Tests failed" + fi + echo "::endgroup::" + done - name: Build mavlink-dump run: cargo build --verbose --bin mavlink-dump --features ardupilotmega @@ -28,7 +36,7 @@ jobs: include: - os: macos-latest TARGET: x86_64-apple-darwin - FEATURES: --features all-dialects + FEATURES: --features ardupilotmega - os: ubuntu-latest TARGET: arm-unknown-linux-musleabihf @@ -40,11 +48,11 @@ jobs: - os: ubuntu-latest TARGET: x86_64-unknown-linux-musl - FLAGS: --features all-dialects + FLAGS: --features ardupilotmega - os: ubuntu-latest TARGET: x86_64-unknown-linux-musl - FLAGS: --features all-dialects,emit-description,emit-extensions + FLAGS: --features ardupilotmega,emit-description,emit-extensions - os: ubuntu-latest TARGET: thumbv7m-none-eabi @@ -52,7 +60,7 @@ jobs: - os: windows-latest TARGET: x86_64-pc-windows-msvc - FLAGS: --features all-dialects + FLAGS: --features ardupilotmega steps: - name: Building ${{ matrix.TARGET }} From 23d45b8b768b5347715ed047913126901a9978ca Mon Sep 17 00:00:00 2001 From: Marcus Loo Vergara Date: Wed, 7 Sep 2022 13:43:34 +0200 Subject: [PATCH 022/159] Changed default dialect to `ardupilotmega` --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 32e79bf5d0..834f955d50 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -75,7 +75,7 @@ nb = { version = "1.0", optional = true } "tcp" = [] "direct-serial" = [] "embedded" = ["embedded-hal", "nb"] -default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "common"] +default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"] # build with all features on docs.rs so that users viewing documentation # can see everything From 6937b0f39aeaf24be8f8d1d2e235608928d56166 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 24 Oct 2022 08:09:30 +0000 Subject: [PATCH 023/159] build(deps): update quick-xml requirement from 0.24 to 0.26 Updates the requirements on [quick-xml](https://github.com/tafia/quick-xml) to permit the latest version. - [Release notes](https://github.com/tafia/quick-xml/releases) - [Changelog](https://github.com/tafia/quick-xml/blob/master/Changelog.md) - [Commits](https://github.com/tafia/quick-xml/compare/v0.24.0...v0.26.0) --- updated-dependencies: - dependency-name: quick-xml dependency-type: direct:production ... Signed-off-by: dependabot[bot] --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 834f955d50..729d8f12be 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ edition = "2018" [build-dependencies] crc-any = "2.3.0" bytes = { version = "1.0", default-features = false } -quick-xml = "0.24" +quick-xml = "0.26" quote = "1" proc-macro2 = "1.0.43" lazy_static = "1.2.0" From af67e5e6a1f5b6cd203505f96fcd708496fcdcc2 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Tue, 23 Aug 2022 06:41:22 +0100 Subject: [PATCH 024/159] refactor and add clippy and rustfmt CI targets --- .github/workflows/lint.yml | 37 +++++++ build/binder.rs | 4 +- build/main.rs | 2 +- build/parser.rs | 204 ++++++++++++++--------------------- src/bin/mavlink-dump.rs | 25 ++--- src/connection/file.rs | 5 +- src/connection/tcp.rs | 4 +- src/connection/udp.rs | 2 +- tests/encode_decode_tests.rs | 6 +- tests/helper_tests.rs | 5 +- tests/process_log_files.rs | 11 +- tests/tcp_loopback_tests.rs | 13 +-- tests/test_shared/mod.rs | 2 + tests/udp_loopback_tests.rs | 13 +-- 14 files changed, 154 insertions(+), 179 deletions(-) create mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 0000000000..f73d1868df --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,37 @@ +name: Lint + +on: [push, pull_request] + +jobs: + + fmt: + name: format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions-rs/toolchain@v1 + with: + toolchain: nightly + override: true + profile: minimal + components: rustfmt + - uses: actions-rs/cargo@v1 + with: + command: fmt + args: --all -- --check + + clippy: + name: lint + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions-rs/toolchain@v1 + with: + toolchain: nightly + override: true + profile: minimal + components: clippy + - uses: actions-rs/clippy-check@v1 + with: + token: ${{ secrets.GITHUB_TOKEN }} + args: --all-targets diff --git a/build/binder.rs b/build/binder.rs index 1037b1e78a..cb9e6024fa 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -7,9 +7,9 @@ pub fn generate(modules: Vec, out: &mut W) { quote! { #[allow(non_camel_case_types)] + #[allow(clippy::derive_partial_eq_without_eq)] + #[allow(clippy::field_reassign_with_default)] #[allow(non_snake_case)] - #[allow(unused_variables)] - #[allow(unused_mut)] #[cfg(feature = #module)] pub mod #module_ident; } diff --git a/build/main.rs b/build/main.rs index 095ba1632d..a32f33e6d5 100644 --- a/build/main.rs +++ b/build/main.rs @@ -19,7 +19,7 @@ pub fn main() { .arg("submodule") .arg("update") .arg("--init") - .current_dir(&src_dir) + .current_dir(src_dir) .status() { eprintln!("{}", error); diff --git a/build/parser.rs b/build/parser.rs index addb7c8422..6d56b03bee 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -95,59 +95,53 @@ impl MavProfile { /// Emit rust messages fn emit_msgs(&self) -> Vec { - self.messages - .iter() - .map(|(_, d)| d.emit_rust()) - .collect::>() + self.messages.values().map(|d| d.emit_rust()).collect() } /// Emit rust enums fn emit_enums(&self) -> Vec { - self.enums - .iter() - .map(|(_, d)| d.emit_rust()) - .collect::>() + self.enums.values().map(|d| d.emit_rust()).collect() } /// Get list of original message names fn emit_enum_names(&self) -> Vec { self.messages - .iter() - .map(|(_, msg)| { + .values() + .map(|msg| { let name = format_ident!("{}", msg.name); quote!(#name) }) - .collect::>() + .collect() } /// Emit message names with "_DATA" at the end fn emit_struct_names(&self) -> Vec { self.messages - .iter() - .map(|(_, msg)| msg.emit_struct_name()) - .collect::>() + .values() + .map(|msg| msg.emit_struct_name()) + .collect() } /// A list of message IDs fn emit_msg_ids(&self) -> Vec { self.messages - .iter() - .map(|(_, msg)| { + .values() + .map(|msg| { let msg_id = msg.id; quote!(#msg_id) }) - .collect::>() + .collect() } /// CRC values needed for mavlink parsing fn emit_msg_crc(&self) -> Vec { self.messages - .iter() - .map(|(_, msg)| { + .values() + .map(|msg| { let ec = extra_crc(msg); quote!(#ec) }) - .collect::>() + .collect() } fn emit_rust(&self) -> TokenStream { @@ -243,7 +237,7 @@ impl MavProfile { quote! { fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { - #(#ids => #structs::deser(version, payload).map(|s| MavMessage::#enums(s)),)* + #(#ids => #structs::deser(version, payload).map(MavMessage::#enums),)* _ => { Err(ParserError::UnknownMessage { id }) }, @@ -271,13 +265,10 @@ impl MavProfile { } fn emit_mav_message_name(&self, enums: &Vec) -> TokenStream { - let enum_names = enums - .iter() - .map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }) - .collect::>(); + let enum_names = enums.iter().map(|enum_name| { + let name = enum_name.to_string(); + quote!(#name) + }); quote! { fn message_name(&self) -> &'static str { @@ -304,13 +295,10 @@ impl MavProfile { enums: &[TokenStream], ids: &[TokenStream], ) -> TokenStream { - let enum_names = enums - .iter() - .map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }) - .collect::>(); + let enum_names = enums.iter().map(|enum_name| { + let name = enum_name.to_string(); + quote!(#name) + }); quote! { fn message_id_from_name(name: &str) -> Result { @@ -329,20 +317,17 @@ impl MavProfile { enums: &[TokenStream], ids: &[TokenStream], ) -> TokenStream { - let data_name = enums - .iter() - .map(|enum_name| { - let name = format_ident!("{}_DATA", enum_name.to_string()); - quote!(#name) - }) - .collect::>(); + let data_name = enums.iter().map(|enum_name| { + let name = format_ident!("{}_DATA", enum_name.to_string()); + quote!(#name) + }); quote! { fn default_message_from_id(id: u32) -> Result { match id { #(#ids => Ok(MavMessage::#enums(#data_name::default())),)* _ => { - return Err("Invalid message id."); + Err("Invalid message id.") } } } @@ -352,8 +337,8 @@ impl MavProfile { fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { fn ser(&self, version: MavlinkVersion) -> Vec { - match self { - #(&MavMessage::#enums(ref body) => body.ser(version),)* + match *self { + #(MavMessage::#enums(ref body) => body.ser(version),)* } } } @@ -371,7 +356,7 @@ pub struct MavEnum { } impl MavEnum { - fn try_combine(&mut self, enm: &MavEnum) { + fn try_combine(&mut self, enm: &Self) { if self.name == enm.name { for enum_entry in &enm.entries { let found_entry = self.entries.iter().find(|elem| { @@ -427,7 +412,7 @@ impl MavEnum { } } }) - .collect::>() + .collect() } fn emit_name(&self) -> TokenStream { @@ -480,7 +465,7 @@ impl MavEnum { impl Default for #enum_name { fn default() -> Self { - #enum_name::#default + Self::#default } } }) @@ -561,11 +546,7 @@ impl MavMessage { } fn emit_serialize_vars(&self) -> TokenStream { - let ser_vars = self - .fields - .iter() - .map(|f| f.rust_writer()) - .collect::>(); + let ser_vars = self.fields.iter().map(|f| f.rust_writer()); quote! { let mut _tmp = Vec::new(); #(#ser_vars)* @@ -583,9 +564,6 @@ impl MavMessage { .map(|f| f.rust_reader()) .collect::>(); - let i = format_ident!("{}_DATA", self.name); - let encoded_len_name = quote!(#i::ENCODED_LEN); - if deser_vars.is_empty() { // struct has no fields quote! { @@ -599,9 +577,9 @@ impl MavMessage { let mut buf = BytesMut::from(_input); // handle payload length truncuation due to empty fields - if avail_len < #encoded_len_name { + if avail_len < Self::ENCODED_LEN { //copy available bytes into an oversized buffer filled with zeros - let mut payload_buf = [0; #encoded_len_name]; + let mut payload_buf = [0; Self::ENCODED_LEN]; payload_buf[0..avail_len].copy_from_slice(_input); buf = BytesMut::from(&payload_buf[..]); } @@ -637,7 +615,7 @@ impl MavMessage { impl #msg_name { pub const ENCODED_LEN: usize = #msg_encoded_len; - pub fn deser(version: MavlinkVersion, _input: &[u8]) -> Result { + pub fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { #deser_vars } @@ -670,21 +648,15 @@ impl MavField { /// Emit rust type of the field fn emit_type(&self) -> TokenStream { let mavtype; - match self.mavtype { - MavType::Array(_, _) => { - let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); - mavtype = quote!(#rt); - } - _ => match self.enumtype { - Some(ref enumname) => { - let en = TokenStream::from_str(enumname).unwrap(); - mavtype = quote!(#en); - } - _ => { - let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); - mavtype = quote!(#rt); - } - }, + if matches!(self.mavtype, MavType::Array(_, _)) { + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); + } else if let Some(ref enumname) = self.enumtype { + let en = TokenStream::from_str(enumname).unwrap(); + mavtype = quote!(#en); + } else { + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); } mavtype } @@ -719,21 +691,18 @@ impl MavField { } else { panic!("Display option not implemented"); } - } else { - match self.mavtype { - MavType::Array(_, _) => {} // cast are not necessary for arrays - _ => { - // an enum, have to use "*foo as u8" cast - name += " as "; - name += &self.mavtype.rust_type(); - } - } + } + // cast are not necessary for arrays + else if !matches!(self.mavtype, MavType::Array(_, _)) { + // an enum, have to use "*foo as u8" cast + name += " as "; + name += &self.mavtype.rust_type(); } } let ts = TokenStream::from_str(&name).unwrap(); let name = quote!(#ts); let buf = format_ident!("_tmp"); - self.mavtype.rust_writer(name, buf) + self.mavtype.rust_writer(&name, buf) } /// Emit reader @@ -746,7 +715,7 @@ impl MavField { if let Some(dsp) = &self.display { if dsp == "bitmask" { // bitflags - let tmp = self.mavtype.rust_reader(quote!(let tmp), buf); + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); let enum_name_ident = format_ident!("{}", enum_name); quote! { #tmp @@ -758,10 +727,10 @@ impl MavField { } } else { if let MavType::Array(_t, _size) = &self.mavtype { - return self.mavtype.rust_reader(name, buf); + return self.mavtype.rust_reader(&name, buf); } // handle enum by FromPrimitive - let tmp = self.mavtype.rust_reader(quote!(let tmp), buf); + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); let val = format_ident!("from_{}", &self.mavtype.rust_type()); quote!( #tmp @@ -770,7 +739,7 @@ impl MavField { ) } } else { - self.mavtype.rust_reader(name, buf) + self.mavtype.rust_reader(&name, buf) } } } @@ -794,13 +763,13 @@ pub enum MavType { } impl Default for MavType { - fn default() -> MavType { - MavType::UInt8 + fn default() -> Self { + Self::UInt8 } } impl MavType { - fn parse_type(s: &str) -> Option { + fn parse_type(s: &str) -> Option { use self::MavType::*; match s { "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion), @@ -820,7 +789,7 @@ impl MavType { if s.ends_with(']') { let start = s.find('[')?; let size = s[start + 1..(s.len() - 1)].parse::().ok()?; - let mtype = MavType::parse_type(&s[0..start])?; + let mtype = Self::parse_type(&s[0..start])?; Some(Array(Box::new(mtype), size)) } else { None @@ -830,7 +799,7 @@ impl MavType { } /// Emit reader of a given type - pub fn rust_reader(&self, val: TokenStream, buf: Ident) -> TokenStream { + pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; match self.clone() { Char => quote! {#val = #buf.get_u8();}, @@ -848,7 +817,7 @@ impl MavType { Array(t, size) => { if size > 32 { // it is a vector - let r = t.rust_reader(quote!(let val), buf); + let r = t.rust_reader("e!(let val), buf); quote! { for _ in 0..#size { #r @@ -857,7 +826,7 @@ impl MavType { } } else { // handle as a slice - let r = t.rust_reader(quote!(let val), buf); + let r = t.rust_reader("e!(let val), buf); quote! { for idx in 0..#size { #r @@ -870,7 +839,7 @@ impl MavType { } /// Emit writer of a given type - pub fn rust_writer(&self, val: TokenStream, buf: Ident) -> TokenStream { + pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; match self.clone() { UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, @@ -886,7 +855,7 @@ impl MavType { Int64 => quote! {#buf.put_i64_le(#val);}, Double => quote! {#buf.put_f64_le(#val);}, Array(t, _size) => { - let w = t.rust_writer(quote!(*val), buf); + let w = t.rust_writer("e!(*val), buf); quote! { for val in &#val { #w @@ -1044,7 +1013,7 @@ pub fn parse_profile( definition_file: &String, parsed_files: &mut HashSet, ) -> MavProfile { - let in_path = Path::new(&definitions_dir).join(&definition_file); + let in_path = Path::new(&definitions_dir).join(definition_file); parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed let mut stack: Vec = vec![]; @@ -1080,7 +1049,7 @@ pub fn parse_profile( for e in events { match e { Ok(Event::Start(bytes)) => { - let id = match identify_element(&bytes.name().into_inner()) { + let id = match identify_element(bytes.name().into_inner()) { None => { panic!( "unexpected element {:?}", @@ -1090,9 +1059,12 @@ pub fn parse_profile( Some(kind) => kind, }; - if !is_valid_parent(stack.last().copied(), id) { - panic!("not valid parent {:?} of {:?}", stack.last(), id); - } + assert!( + is_valid_parent(stack.last().copied(), id), + "not valid parent {:?} of {:?}", + stack.last(), + id + ); match id { MavXmlElement::Extensions => { @@ -1136,8 +1108,7 @@ pub fn parse_profile( v[0] = v[0].to_ascii_uppercase(); String::from_utf8(v).unwrap() }) - .collect::>() - .join(""); + .collect(); //mavenum.name = attr.value.clone(); } } @@ -1213,8 +1184,7 @@ pub fn parse_profile( v[0] = v[0].to_ascii_uppercase(); String::from_utf8(v).unwrap() }) - .collect::>() - .join(""), + .collect(), ); //field.enumtype = Some(attr.value.clone()); } @@ -1423,8 +1393,8 @@ struct MavXmlFilter { } impl Default for MavXmlFilter { - fn default() -> MavXmlFilter { - MavXmlFilter { + fn default() -> Self { + Self { #[cfg(not(feature = "emit-extensions"))] extension_filter: ExtensionFilter { is_in: false }, } @@ -1439,7 +1409,7 @@ impl MavXmlFilter { #[cfg(feature = "emit-extensions")] pub fn filter_extension(&mut self, _element: &Result) -> bool { - return true; + true } /// Ignore extension fields @@ -1448,21 +1418,7 @@ impl MavXmlFilter { match element { Ok(content) => { match content { - Event::Start(bytes) => { - let id = match identify_element(bytes.name().into_inner()) { - None => { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - } - Some(kind) => kind, - }; - if let MavXmlElement::Extensions = id { - self.extension_filter.is_in = true; - } - } - Event::Empty(bytes) => { + Event::Start(bytes) | Event::Empty(bytes) => { let id = match identify_element(bytes.name().into_inner()) { None => { panic!( diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index ec68c572c8..845538f442 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -1,12 +1,6 @@ use mavlink::error::MessageReadError; #[cfg(feature = "std")] -use std::env; -#[cfg(feature = "std")] -use std::sync::Arc; -#[cfg(feature = "std")] -use std::thread; -#[cfg(feature = "std")] -use std::time::Duration; +use std::{env, sync::Arc, thread, time::Duration}; #[cfg(not(feature = "std"))] fn main() {} @@ -55,16 +49,13 @@ fn main() { println!("received: {:?}", msg); } Err(MessageReadError::Io(e)) => { - match e.kind() { - std::io::ErrorKind::WouldBlock => { - //no messages currently available to receive -- wait a while - thread::sleep(Duration::from_secs(1)); - continue; - } - _ => { - println!("recv error: {:?}", e); - break; - } + if let std::io::ErrorKind::WouldBlock = e.kind() { + //no messages currently available to receive -- wait a while + thread::sleep(Duration::from_secs(1)); + continue; + } else { + println!("recv error: {:?}", e); + break; } } // messages that didn't get through due to parser errors are ignored diff --git a/src/connection/file.rs b/src/connection/file.rs index 777f1b280c..5972c4446c 100644 --- a/src/connection/file.rs +++ b/src/connection/file.rs @@ -8,10 +8,7 @@ use std::sync::Mutex; /// File MAVLINK connection pub fn open(file_path: &str) -> io::Result { - let file = match File::open(&file_path) { - Err(e) => return Err(e), - Ok(file) => file, - }; + let file = File::open(file_path)?; Ok(FileConnection { file: Mutex::new(file), diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs index 000c7300cc..547bdeca0b 100644 --- a/src/connection/tcp.rs +++ b/src/connection/tcp.rs @@ -31,7 +31,7 @@ pub fn tcpout(address: T) -> io::Result { .unwrap() .next() .expect("Host address lookup failed."); - let socket = TcpStream::connect(&addr)?; + let socket = TcpStream::connect(addr)?; socket.set_read_timeout(Some(Duration::from_millis(100)))?; Ok(TcpConnection { @@ -50,7 +50,7 @@ pub fn tcpin(address: T) -> io::Result { .unwrap() .next() .expect("Invalid address"); - let listener = TcpListener::bind(&addr)?; + let listener = TcpListener::bind(addr)?; //For now we only accept one incoming stream: this blocks until we get one for incoming in listener.incoming() { diff --git a/src/connection/udp.rs b/src/connection/udp.rs index 0fbe88f7c0..adf3b2d4d3 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -56,7 +56,7 @@ pub fn udpin(address: T) -> io::Result { .unwrap() .next() .expect("Invalid address"); - let socket = UdpSocket::bind(&addr)?; + let socket = UdpSocket::bind(addr)?; UdpConnection::new(socket, true, None) } diff --git a/tests/encode_decode_tests.rs b/tests/encode_decode_tests.rs index 12bac4a396..f4b380074e 100644 --- a/tests/encode_decode_tests.rs +++ b/tests/encode_decode_tests.rs @@ -81,7 +81,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &mavlink::common::MavMessage::HEARTBEAT(send_msg.clone()), + &mavlink::common::MavMessage::HEARTBEAT(send_msg), ) .expect("Failed to write message"); @@ -111,7 +111,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &ardupilotmega::MavMessage::MOUNT_STATUS(send_msg.clone()), + &ardupilotmega::MavMessage::MOUNT_STATUS(send_msg), ) .expect("Failed to write message"); @@ -135,7 +135,7 @@ mod test_encode_decode { mavlink::write_v2_msg( &mut v, crate::test_shared::COMMON_MSG_HEADER, - &common::MavMessage::COMMAND_INT(send_msg.clone()), + &common::MavMessage::COMMAND_INT(send_msg), ) .expect("Failed to write message"); diff --git a/tests/helper_tests.rs b/tests/helper_tests.rs index 6f37c08ff5..2a36c6abca 100644 --- a/tests/helper_tests.rs +++ b/tests/helper_tests.rs @@ -10,9 +10,8 @@ mod helper_tests { let id = id.unwrap(); assert!(id == 4, "Invalid id for message name: PING"); let message = MavMessage::default_message_from_id(id); - match message { - Ok(MavMessage::PING(_)) => {} - _ => unreachable!("Invalid message type."), + if !matches!(message, Ok(MavMessage::PING(_))) { + unreachable!("Invalid message type.") } assert!( message.unwrap().message_name() == message_name, diff --git a/tests/process_log_files.rs b/tests/process_log_files.rs index f0785761e6..f45d9f8d02 100644 --- a/tests/process_log_files.rs +++ b/tests/process_log_files.rs @@ -25,7 +25,7 @@ mod process_files { } pub fn process_file(connection_string: &str) { - let vehicle = mavlink::connect::(&connection_string); + let vehicle = mavlink::connect::(connection_string); assert!(vehicle.is_ok(), "Incomplete address should error"); let vehicle = vehicle.unwrap(); @@ -35,15 +35,14 @@ mod process_files { Ok((_header, _msg)) => { counter += 1; } - Err(MessageReadError::Io(e)) => match e.kind() { - std::io::ErrorKind::WouldBlock => { + Err(MessageReadError::Io(e)) => { + if let std::io::ErrorKind::WouldBlock = e.kind() { continue; - } - _ => { + } else { println!("recv error: {:?}", e); break; } - }, + } _ => {} } } diff --git a/tests/tcp_loopback_tests.rs b/tests/tcp_loopback_tests.rs index 606aa2aaa3..abea99598b 100644 --- a/tests/tcp_loopback_tests.rs +++ b/tests/tcp_loopback_tests.rs @@ -17,14 +17,11 @@ mod test_tcp_connections { for _i in 0..RECEIVE_CHECK_COUNT { match server.recv() { Ok((_header, msg)) => { - match msg { - mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) => { - recv_count += 1; - } - _ => { - // one message parse failure fails the test - break; - } + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; } } Err(..) => { diff --git a/tests/test_shared/mod.rs b/tests/test_shared/mod.rs index 49fda509c5..4532b6bd40 100644 --- a/tests/test_shared/mod.rs +++ b/tests/test_shared/mod.rs @@ -1,3 +1,5 @@ +#![allow(unused)] + pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { sequence: 239, system_id: 1, diff --git a/tests/udp_loopback_tests.rs b/tests/udp_loopback_tests.rs index a884014f5c..fe366fcb4e 100644 --- a/tests/udp_loopback_tests.rs +++ b/tests/udp_loopback_tests.rs @@ -29,14 +29,11 @@ mod test_udp_connections { for _i in 0..RECEIVE_CHECK_COUNT { match server.recv() { Ok((_header, msg)) => { - match msg { - mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) => { - recv_count += 1; - } - _ => { - // one message parse failure fails the test - break; - } + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; } } Err(..) => { From fd5a139118b193b7f2c6c0f463837b6ae4aa4d8f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Mon, 14 Nov 2022 14:56:39 +0400 Subject: [PATCH 025/159] Remove alloc dependency --- Cargo.toml | 2 +- build/parser.rs | 43 ++++++------ src/bytes.rs | 159 ++++++++++++++++++++++++++++++++++++++++++++ src/bytes_mut.rs | 167 +++++++++++++++++++++++++++++++++++++++++++++++ src/error.rs | 7 +- src/lib.rs | 24 +++---- src/utils.rs | 17 +++-- 7 files changed, 368 insertions(+), 51 deletions(-) create mode 100644 src/bytes.rs create mode 100644 src/bytes_mut.rs diff --git a/Cargo.toml b/Cargo.toml index 729d8f12be..55fc13da19 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -25,7 +25,6 @@ required-features = ["ardupilotmega"] [dependencies] crc-any = "2.3.5" -bytes = { version = "1.0", default-features = false } num-traits = { version = "0.2", default-features = false } num-derive = "0.3.2" bitflags = "1.2.1" @@ -34,6 +33,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } embedded-hal = { version = "0.2", optional = true } nb = { version = "1.0", optional = true } +heapless = { version = "0.7", features = ["serde"] } [features] "ardupilotmega" = ["common", "icarous", "uavionix"] diff --git a/build/parser.rs b/build/parser.rs index 6d56b03bee..a24a32ecaa 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -170,8 +170,6 @@ impl MavProfile { #comment use crate::MavlinkVersion; #[allow(unused_imports)] - use bytes::{Buf, BufMut, Bytes, BytesMut}; - #[allow(unused_imports)] use num_derive::FromPrimitive; #[allow(unused_imports)] use num_traits::FromPrimitive; @@ -181,18 +179,14 @@ impl MavProfile { use num_traits::ToPrimitive; #[allow(unused_imports)] use bitflags::bitflags; + #[allow(unused_imports)] + use heapless::Vec; - use crate::{Message, error::*}; + use crate::{Message, error::*, MAX_FRAME_SIZE, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; - #[cfg(not(feature = "std"))] - use alloc::vec::Vec; - - #[cfg(not(feature = "std"))] - use alloc::string::ToString; - #(#enums)* #(#msgs)* @@ -336,7 +330,7 @@ impl MavProfile { fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { - fn ser(&self, version: MavlinkVersion) -> Vec { + fn ser(&self, version: MavlinkVersion) -> BytesMut { match *self { #(MavMessage::#enums(ref body) => body.ser(version),)* } @@ -548,7 +542,7 @@ impl MavMessage { fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self.fields.iter().map(|f| f.rust_writer()); quote! { - let mut _tmp = Vec::new(); + let mut _tmp = BytesMut::new(); #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { crate::remove_trailing_zeroes(&mut _tmp); @@ -573,16 +567,15 @@ impl MavMessage { quote! { let avail_len = _input.len(); - // fast zero copy - let mut buf = BytesMut::from(_input); - - // handle payload length truncuation due to empty fields - if avail_len < Self::ENCODED_LEN { + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { //copy available bytes into an oversized buffer filled with zeros - let mut payload_buf = [0; Self::ENCODED_LEN]; payload_buf[0..avail_len].copy_from_slice(_input); - buf = BytesMut::from(&payload_buf[..]); - } + Bytes::new(&payload_buf) + } else { + // fast zero copy + Bytes::new(_input) + }; let mut _struct = Self::default(); #(#deser_vars)* @@ -619,7 +612,7 @@ impl MavMessage { #deser_vars } - pub fn ser(&self, version: MavlinkVersion) -> Vec { + pub fn ser(&self, version: MavlinkVersion) -> BytesMut { #serialize_vars } } @@ -720,7 +713,7 @@ impl MavField { quote! { #tmp #name = #enum_name_ident::from_bits(tmp & #enum_name_ident::all().bits()) - .ok_or(ParserError::InvalidFlag { flag_type: #enum_name.to_string(), value: tmp as u32 })?; + .ok_or(ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u32 })?; } } else { panic!("Display option not implemented"); @@ -735,7 +728,7 @@ impl MavField { quote!( #tmp #name = FromPrimitive::#val(tmp) - .ok_or(ParserError::InvalidEnum { enum_type: #enum_name.to_string(), value: tmp as u32 })?; + .ok_or(ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u32 })?; ) } } else { @@ -821,7 +814,7 @@ impl MavType { quote! { for _ in 0..#size { #r - #val.push(val); + #val.push(val).unwrap(); } } } else { @@ -926,13 +919,15 @@ impl MavType { Int64 => "i64".into(), Double => "f64".into(), Array(t, size) => { + // format!("[{};{}]", t.rust_type(), size) if size > 32 { // we have to use a vector to make our lives easier - format!("Vec<{}> /* {} elements */", t.rust_type(), size) + format!("Vec<{}, {}> /* {} elements */", t.rust_type(), size, size) } else { // we can use a slice, as Rust derives lot of thinsg for slices <= 32 elements format!("[{};{}]", t.rust_type(), size) } + } } } diff --git a/src/bytes.rs b/src/bytes.rs new file mode 100644 index 0000000000..573a5ede32 --- /dev/null +++ b/src/bytes.rs @@ -0,0 +1,159 @@ +pub struct Bytes<'a> { + data: &'a [u8], + pos: usize, +} + +impl<'a> Bytes<'a> { + pub fn new(data: &'a [u8]) -> Self { + Self { + data, + pos: 0 + } + } + + #[inline] + fn remaining(&self) -> usize { + self.data.len() - self.pos + } + + pub fn remaining_bytes(&self) -> &'a [u8] { + &self.data[self.pos..] + } + + fn check_remaining(&self, count: usize) { + assert!( + self.remaining() >= count, + "read buffer exhausted; remaining {} bytes, try read {} bytes", + self.remaining(), + count + ); + } + + pub fn get_bytes(&mut self, count: usize) -> &[u8] { + self.check_remaining(count); + + let bytes = &self.data[self.pos..(self.pos + count)]; + self.pos += count; + bytes + } + + pub fn get_u8(&mut self) -> u8 { + self.check_remaining(1); + + let val = self.data[self.pos]; + self.pos += 1; + val + } + + pub fn get_i8(&mut self) -> i8 { + self.check_remaining(1); + + let val = self.data[self.pos] as i8; + self.pos += 1; + val + } + + pub fn get_u16_le(&mut self) -> u16 { + self.check_remaining(2); + + let mut val = [0u8; 2]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + self.pos += 2; + u16::from_le_bytes(val) + } + + pub fn get_i16_le(&mut self) -> i16 { + self.check_remaining(2); + + let mut val = [0u8; 2]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + self.pos += 2; + i16::from_le_bytes(val) + } + + pub fn get_u32_le(&mut self) -> u32 { + self.check_remaining(4); + + let mut val = [0u8; 4]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + self.pos += 4; + u32::from_le_bytes(val) + } + + pub fn get_i32_le(&mut self) -> i32 { + self.check_remaining(4); + + let mut val = [0u8; 4]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + self.pos += 4; + i32::from_le_bytes(val) + } + + pub fn get_u64_le(&mut self) -> u64 { + self.check_remaining(8); + + let mut val = [0u8; 8]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + val[4] = self.data[self.pos + 4]; + val[5] = self.data[self.pos + 5]; + val[6] = self.data[self.pos + 6]; + val[7] = self.data[self.pos + 7]; + self.pos += 8; + u64::from_le_bytes(val) + } + + pub fn get_i64_le(&mut self) -> i64 { + self.check_remaining(8); + + let mut val = [0u8; 8]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + val[4] = self.data[self.pos + 4]; + val[5] = self.data[self.pos + 5]; + val[6] = self.data[self.pos + 6]; + val[7] = self.data[self.pos + 7]; + self.pos += 8; + i64::from_le_bytes(val) + } + + pub fn get_f32_le(&mut self) -> f32 { + self.check_remaining(4); + + let mut val = [0u8; 4]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + self.pos += 4; + f32::from_le_bytes(val) + } + + pub fn get_f64_le(&mut self) -> f64 { + self.check_remaining(8); + + let mut val = [0u8; 8]; + val[0] = self.data[self.pos + 0]; + val[1] = self.data[self.pos + 1]; + val[2] = self.data[self.pos + 2]; + val[3] = self.data[self.pos + 3]; + val[4] = self.data[self.pos + 4]; + val[5] = self.data[self.pos + 5]; + val[6] = self.data[self.pos + 6]; + val[7] = self.data[self.pos + 7]; + self.pos += 8; + f64::from_le_bytes(val) + } +} diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs new file mode 100644 index 0000000000..2c2399fee2 --- /dev/null +++ b/src/bytes_mut.rs @@ -0,0 +1,167 @@ +pub struct BytesMut { + data: [u8; N], + len: usize, +} + +impl BytesMut { + pub fn new() -> Self { + Self { + data: [0; N], + len: 0 + } + } + + #[inline] + pub fn len(&self) -> usize { + self.len + } + + #[inline] + pub fn remaining(&self) -> usize { + N - self.len + } + + pub fn set_len(&mut self, len: usize) { + assert!(len >= 1); + assert!(len <= N); + self.len = len; + } + + #[inline] + fn check_remaining(&self, count: usize) { + assert!( + self.remaining() >= count, + "write buffer overflow; remaining {} bytes, try add {} bytes", + self.remaining(), + count + ); + } + + pub fn put_slice(&mut self, src: &[u8]) { + self.check_remaining(src.len()); + + unsafe { + core::ptr::copy_nonoverlapping(src.as_ptr(), &mut self.data[self.len], src.len()); + } + self.len += src.len(); + } + + pub fn put_u8(&mut self, val: u8) { + self.check_remaining(1); + + self.data[self.len] = val; + self.len += 1; + } + + pub fn put_i8(&mut self, val: i8) { + self.check_remaining(1); + + self.data[self.len] = val as u8; + self.len += 1; + } + + pub fn put_u16_le(&mut self, val: u16) { + self.check_remaining(2); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.len += 2; + } + + pub fn put_i16_le(&mut self, val: i16) { + self.check_remaining(2); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.len += 2; + } + + pub fn put_u32_le(&mut self, val: u32) { + self.check_remaining(4); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.len += 4; + } + + pub fn put_i32_le(&mut self, val: i32) { + self.check_remaining(4); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.len += 4; + } + + pub fn put_u64_le(&mut self, val: u64) { + self.check_remaining(8); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.data[self.len + 4] = src[4]; + self.data[self.len + 5] = src[5]; + self.data[self.len + 6] = src[6]; + self.data[self.len + 7] = src[7]; + self.len += 8; + } + + pub fn put_i64_le(&mut self, val: i64) { + self.check_remaining(8); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.data[self.len + 4] = src[4]; + self.data[self.len + 5] = src[5]; + self.data[self.len + 6] = src[6]; + self.data[self.len + 7] = src[7]; + self.len += 8; + } + + pub fn put_f32_le(&mut self, val: f32) { + self.check_remaining(4); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.len += 4; + } + + pub fn put_f64_le(&mut self, val: f64) { + self.check_remaining(8); + + let src = val.to_le_bytes(); + self.data[self.len + 0] = src[0]; + self.data[self.len + 1] = src[1]; + self.data[self.len + 2] = src[2]; + self.data[self.len + 3] = src[3]; + self.data[self.len + 4] = src[4]; + self.data[self.len + 5] = src[5]; + self.data[self.len + 6] = src[6]; + self.data[self.len + 7] = src[7]; + self.len += 8; + } +} + +impl core::ops::Deref for BytesMut { + type Target = [u8]; + + #[inline] + fn deref(&self) -> &[u8] { + &self.data[..self.len] + } +} diff --git a/src/error.rs b/src/error.rs index 3aece59b30..5d0cc82825 100644 --- a/src/error.rs +++ b/src/error.rs @@ -2,13 +2,10 @@ use core::fmt::{Display, Formatter}; #[cfg(feature = "std")] use std::error::Error; -#[cfg(not(feature = "std"))] -use alloc::string::String; - #[derive(Debug)] pub enum ParserError { - InvalidFlag { flag_type: String, value: u32 }, - InvalidEnum { enum_type: String, value: u32 }, + InvalidFlag { flag_type: &'static str, value: u32 }, + InvalidEnum { enum_type: &'static str, value: u32 }, UnknownMessage { id: u32 }, } diff --git a/src/lib.rs b/src/lib.rs index cd309b1bcc..24e73908c0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -18,15 +18,6 @@ //! feature for the message sets that it includes. For example, you cannot use the `ardupilotmega` //! feature without also using the `uavionix` and `icarous` features. //! -#![cfg_attr(not(feature = "std"), no_std)] -#[cfg(not(feature = "std"))] -extern crate alloc; - -#[cfg(not(feature = "std"))] -use alloc::vec; -#[cfg(not(feature = "std"))] -use alloc::vec::Vec; - use core::result::Result; #[cfg(feature = "std")] @@ -48,14 +39,15 @@ use utils::remove_trailing_zeroes; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -use crate::error::ParserError; -use bytes::{Buf, BytesMut}; +use crate::{error::ParserError, bytes::Bytes, bytes_mut::BytesMut}; use crc_any::CRCu16; // include generate definitions include!(concat!(env!("OUT_DIR"), "/mod.rs")); +pub mod bytes; +pub mod bytes_mut; pub mod error; #[cfg(feature = "embedded")] @@ -63,13 +55,15 @@ mod embedded; #[cfg(feature = "embedded")] use embedded::{Read, Write}; +pub const MAX_FRAME_SIZE: usize = 280; + pub trait Message where Self: Sized, { fn message_id(&self) -> u32; fn message_name(&self) -> &'static str; - fn ser(&self, version: MavlinkVersion) -> Vec; + fn ser(&self, version: MavlinkVersion) -> BytesMut; fn parse( version: MavlinkVersion, @@ -158,14 +152,14 @@ impl MavFrame { } } // serialize message - v.append(&mut self.msg.ser(self.protocol_version)); + v.extend_from_slice(&mut self.msg.ser(self.protocol_version)); v } /// Deserialize MavFrame from a slice that has been received from, for example, a socket. pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { - let mut buf = BytesMut::from(input); + let mut buf = Bytes::new(input); let system_id = buf.get_u8(); let component_id = buf.get_u8(); @@ -181,7 +175,7 @@ impl MavFrame { MavlinkVersion::V1 => buf.get_u8() as u32, }; - match M::parse(version, msg_id, &buf.into_iter().collect::>()) { + match M::parse(version, msg_id, buf.remaining_bytes()) { Ok(msg) => Ok(MavFrame { header, msg, diff --git a/src/utils.rs b/src/utils.rs index 3de872dd98..9e6db7369d 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,5 +1,4 @@ -#[cfg(not(feature = "std"))] -use alloc::vec::Vec; +use crate::{MAX_FRAME_SIZE, bytes_mut::BytesMut}; /// Removes the trailing zeroes in the payload /// @@ -8,11 +7,17 @@ use alloc::vec::Vec; /// There must always be at least one remaining byte even if it is a /// zero byte. #[allow(dead_code)] -pub(crate) fn remove_trailing_zeroes(buf: &mut Vec) { - while let Some(&0) = buf.last() { - if buf.len() <= 1 { +pub(crate) fn remove_trailing_zeroes(buf: &mut BytesMut) { + let data = &**buf; + let mut len = data.len(); + + for b in data[1..].iter().rev() { + if *b != 0 { break; } - buf.pop(); + + len -= 1; } + + buf.set_len(len); } From 670330e977a7aff20569a63c346407ab3025829c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Mon, 14 Nov 2022 19:06:52 +0400 Subject: [PATCH 026/159] Remove dependency on global Allocator --- Cargo.toml | 5 +++-- src/lib.rs | 16 ++++++++++------ 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 55fc13da19..6229da1b1e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -24,7 +24,7 @@ name = "mavlink-dump" required-features = ["ardupilotmega"] [dependencies] -crc-any = "2.3.5" +crc-any = { version = "2.3.5", default-features = false } num-traits = { version = "0.2", default-features = false } num-derive = "0.3.2" bitflags = "1.2.1" @@ -33,7 +33,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } embedded-hal = { version = "0.2", optional = true } nb = { version = "1.0", optional = true } -heapless = { version = "0.7", features = ["serde"] } +heapless = "0.7" [features] "ardupilotmega" = ["common", "icarous", "uavionix"] @@ -75,6 +75,7 @@ heapless = { version = "0.7", features = ["serde"] } "tcp" = [] "direct-serial" = [] "embedded" = ["embedded-hal", "nb"] +"serde" = ["dep:serde", "heapless/serde"] default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"] # build with all features on docs.rs so that users viewing documentation diff --git a/src/lib.rs b/src/lib.rs index 24e73908c0..1230ee657f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -18,6 +18,8 @@ //! feature for the message sets that it includes. For example, you cannot use the `ardupilotmega` //! feature without also using the `uavionix` and `icarous` features. //! +#![cfg_attr(not(feature = "std"), no_std)] + use core::result::Result; #[cfg(feature = "std")] @@ -27,6 +29,8 @@ use byteorder::LittleEndian; #[cfg(feature = "std")] use byteorder::ReadBytesExt; +use heapless::Vec; + #[cfg(feature = "std")] mod connection; #[cfg(feature = "std")] @@ -133,26 +137,26 @@ impl MavFrame { // } /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. - pub fn ser(&self) -> Vec { + pub fn ser(&self) -> Vec { // serialize header - let mut v = vec![ + let mut v = Vec::from_slice(&[ self.header.system_id, self.header.component_id, self.header.sequence, - ]; + ]).unwrap(); // message id match self.protocol_version { MavlinkVersion::V2 => { let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); - v.extend_from_slice(&bytes); + v.extend_from_slice(&bytes).unwrap(); } MavlinkVersion::V1 => { - v.push(self.msg.message_id() as u8); //TODO check + v.push(self.msg.message_id() as u8).unwrap(); //TODO check } } // serialize message - v.extend_from_slice(&mut self.msg.ser(self.protocol_version)); + v.extend_from_slice(&mut self.msg.ser(self.protocol_version)).unwrap(); v } From 0d4a1970b448c83aea530d9b7329520e01708814 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Tue, 15 Nov 2022 15:20:52 +0400 Subject: [PATCH 027/159] Optimize BytesMut::put_* and Bytes::get_* methods --- build/parser.rs | 4 +- src/bytes.rs | 116 +++++++++++++++++++---------------------------- src/bytes_mut.rs | 88 +++++++++++++---------------------- 3 files changed, 80 insertions(+), 128 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index a24a32ecaa..d3b628ecf4 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -922,9 +922,9 @@ impl MavType { // format!("[{};{}]", t.rust_type(), size) if size > 32 { // we have to use a vector to make our lives easier - format!("Vec<{}, {}> /* {} elements */", t.rust_type(), size, size) + format!("Vec<{}, {}>", t.rust_type(), size) } else { - // we can use a slice, as Rust derives lot of thinsg for slices <= 32 elements + // we can use a slice, as Rust derives lot of things for slices <= 32 elements format!("[{};{}]", t.rust_type(), size) } diff --git a/src/bytes.rs b/src/bytes.rs index 573a5ede32..44baf12b1a 100644 --- a/src/bytes.rs +++ b/src/bytes.rs @@ -54,106 +54,82 @@ impl<'a> Bytes<'a> { } pub fn get_u16_le(&mut self) -> u16 { - self.check_remaining(2); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); - let mut val = [0u8; 2]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - self.pos += 2; + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; u16::from_le_bytes(val) } pub fn get_i16_le(&mut self) -> i16 { - self.check_remaining(2); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); - let mut val = [0u8; 2]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - self.pos += 2; + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; i16::from_le_bytes(val) } pub fn get_u32_le(&mut self) -> u32 { - self.check_remaining(4); - - let mut val = [0u8; 4]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - self.pos += 4; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; u32::from_le_bytes(val) } pub fn get_i32_le(&mut self) -> i32 { - self.check_remaining(4); - - let mut val = [0u8; 4]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - self.pos += 4; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; i32::from_le_bytes(val) } pub fn get_u64_le(&mut self) -> u64 { - self.check_remaining(8); - - let mut val = [0u8; 8]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - val[4] = self.data[self.pos + 4]; - val[5] = self.data[self.pos + 5]; - val[6] = self.data[self.pos + 6]; - val[7] = self.data[self.pos + 7]; - self.pos += 8; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; u64::from_le_bytes(val) } pub fn get_i64_le(&mut self) -> i64 { - self.check_remaining(8); - - let mut val = [0u8; 8]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - val[4] = self.data[self.pos + 4]; - val[5] = self.data[self.pos + 5]; - val[6] = self.data[self.pos + 6]; - val[7] = self.data[self.pos + 7]; - self.pos += 8; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; i64::from_le_bytes(val) } pub fn get_f32_le(&mut self) -> f32 { - self.check_remaining(4); - - let mut val = [0u8; 4]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - self.pos += 4; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; f32::from_le_bytes(val) } pub fn get_f64_le(&mut self) -> f64 { - self.check_remaining(8); - - let mut val = [0u8; 8]; - val[0] = self.data[self.pos + 0]; - val[1] = self.data[self.pos + 1]; - val[2] = self.data[self.pos + 2]; - val[3] = self.data[self.pos + 3]; - val[4] = self.data[self.pos + 4]; - val[5] = self.data[self.pos + 5]; - val[6] = self.data[self.pos + 6]; - val[7] = self.data[self.pos + 7]; - self.pos += 8; + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); + + let mut val = [0u8; SIZE]; + val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + self.pos += SIZE; f64::from_le_bytes(val) } } diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index 2c2399fee2..78e6dcc9f2 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -61,99 +61,75 @@ impl BytesMut { } pub fn put_u16_le(&mut self, val: u16) { - self.check_remaining(2); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.len += 2; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_i16_le(&mut self, val: i16) { - self.check_remaining(2); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.len += 2; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_u32_le(&mut self, val: u32) { - self.check_remaining(4); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.len += 4; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_i32_le(&mut self, val: i32) { - self.check_remaining(4); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.len += 4; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_u64_le(&mut self, val: u64) { - self.check_remaining(8); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.data[self.len + 4] = src[4]; - self.data[self.len + 5] = src[5]; - self.data[self.len + 6] = src[6]; - self.data[self.len + 7] = src[7]; - self.len += 8; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_i64_le(&mut self, val: i64) { - self.check_remaining(8); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.data[self.len + 4] = src[4]; - self.data[self.len + 5] = src[5]; - self.data[self.len + 6] = src[6]; - self.data[self.len + 7] = src[7]; - self.len += 8; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_f32_le(&mut self, val: f32) { - self.check_remaining(4); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.len += 4; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } pub fn put_f64_le(&mut self, val: f64) { - self.check_remaining(8); + const SIZE: usize = core::mem::size_of::(); + self.check_remaining(SIZE); let src = val.to_le_bytes(); - self.data[self.len + 0] = src[0]; - self.data[self.len + 1] = src[1]; - self.data[self.len + 2] = src[2]; - self.data[self.len + 3] = src[3]; - self.data[self.len + 4] = src[4]; - self.data[self.len + 5] = src[5]; - self.data[self.len + 6] = src[6]; - self.data[self.len + 7] = src[7]; - self.len += 8; + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..]); + self.len += SIZE; } } From fcc5261e95b4e3dca37e1c702c3ecc5cb685f3a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Wed, 16 Nov 2022 18:33:04 +0400 Subject: [PATCH 028/159] Update embedded example --- Cargo.toml | 3 +-- .../embedded/.cargo/{config => config.toml} | 0 examples/embedded/Cargo.toml | 7 +++-- examples/embedded/README.md | 8 +++--- examples/embedded/src/main.rs | 26 +++---------------- 5 files changed, 12 insertions(+), 32 deletions(-) rename examples/embedded/.cargo/{config => config.toml} (100%) diff --git a/Cargo.toml b/Cargo.toml index 6229da1b1e..a9e411c7f0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,8 +11,7 @@ repository = "https://github.com/mavlink/rust-mavlink" edition = "2018" [build-dependencies] -crc-any = "2.3.0" -bytes = { version = "1.0", default-features = false } +crc-any = { version = "2.3.0", default-features = false } quick-xml = "0.26" quote = "1" proc-macro2 = "1.0.43" diff --git a/examples/embedded/.cargo/config b/examples/embedded/.cargo/config.toml similarity index 100% rename from examples/embedded/.cargo/config rename to examples/embedded/.cargo/config.toml diff --git a/examples/embedded/Cargo.toml b/examples/embedded/Cargo.toml index 1f894587a3..4f9fd15e7e 100644 --- a/examples/embedded/Cargo.toml +++ b/examples/embedded/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-embedded" -edition = "2018" +edition = "2021" authors = [ "Patrick José Pereira ", ] @@ -12,10 +12,9 @@ lto = true # Performs "fat" LTO which attempts to perform optimizations acr [dependencies] cortex-m = "0.7" # Low level access to Cortex-M processors -cortex-m-rt = "0.6" # Startup code and minimal runtime for Cortex-M microcontrollers +cortex-m-rt = "0.7" # Startup code and minimal runtime for Cortex-M microcontrollers panic-halt = "0.2" # Panic handler -static-alloc = "0.2" # Small general purpose global allocator -stm32f3xx-hal = { version = "0.6", features = ["stm32f303xe"] } # HAL for stm32f303xe +stm32f3xx-hal = { version = "0.9", features = ["stm32f303xe"] } # HAL for stm32f303xe [dependencies.mavlink] # MAVLink library (wait for 0.9.0 version) path = "../../" diff --git a/examples/embedded/README.md b/examples/embedded/README.md index 83fb338847..78c9fd8538 100644 --- a/examples/embedded/README.md +++ b/examples/embedded/README.md @@ -2,10 +2,10 @@ ### How to run: - Install cargo flash: - cargo install cargo-flash -- Install toolchain - - rustup target add thumbv7em-none-eabihf --toolchain nightly +- Install target + - rustup target add thumbv7em-none-eabihf - Check if we can build the project - - cargo +nightly build + - cargo build - Connect your STM32f303Xe board - Flash it! - - cargo +nightly flash --chip stm32f303RETx --release --log info + - cargo flash --chip stm32f303RETx --release --log info diff --git a/examples/embedded/src/main.rs b/examples/embedded/src/main.rs index b6131b8eee..4a71ca8fcf 100644 --- a/examples/embedded/src/main.rs +++ b/examples/embedded/src/main.rs @@ -1,6 +1,5 @@ //! Target board: stm32f303RETx (stm32nucleo) //! Manual: https://www.st.com/resource/en/reference_manual/dm00043574-stm32f303xb-c-d-e-stm32f303x6-8-stm32f328x8-stm32f358xc-stm32f398xe-advanced-arm-based-mcus-stmicroelectronics.pdf -#![feature(alloc_error_handler)] #![no_main] #![no_std] @@ -11,12 +10,8 @@ use cortex_m_rt::entry; use hal::pac; use hal::prelude::*; use mavlink; -use static_alloc::Bump; use stm32f3xx_hal as hal; -#[global_allocator] // 1KB allocator -static mut ALLOCATOR: Bump<[u8; 1 << 10]> = Bump::uninit(); - #[entry] fn main() -> ! { // Peripherals access @@ -49,14 +44,14 @@ fn main() -> ! { // We don't need the datasheet to check which alternative function to use // https://docs.rs/stm32f3xx-hal/0.6.1/stm32f3xx_hal/gpio/gpioa/struct.PA2.html#impl-TxPin%3CUSART2%3E // The documentation provide the necessary information about each possible hardware configuration - let pin_tx = gpioa.pa2.into_af7(&mut gpioa.moder, &mut gpioa.afrl); - let pin_rx = gpioa.pa3.into_af7(&mut gpioa.moder, &mut gpioa.afrl); + let pin_tx = gpioa.pa2.into_af_push_pull(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrl); + let pin_rx = gpioa.pa3.into_af_push_pull(&mut gpioa.moder, &mut gpioa.otyper, &mut gpioa.afrl); // Create an interface USART2 with 115200 baudrate - let serial = hal::serial::Serial::usart2( + let serial = hal::serial::Serial::new( dp.USART2, (pin_tx, pin_rx), - 115_200.bps(), + 115_200.Bd(), clocks, &mut rcc.apb1, ); @@ -73,12 +68,6 @@ fn main() -> ! { // Main loop loop { - // Clear allocator before restarting our loop (cheap allocator) - // unnecessary for alloc_cortex_m::CortexMHeap - unsafe { - ALLOCATOR.reset(); - }; - // Write the mavlink message via serial mavlink::write_versioned_msg(&mut tx, mavlink::MavlinkVersion::V2, header, &heartbeat) .unwrap(); @@ -109,10 +98,3 @@ pub fn mavlink_heartbeat_message() -> mavlink::common::MavMessage { mavlink_version: 0x3, }) } - -#[alloc_error_handler] -fn alloc_error(_layout: core::alloc::Layout) -> ! { - // Init debug state - cortex_m::asm::bkpt(); - loop {} -} From b7e7e29ff24c2ec9dfaca5271e46f6fbe66f4d14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Mon, 21 Nov 2022 17:19:50 +0400 Subject: [PATCH 029/159] rustfmt run --- build/parser.rs | 1 - src/bytes.rs | 5 +---- src/bytes_mut.rs | 2 +- src/lib.rs | 8 +++++--- src/utils.rs | 2 +- 5 files changed, 8 insertions(+), 10 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index d3b628ecf4..7cb927ea2f 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -927,7 +927,6 @@ impl MavType { // we can use a slice, as Rust derives lot of things for slices <= 32 elements format!("[{};{}]", t.rust_type(), size) } - } } } diff --git a/src/bytes.rs b/src/bytes.rs index 44baf12b1a..d6b7fdcd8b 100644 --- a/src/bytes.rs +++ b/src/bytes.rs @@ -5,10 +5,7 @@ pub struct Bytes<'a> { impl<'a> Bytes<'a> { pub fn new(data: &'a [u8]) -> Self { - Self { - data, - pos: 0 - } + Self { data, pos: 0 } } #[inline] diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index 78e6dcc9f2..3d3370046f 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -7,7 +7,7 @@ impl BytesMut { pub fn new() -> Self { Self { data: [0; N], - len: 0 + len: 0, } } diff --git a/src/lib.rs b/src/lib.rs index 1230ee657f..3699d0d859 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -43,7 +43,7 @@ use utils::remove_trailing_zeroes; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -use crate::{error::ParserError, bytes::Bytes, bytes_mut::BytesMut}; +use crate::{bytes::Bytes, bytes_mut::BytesMut, error::ParserError}; use crc_any::CRCu16; @@ -143,7 +143,8 @@ impl MavFrame { self.header.system_id, self.header.component_id, self.header.sequence, - ]).unwrap(); + ]) + .unwrap(); // message id match self.protocol_version { @@ -156,7 +157,8 @@ impl MavFrame { } } // serialize message - v.extend_from_slice(&mut self.msg.ser(self.protocol_version)).unwrap(); + v.extend_from_slice(&mut self.msg.ser(self.protocol_version)) + .unwrap(); v } diff --git a/src/utils.rs b/src/utils.rs index 9e6db7369d..d89058526a 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,4 +1,4 @@ -use crate::{MAX_FRAME_SIZE, bytes_mut::BytesMut}; +use crate::{bytes_mut::BytesMut, MAX_FRAME_SIZE}; /// Removes the trailing zeroes in the payload /// From 1bf58e94b693341c91a504c06cb07f4f7f99e569 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 21 Nov 2022 12:07:47 -0300 Subject: [PATCH 030/159] github: test: Move alway from deprecated actions-rs MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/test.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e59808c90e..ab947a2e9a 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -7,7 +7,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.7 + - uses: dtolnay/rust-toolchain@master with: toolchain: stable target: ${{ matrix.TARGET }} @@ -66,7 +66,7 @@ jobs: - name: Building ${{ matrix.TARGET }} run: echo "${{ matrix.TARGET }}" - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.7 + - uses: dtolnay/rust-toolchain@master with: toolchain: stable target: ${{ matrix.TARGET }} @@ -82,7 +82,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.7 + - uses: dtolnay/rust-toolchain@master with: toolchain: nightly target: thumbv7em-none-eabihf From 6cab2a5d9075acc51bec0530405a457dc13f66bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 21 Nov 2022 12:13:41 -0300 Subject: [PATCH 031/159] github: Moerge lint with tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/lint.yml | 37 ------------------------------------- .github/workflows/test.yml | 10 ++++++++-- 2 files changed, 8 insertions(+), 39 deletions(-) delete mode 100644 .github/workflows/lint.yml diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml deleted file mode 100644 index f73d1868df..0000000000 --- a/.github/workflows/lint.yml +++ /dev/null @@ -1,37 +0,0 @@ -name: Lint - -on: [push, pull_request] - -jobs: - - fmt: - name: format - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: actions-rs/toolchain@v1 - with: - toolchain: nightly - override: true - profile: minimal - components: rustfmt - - uses: actions-rs/cargo@v1 - with: - command: fmt - args: --all -- --check - - clippy: - name: lint - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: actions-rs/toolchain@v1 - with: - toolchain: nightly - override: true - profile: minimal - components: clippy - - uses: actions-rs/clippy-check@v1 - with: - token: ${{ secrets.GITHUB_TOKEN }} - args: --all-targets diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index ab947a2e9a..d6f25baad6 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -12,8 +12,14 @@ jobs: toolchain: stable target: ${{ matrix.TARGET }} override: true - - name: Check Type - run: cargo fmt -- --check + components: clippy, rustfmt + - name: Run rustfmt + run: cargo fmt --all -- --check + - uses: actions-rs/clippy-check@v1 + with: + token: ${{ secrets.GITHUB_TOKEN }} + args: --all-targets + - name: Run internal tests run: | dialects=("ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common") From d1caf7a78fe4e6d98ac04ded5cd5d50dbe29ad02 Mon Sep 17 00:00:00 2001 From: "daniel.eades" Date: Tue, 29 Nov 2022 15:21:38 +0000 Subject: [PATCH 032/159] add msrv target to CI --- .clippy.toml | 1 + .github/workflows/test.yml | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+) create mode 100644 .clippy.toml diff --git a/.clippy.toml b/.clippy.toml new file mode 100644 index 0000000000..16caf02ee9 --- /dev/null +++ b/.clippy.toml @@ -0,0 +1 @@ +msrv = "1.60.0" diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d6f25baad6..050dd1ec74 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -3,6 +3,7 @@ name: Test all targets on: [push, pull_request] jobs: + quick-tests: runs-on: ubuntu-latest steps: @@ -33,6 +34,23 @@ jobs: - name: Build mavlink-dump run: cargo build --verbose --bin mavlink-dump --features ardupilotmega + msrv: + needs: quick-tests + runs-on: ubuntu-latest + strategy: + fail-fast: false + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: "1.60.0" + override: true + - uses: actions-rs/cargo@v1 + with: + use-cross: true + command: check + args: --all-targets + build: needs: quick-tests runs-on: ${{ matrix.os }} From d5f286588ee6f7eeb3a70f3d5190906f3d13a81d Mon Sep 17 00:00:00 2001 From: "daniel.eades" Date: Thu, 1 Dec 2022 14:38:22 +0000 Subject: [PATCH 033/159] split CI jobs --- .github/workflows/test.yml | 182 ++++++++++++++++++++----------------- 1 file changed, 100 insertions(+), 82 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 050dd1ec74..6b19c12bb3 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -3,113 +3,131 @@ name: Test all targets on: [push, pull_request] jobs: + formatting: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + components: rustfmt + - name: Run rustfmt + run: cargo fmt --all -- --check - quick-tests: + linting: runs-on: ubuntu-latest steps: - - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable - target: ${{ matrix.TARGET }} - override: true - components: clippy, rustfmt - - name: Run rustfmt - run: cargo fmt --all -- --check - - uses: actions-rs/clippy-check@v1 - with: - token: ${{ secrets.GITHUB_TOKEN }} - args: --all-targets + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: nightly-2022-11-30 + components: clippy + - uses: actions-rs/clippy-check@v1 + with: + token: ${{ secrets.GITHUB_TOKEN }} + args: --all --all-targets - - name: Run internal tests - run: | - dialects=("ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common") - for dialect in "${dialects[@]}"; do - echo "::group::Testing $dialect" - if ! cargo test --verbose --features "$dialect" -- --nocapture; then - echo "::error::Tests failed" - fi - echo "::endgroup::" - done - - name: Build mavlink-dump - run: cargo build --verbose --bin mavlink-dump --features ardupilotmega + internal-tests: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + - name: Run internal tests + run: | + dialects=("ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common") + for dialect in "${dialects[@]}"; do + echo "::group::Testing $dialect" + if ! cargo test --verbose --features "$dialect" -- --nocapture; then + echo "::error::Tests failed" + fi + echo "::endgroup::" + done + + mavlink-dump: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + - name: Build mavlink-dump + run: cargo build --verbose --bin mavlink-dump --features ardupilotmega msrv: - needs: quick-tests runs-on: ubuntu-latest - strategy: - fail-fast: false steps: - - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: "1.60.0" - override: true - - uses: actions-rs/cargo@v1 - with: - use-cross: true - command: check - args: --all-targets + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: "1.60.0" + override: true + - uses: actions-rs/cargo@v1 + with: + use-cross: true + command: check + args: --all-targets build: - needs: quick-tests + needs: [formatting, linting, internal-tests, mavlink-dump, msrv] runs-on: ${{ matrix.os }} strategy: fail-fast: false matrix: include: - - os: macos-latest - TARGET: x86_64-apple-darwin - FEATURES: --features ardupilotmega + - os: macos-latest + TARGET: x86_64-apple-darwin + FEATURES: --features ardupilotmega - - os: ubuntu-latest - TARGET: arm-unknown-linux-musleabihf - FLAGS: --features ardupilotmega + - os: ubuntu-latest + TARGET: arm-unknown-linux-musleabihf + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: armv7-unknown-linux-musleabihf - FLAGS: --features ardupilotmega + - os: ubuntu-latest + TARGET: armv7-unknown-linux-musleabihf + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: x86_64-unknown-linux-musl - FLAGS: --features ardupilotmega + - os: ubuntu-latest + TARGET: x86_64-unknown-linux-musl + FLAGS: --features ardupilotmega - - os: ubuntu-latest - TARGET: x86_64-unknown-linux-musl - FLAGS: --features ardupilotmega,emit-description,emit-extensions + - os: ubuntu-latest + TARGET: x86_64-unknown-linux-musl + FLAGS: --features ardupilotmega,emit-description,emit-extensions - - os: ubuntu-latest - TARGET: thumbv7m-none-eabi - FLAGS: --no-default-features --features embedded + - os: ubuntu-latest + TARGET: thumbv7m-none-eabi + FLAGS: --no-default-features --features embedded - - os: windows-latest - TARGET: x86_64-pc-windows-msvc - FLAGS: --features ardupilotmega + - os: windows-latest + TARGET: x86_64-pc-windows-msvc + FLAGS: --features ardupilotmega steps: - - name: Building ${{ matrix.TARGET }} - run: echo "${{ matrix.TARGET }}" - - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable - target: ${{ matrix.TARGET }} - override: true - - uses: actions-rs/cargo@v1 - with: - use-cross: true - command: build - args: --verbose --release --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} + - name: Building ${{ matrix.TARGET }} + run: echo "${{ matrix.TARGET }}" + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: stable + target: ${{ matrix.TARGET }} + override: true + - uses: actions-rs/cargo@v1 + with: + use-cross: true + command: build + args: --verbose --release --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} test-embedded-size: needs: build runs-on: ubuntu-latest steps: - - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: nightly - target: thumbv7em-none-eabihf - override: true - - name: Build - run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options + - uses: actions/checkout@master + - uses: dtolnay/rust-toolchain@master + with: + toolchain: nightly + target: thumbv7em-none-eabihf + override: true + - name: Build + run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options From 2f9ef39255f4c62afa8e32232c3ca011b826506b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Sat, 26 Nov 2022 16:07:55 +0400 Subject: [PATCH 034/159] MAVLinkV1MessageRaw and MAVLinkV2MessageRaw refactoring --- build/parser.rs | 16 +- src/bytes_mut.rs | 30 +--- src/embedded.rs | 14 -- src/lib.rs | 405 ++++++++++++++++++++++++++++------------------- src/utils.rs | 8 +- 5 files changed, 259 insertions(+), 214 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 7cb927ea2f..d9e6d2b9ec 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -182,7 +182,7 @@ impl MavProfile { #[allow(unused_imports)] use heapless::Vec; - use crate::{Message, error::*, MAX_FRAME_SIZE, bytes::Bytes, bytes_mut::BytesMut}; + use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -330,9 +330,9 @@ impl MavProfile { fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { - fn ser(&self, version: MavlinkVersion) -> BytesMut { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { match *self { - #(MavMessage::#enums(ref body) => body.ser(version),)* + #(MavMessage::#enums(ref body) => body.ser(version, bytes),)* } } } @@ -542,12 +542,14 @@ impl MavMessage { fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self.fields.iter().map(|f| f.rust_writer()); quote! { - let mut _tmp = BytesMut::new(); + let mut _tmp = BytesMut::new(bytes); #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { - crate::remove_trailing_zeroes(&mut _tmp); + let len = _tmp.len(); + crate::remove_trailing_zeroes(&mut bytes[..len]) + } else { + _tmp.len() } - _tmp } } @@ -612,7 +614,7 @@ impl MavMessage { #deser_vars } - pub fn ser(&self, version: MavlinkVersion) -> BytesMut { + pub fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { #serialize_vars } } diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index 3d3370046f..a3b70ad45c 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -1,14 +1,11 @@ -pub struct BytesMut { - data: [u8; N], +pub struct BytesMut<'a> { + data: &'a mut [u8], len: usize, } -impl BytesMut { - pub fn new() -> Self { - Self { - data: [0; N], - len: 0, - } +impl<'a> BytesMut<'a> { + pub fn new(data: &'a mut [u8]) -> Self { + Self { data, len: 0 } } #[inline] @@ -18,13 +15,7 @@ impl BytesMut { #[inline] pub fn remaining(&self) -> usize { - N - self.len - } - - pub fn set_len(&mut self, len: usize) { - assert!(len >= 1); - assert!(len <= N); - self.len = len; + self.data.len() - self.len } #[inline] @@ -132,12 +123,3 @@ impl BytesMut { self.len += SIZE; } } - -impl core::ops::Deref for BytesMut { - type Target = [u8]; - - #[inline] - fn deref(&self) -> &[u8] { - &self.data[..self.len] - } -} diff --git a/src/embedded.rs b/src/embedded.rs index 3c27b8f1bb..64138a5b14 100644 --- a/src/embedded.rs +++ b/src/embedded.rs @@ -1,17 +1,9 @@ -use byteorder::ByteOrder; - use crate::error::*; /// Replacement for std::io::Read + byteorder::ReadBytesExt in no_std envs pub trait Read { fn read_u8(&mut self) -> Result; - fn read_u16(&mut self) -> Result { - let mut buffer = [0; 2]; - self.read_exact(&mut buffer)?; - Ok(B::read_u16(&buffer)) - } - fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { for i in 0..buf.len() { buf[i] = self.read_u8()?; @@ -30,12 +22,6 @@ impl> Read for R { /// Replacement for std::io::Write + byteorder::WriteBytesExt in no_std envs pub trait Write { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError>; - - fn write_u16(&mut self, n: u16) -> Result<(), MessageWriteError> { - let mut buffer = [0; 2]; - B::write_u16(&mut buffer, n); - self.write_all(&buffer) - } } impl> Write for W { diff --git a/src/lib.rs b/src/lib.rs index 3699d0d859..40c14f200e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -25,7 +25,6 @@ use core::result::Result; #[cfg(feature = "std")] use std::io::{Read, Write}; -use byteorder::LittleEndian; #[cfg(feature = "std")] use byteorder::ReadBytesExt; @@ -43,7 +42,7 @@ use utils::remove_trailing_zeroes; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -use crate::{bytes::Bytes, bytes_mut::BytesMut, error::ParserError}; +use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; @@ -67,7 +66,9 @@ where { fn message_id(&self) -> u32; fn message_name(&self) -> &'static str; - fn ser(&self, version: MavlinkVersion) -> BytesMut; + + /// Serialize **Message** into byte slice and return count of bytes written + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize; fn parse( version: MavlinkVersion, @@ -157,8 +158,10 @@ impl MavFrame { } } // serialize message - v.extend_from_slice(&mut self.msg.ser(self.protocol_version)) - .unwrap(); + let mut payload_buf = [0u8; 255]; + let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); + + v.extend_from_slice(&payload_buf[..payload_len]).unwrap(); v } @@ -178,11 +181,11 @@ impl MavFrame { let msg_id = match version { MavlinkVersion::V2 => buf.get_u32_le(), - MavlinkVersion::V1 => buf.get_u8() as u32, + MavlinkVersion::V1 => buf.get_u8().into(), }; match M::parse(version, msg_id, buf.remaining_bytes()) { - Ok(msg) => Ok(MavFrame { + Ok(msg) => Ok(Self { header, msg, protocol_version: version, @@ -209,45 +212,111 @@ pub fn read_versioned_msg( #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: https://mavlink.io/en/guide/serialization.html#v1_packet_format -pub struct MAVLinkV1MessageRaw { - pub start: u8, // = MAV_STX - pub payload_length: u8, - pub sequence: u8, - pub system_id: u8, - pub component_id: u8, - pub message_id: u8, - pub payload_buffer: [u8; 255], - pub checksum: u16, +pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); + +impl Default for MAVLinkV1MessageRaw { + fn default() -> Self { + Self::new() + } } impl MAVLinkV1MessageRaw { + const HEADER_SIZE: usize = 5; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2]) + } + + #[inline] + pub fn header(&mut self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn message_id(&self) -> u8 { + self.0[5] + } + + #[inline] pub fn payload(&self) -> &[u8] { - &self.payload_buffer[..self.payload_length as usize] + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] + } + + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) } - pub fn mut_payload(&mut self) -> &mut [u8] { - &mut self.payload_buffer[..self.payload_length as usize] + #[inline] + fn mut_payload_and_checksum(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)] } pub fn calculate_crc(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&[ - self.payload_length, - self.sequence, - self.system_id, - self.component_id, - self.message_id, - ]); - let payload = self.payload(); - crc_calculator.digest(payload); - let extra_crc = M::extra_crc(self.message_id.into()); + crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); + let extra_crc = M::extra_crc(self.message_id().into()); crc_calculator.digest(&[extra_crc]); crc_calculator.get_crc() } + #[inline] pub fn has_valid_crc(&self) -> bool { - self.checksum == self.calculate_crc::() + self.checksum() == self.calculate_crc::() + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + self.0[0] = MAV_STX; + let msgid = message.message_id(); + + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_len = message.ser(MavlinkVersion::V1, payload_buf); + + let header_buf = self.mut_header(); + header_buf.copy_from_slice(&[ + payload_len as u8, + header.sequence, + header.system_id, + header.component_id, + msgid as u8, + ]); + + let crc = self.calculate_crc::(); + self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + .copy_from_slice(&crc.to_le_bytes()); } } @@ -263,21 +332,11 @@ pub fn read_v1_raw_message( } } - let mut message = MAVLinkV1MessageRaw { - start: MAV_STX, - payload_length: reader.read_u8()?, - sequence: reader.read_u8()?, - system_id: reader.read_u8()?, - component_id: reader.read_u8()?, - message_id: reader.read_u8()?, - payload_buffer: [0; 255], - checksum: 0, - }; - - let payload = &mut message.payload_buffer[..message.payload_length as usize]; - reader.read_exact(payload)?; + let mut message = MAVLinkV1MessageRaw::new(); - message.checksum = reader.read_u16::()?; + message.0[0] = MAV_STX; + reader.read_exact(message.mut_header())?; + reader.read_exact(message.mut_payload_and_checksum())?; Ok(message) } @@ -294,15 +353,15 @@ pub fn read_v1_msg( return M::parse( MavlinkVersion::V1, - message.message_id as u32, + u32::from(message.message_id()), message.payload(), ) .map(|msg| { ( MavHeader { - sequence: message.sequence, - system_id: message.system_id, - component_id: message.component_id, + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), }, msg, ) @@ -315,49 +374,135 @@ const MAVLINK_IFLAG_SIGNED: u8 = 0x01; #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format -pub struct MAVLinkV2MessageRaw { - pub start: u8, // = MAV_STX_V2 - pub payload_length: u8, - pub incompatibility_flags: u8, - pub compatibility_flags: u8, - pub sequence: u8, - pub system_id: u8, - pub component_id: u8, - pub message_id: u32, - pub payload_buffer: [u8; 255], - pub checksum: u16, +pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]); + +impl Default for MAVLinkV2MessageRaw { + fn default() -> Self { + Self::new() + } } impl MAVLinkV2MessageRaw { + const HEADER_SIZE: usize = 9; + const SIGNATURE_SIZE: usize = 13; + + pub const fn new() -> Self { + Self([0; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]) + } + + #[inline] + pub fn header(&mut self) -> &[u8] { + &self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + fn mut_header(&mut self) -> &mut [u8] { + &mut self.0[1..=Self::HEADER_SIZE] + } + + #[inline] + pub fn payload_length(&self) -> u8 { + self.0[1] + } + + #[inline] + pub fn incompatibility_flags(&self) -> u8 { + self.0[2] + } + + #[inline] + pub fn compatibility_flags(&self) -> u8 { + self.0[3] + } + + #[inline] + pub fn sequence(&self) -> u8 { + self.0[4] + } + + #[inline] + pub fn system_id(&self) -> u8 { + self.0[5] + } + + #[inline] + pub fn component_id(&self) -> u8 { + self.0[6] + } + + #[inline] + pub fn message_id(&self) -> u32 { + u32::from_le_bytes([self.0[7], self.0[8], self.0[9], 0]) + } + + #[inline] pub fn payload(&self) -> &[u8] { - &self.payload_buffer[..self.payload_length as usize] + let payload_length: usize = self.payload_length().into(); + &self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length)] } - pub fn mut_payload(&mut self) -> &mut [u8] { - &mut self.payload_buffer[..self.payload_length as usize] + #[inline] + pub fn checksum(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); + u16::from_le_bytes([ + self.0[1 + Self::HEADER_SIZE + payload_length], + self.0[1 + Self::HEADER_SIZE + payload_length + 1], + ]) + } + + fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + + // Signature to ensure the link is tamper-proof. + let signature_size = if (self.incompatibility_flags() & 0x01) == MAVLINK_IFLAG_SIGNED { + Self::SIGNATURE_SIZE + } else { + 0 + }; + + &mut self.0 + [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2 + signature_size)] } pub fn calculate_crc(&self) -> u16 { + let payload_length: usize = self.payload_length().into(); let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&[ - self.payload_length, - self.incompatibility_flags, - self.compatibility_flags, - self.sequence, - self.system_id, - self.component_id, - ]); - let payload = self.payload(); - crc_calculator.digest(&self.message_id.to_le_bytes()[..3]); - crc_calculator.digest(payload); - let extra_crc = M::extra_crc(self.message_id); + crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); + let extra_crc = M::extra_crc(self.message_id()); crc_calculator.digest(&[extra_crc]); crc_calculator.get_crc() } + #[inline] pub fn has_valid_crc(&self) -> bool { - self.checksum == self.calculate_crc::() + self.checksum() == self.calculate_crc::() + } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + self.0[0] = MAV_STX_V2; + let msgid = message.message_id(); + let msgid_bytes = msgid.to_le_bytes(); + + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_len = message.ser(MavlinkVersion::V2, payload_buf); + + let header_buf = self.mut_header(); + header_buf.copy_from_slice(&[ + payload_len as u8, + 0, //incompat_flags + 0, //compat_flags + header.sequence, + header.system_id, + header.component_id, + msgid_bytes[0], + msgid_bytes[1], + msgid_bytes[2], + ]); + + let crc = self.calculate_crc::(); + self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + .copy_from_slice(&crc.to_le_bytes()); } } @@ -373,31 +518,11 @@ pub fn read_v2_raw_message( } } - let mut message = MAVLinkV2MessageRaw { - start: MAV_STX_V2, - payload_length: reader.read_u8()?, - incompatibility_flags: reader.read_u8()?, - compatibility_flags: reader.read_u8()?, - sequence: reader.read_u8()?, - system_id: reader.read_u8()?, - component_id: reader.read_u8()?, - message_id: 0, - payload_buffer: [0; 255], - checksum: 0, - }; - - message.message_id = - u32::from_le_bytes([reader.read_u8()?, reader.read_u8()?, reader.read_u8()?, 0]); - - reader.read_exact(message.mut_payload())?; - - message.checksum = reader.read_u16::()?; + let mut message = MAVLinkV2MessageRaw::new(); - // Signature to ensure the link is tamper-proof. - if (message.incompatibility_flags & 0x01) == MAVLINK_IFLAG_SIGNED { - let mut sign = [0; 13]; - reader.read_exact(&mut sign)?; - } + message.0[0] = MAV_STX_V2; + reader.read_exact(message.mut_header())?; + reader.read_exact(message.mut_payload_and_checksum_and_sign())?; Ok(message) } @@ -413,13 +538,13 @@ pub fn read_v2_msg( continue; } - return M::parse(MavlinkVersion::V2, message.message_id, message.payload()) + 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, + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), }, msg, ) @@ -447,42 +572,13 @@ pub fn write_v2_msg( header: MavHeader, data: &M, ) -> Result { - let msgid = data.message_id(); - let payload = data.ser(MavlinkVersion::V2); - // println!("write payload_len : {}", payload.len()); - - let header = &[ - MAV_STX_V2, - payload.len() as u8, - 0, //incompat_flags - 0, //compat_flags - header.sequence, - header.system_id, - header.component_id, - (msgid & 0x0000FF) as u8, - ((msgid & 0x00FF00) >> 8) as u8, - ((msgid & 0xFF0000) >> 16) as u8, - ]; - - // println!("write H: {:?}",header ); - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - // let header_crc = crc.get_crc(); - crc.digest(&payload[..]); - // let base_crc = crc.get_crc(); - let extra_crc = M::extra_crc(msgid); - // println!("write header_crc: {} base_crc: {} extra_crc: {}", - // header_crc, base_crc, extra_crc); - crc.digest(&[extra_crc]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; + let mut message_raw = MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len])?; Ok(len) } @@ -493,30 +589,13 @@ pub fn write_v1_msg( header: MavHeader, data: &M, ) -> Result { - let msgid = data.message_id(); - let payload = data.ser(MavlinkVersion::V1); - - let header = &[ - MAV_STX, - payload.len() as u8, - header.sequence, - header.system_id, - header.component_id, - msgid as u8, - ]; - - let mut crc = CRCu16::crc16mcrf4cc(); - crc.digest(&header[1..]); - crc.digest(&payload[..]); - crc.digest(&[M::extra_crc(msgid)]); - - let crc_bytes = crc.get_crc().to_le_bytes(); - - let len = payload.len() + header.len() + crc_bytes.len(); - - w.write_all(header)?; - w.write_all(&payload[..])?; - w.write_all(&crc_bytes)?; + let mut message_raw = MAVLinkV1MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len])?; Ok(len) } diff --git a/src/utils.rs b/src/utils.rs index d89058526a..2f43bbbbac 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,14 +1,10 @@ -use crate::{bytes_mut::BytesMut, MAX_FRAME_SIZE}; - /// Removes the trailing zeroes in the payload /// /// # Note: /// /// There must always be at least one remaining byte even if it is a /// zero byte. -#[allow(dead_code)] -pub(crate) fn remove_trailing_zeroes(buf: &mut BytesMut) { - let data = &**buf; +pub(crate) fn remove_trailing_zeroes<'a>(data: &mut [u8]) -> usize { let mut len = data.len(); for b in data[1..].iter().rev() { @@ -19,5 +15,5 @@ pub(crate) fn remove_trailing_zeroes(buf: &mut BytesMut) { len -= 1; } - buf.set_len(len); + len } From f24fa89201f82030eb6f42c0f0ee59b9b2118a17 Mon Sep 17 00:00:00 2001 From: Jonathan Plasse <13716151+JonathanPlasse@users.noreply.github.com> Date: Tue, 20 Dec 2022 17:05:56 +0100 Subject: [PATCH 035/159] Fix enum value generation --- build/parser.rs | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index d9e6d2b9ec..c17a8ac497 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -364,10 +364,6 @@ impl MavEnum { } } - fn has_enum_values(&self) -> bool { - self.entries.iter().all(|x| x.value.is_some()) - } - fn emit_defs(&self) -> Vec { let mut cnt = 0isize; self.entries @@ -386,12 +382,13 @@ impl MavEnum { #[cfg(not(feature = "emit-description"))] let description = quote!(); - if !self.has_enum_values() { - value = quote!(#cnt); + if enum_entry.value.is_none() { cnt += 1; + value = quote!(#cnt); } else { - let tmp = - TokenStream::from_str(&enum_entry.value.unwrap().to_string()).unwrap(); + let tmp_value = enum_entry.value.unwrap(); + cnt = cnt.max(tmp_value as isize); + let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); value = quote!(#tmp); }; if self.bitfield.is_some() { From 7ccd09ee0a640a92257581a3e410fd08f3520ea9 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Tue, 17 Jan 2023 08:48:56 +0000 Subject: [PATCH 036/159] use inline format args (clippy::uninlined_format_args) --- build/binder.rs | 2 +- build/main.rs | 6 +++--- build/parser.rs | 16 ++++++++-------- src/bin/mavlink-dump.rs | 6 +++--- src/connection/tcp.rs | 2 +- src/error.rs | 14 ++++++-------- tests/process_log_files.rs | 4 ++-- 7 files changed, 24 insertions(+), 26 deletions(-) diff --git a/build/binder.rs b/build/binder.rs index cb9e6024fa..d03304dd1e 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -19,5 +19,5 @@ pub fn generate(modules: Vec, out: &mut W) { #(#modules_tokens)* }; - writeln!(out, "{}", tokens).unwrap(); + writeln!(out, "{tokens}").unwrap(); } diff --git a/build/main.rs b/build/main.rs index a32f33e6d5..0328c5b245 100644 --- a/build/main.rs +++ b/build/main.rs @@ -22,7 +22,7 @@ pub fn main() { .current_dir(src_dir) .status() { - eprintln!("{}", error); + eprintln!("{error}"); } // find & apply patches to XML definitions to avoid crashes @@ -39,7 +39,7 @@ pub fn main() { .current_dir(&mavlink_dir) .status() { - eprintln!("{}", error); + eprintln!("{error}"); } } } @@ -90,6 +90,6 @@ pub fn main() { fn format_code(cwd: impl AsRef, path: impl AsRef) { if let Err(error) = Command::new("rustfmt").arg(path).current_dir(cwd).status() { - eprintln!("{}", error); + eprintln!("{error}"); } } diff --git a/build/parser.rs b/build/parser.rs index c17a8ac497..a0797e8cc7 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -418,7 +418,7 @@ impl MavEnum { #[cfg(feature = "emit-description")] let description = if let Some(description) = self.description.as_ref() { - let desc = format!("{}", description); + let desc = format!("{description}"); quote!(#[doc = #desc]) } else { quote!() @@ -530,7 +530,7 @@ impl MavMessage { let desc = format!("id: {}", self.id); ts.extend(quote!(#[doc = #desc])); if let Some(val) = self.description.clone() { - let doc = &format!("{}.", val); + let doc = &format!("{val}."); ts.extend(quote!(#[doc = #doc])); } ts @@ -658,7 +658,7 @@ impl MavField { fn emit_description(&self) -> TokenStream { let mut ts = TokenStream::new(); if let Some(val) = self.description.clone() { - let desc = format!("{}.", val); + let desc = format!("{val}."); ts.extend(quote!(#[doc = #desc])); } ts @@ -1257,13 +1257,13 @@ pub fn parse_profile( include = s.replace('\n', ""); } (Some(&Version), Some(&Mavlink)) => { - eprintln!("TODO: version {:?}", s); + eprintln!("TODO: version {s:?}"); } (Some(&Dialect), Some(&Mavlink)) => { - eprintln!("TODO: dialect {:?}", s); + eprintln!("TODO: dialect {s:?}"); } (Some(Deprecated), _) => { - eprintln!("TODO: deprecated {:?}", s); + eprintln!("TODO: deprecated {s:?}"); } data => { panic!("unexpected text data {:?} reading {:?}", data, s); @@ -1318,7 +1318,7 @@ pub fn parse_profile( // println!("{}-{}", indent(depth), name); } Err(e) => { - eprintln!("Error: {}", e); + eprintln!("Error: {e}"); break; } _ => {} @@ -1337,7 +1337,7 @@ pub fn generate(definitions_dir: &Path, definition_file: &String, outp // rust file let rust_tokens = profile.emit_rust(); - writeln!(output_rust, "{}", rust_tokens).unwrap(); + writeln!(output_rust, "{rust_tokens}").unwrap(); } /// CRC operates over names of the message and names of its fields diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index 845538f442..2f262af9f9 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -38,7 +38,7 @@ fn main() { if res.is_ok() { thread::sleep(Duration::from_secs(1)); } else { - println!("send failed: {:?}", res); + println!("send failed: {res:?}"); } } }); @@ -46,7 +46,7 @@ fn main() { loop { match vehicle.recv() { Ok((_header, msg)) => { - println!("received: {:?}", msg); + println!("received: {msg:?}"); } Err(MessageReadError::Io(e)) => { if let std::io::ErrorKind::WouldBlock = e.kind() { @@ -54,7 +54,7 @@ fn main() { thread::sleep(Duration::from_secs(1)); continue; } else { - println!("recv error: {:?}", e); + println!("recv error: {e:?}"); break; } } diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs index 547bdeca0b..491dff0129 100644 --- a/src/connection/tcp.rs +++ b/src/connection/tcp.rs @@ -67,7 +67,7 @@ pub fn tcpin(address: T) -> io::Result { } Err(e) => { //TODO don't println in lib - println!("listener err: {}", e); + println!("listener err: {e}"); } } } diff --git a/src/error.rs b/src/error.rs index 5d0cc82825..a8f74f2f65 100644 --- a/src/error.rs +++ b/src/error.rs @@ -14,15 +14,13 @@ impl Display for ParserError { match self { ParserError::InvalidFlag { flag_type, value } => write!( f, - "Invalid flag value for flag type {:?}, got {:?}", - flag_type, value + "Invalid flag value for flag type {flag_type:?}, got {value:?}" ), ParserError::InvalidEnum { enum_type, value } => write!( f, - "Invalid enum value for enum type {:?}, got {:?}", - enum_type, value + "Invalid enum value for enum type {enum_type:?}, got {value:?}" ), - ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {:?}", id), + ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {id:?}"), } } } @@ -43,10 +41,10 @@ impl Display for MessageReadError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { #[cfg(feature = "std")] - Self::Io(e) => write!(f, "Failed to read message: {:#?}", e), + Self::Io(e) => write!(f, "Failed to read message: {e:#?}"), #[cfg(feature = "embedded")] Self::Io => write!(f, "Failed to read message"), - Self::Parse(e) => write!(f, "Failed to read message: {:#?}", e), + Self::Parse(e) => write!(f, "Failed to read message: {e:#?}"), } } } @@ -79,7 +77,7 @@ impl Display for MessageWriteError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { #[cfg(feature = "std")] - Self::Io(e) => write!(f, "Failed to write message: {:#?}", e), + Self::Io(e) => write!(f, "Failed to write message: {e:#?}"), #[cfg(feature = "embedded")] Self::Io => write!(f, "Failed to write message"), } diff --git a/tests/process_log_files.rs b/tests/process_log_files.rs index f45d9f8d02..6492e3d891 100644 --- a/tests/process_log_files.rs +++ b/tests/process_log_files.rs @@ -39,7 +39,7 @@ mod process_files { if let std::io::ErrorKind::WouldBlock = e.kind() { continue; } else { - println!("recv error: {:?}", e); + println!("recv error: {e:?}"); break; } } @@ -47,7 +47,7 @@ mod process_files { } } - println!("Number of parsed messages: {}", counter); + println!("Number of parsed messages: {counter}"); assert!( counter == 1374, "Unable to hit the necessary amount of matches" From c0417b5cf20d0edd418d2f018163eccb1b8e9deb Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Tue, 17 Jan 2023 08:45:38 +0000 Subject: [PATCH 037/159] move msrv from clippy config to Cargo.toml --- .clippy.toml | 1 - Cargo.toml | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 100644 .clippy.toml diff --git a/.clippy.toml b/.clippy.toml deleted file mode 100644 index 16caf02ee9..0000000000 --- a/.clippy.toml +++ /dev/null @@ -1 +0,0 @@ -msrv = "1.60.0" diff --git a/Cargo.toml b/Cargo.toml index a9e411c7f0..cb34fb1441 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,7 @@ readme = "README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition = "2018" +rust-version = "1.60.0" [build-dependencies] crc-any = { version = "2.3.0", default-features = false } From c60c4827d37bf8686ebffe7b8f434edf21800a77 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:43:20 +0000 Subject: [PATCH 038/159] use inline format args --- tests/process_log_files.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/process_log_files.rs b/tests/process_log_files.rs index 6492e3d891..a38665fd8f 100644 --- a/tests/process_log_files.rs +++ b/tests/process_log_files.rs @@ -17,8 +17,8 @@ mod process_files { let filename = filename.to_str().unwrap(); dbg!(filename); - println!("Processing file: {}", &filename); - let connection_string = format!("file:{}", &filename); + println!("Processing file: {filename}"); + let connection_string = format!("file:{filename}"); // Process file process_file(&connection_string); From 4d84db59c67593505eb87211e73e6866642ed557 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:44:34 +0000 Subject: [PATCH 039/159] remove extra unused lifetimes --- src/utils.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils.rs b/src/utils.rs index 2f43bbbbac..1b814023bc 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -4,7 +4,7 @@ /// /// There must always be at least one remaining byte even if it is a /// zero byte. -pub(crate) fn remove_trailing_zeroes<'a>(data: &mut [u8]) -> usize { +pub(crate) fn remove_trailing_zeroes(data: &mut [u8]) -> usize { let mut len = data.len(); for b in data[1..].iter().rev() { From d14508fce49ad6d6abf25e00a7c5fc211dffd2c8 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:48:58 +0000 Subject: [PATCH 040/159] suppress 'clippy::unnecessary_cast' lint for generated code --- build/binder.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/build/binder.rs b/build/binder.rs index d03304dd1e..be68f7a9c3 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -10,6 +10,7 @@ pub fn generate(modules: Vec, out: &mut W) { #[allow(clippy::derive_partial_eq_without_eq)] #[allow(clippy::field_reassign_with_default)] #[allow(non_snake_case)] + #[allow(clippy::unnecessary_cast)] #[cfg(feature = #module)] pub mod #module_ident; } From 4cb809d023b6e76a59fc380c450bd9729351cf79 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:51:16 +0000 Subject: [PATCH 041/159] add 'is_empty' method where types have a 'len' method --- src/bytes_mut.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index a3b70ad45c..dab587f4d1 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -13,6 +13,11 @@ impl<'a> BytesMut<'a> { self.len } + #[inline] + pub fn is_empty(&self) -> bool { + self.len() == 0 + } + #[inline] pub fn remaining(&self) -> usize { self.data.len() - self.len From e366da468e79549006452237fb6ef016bbe15a28 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:52:51 +0000 Subject: [PATCH 042/159] deny 'clippy::all' warnings --- src/lib.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib.rs b/src/lib.rs index 40c14f200e..1337f1f8b6 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -19,6 +19,7 @@ //! feature without also using the `uavionix` and `icarous` features. //! #![cfg_attr(not(feature = "std"), no_std)] +#![deny(clippy::all)] use core::result::Result; From c8349f3993cb0d744a73659d4e33877e858f85c2 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Dec 2022 09:54:13 +0000 Subject: [PATCH 043/159] use 'Self' keyword to refer to own type --- build/parser.rs | 16 ++++++++-------- src/connection/udp.rs | 8 ++++---- src/error.rs | 6 +++--- src/lib.rs | 1 + 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index a0797e8cc7..77dcf9ede2 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -229,9 +229,9 @@ impl MavProfile { let id_width = format_ident!("u32"); quote! { - fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { + fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { - #(#ids => #structs::deser(version, payload).map(MavMessage::#enums),)* + #(#ids => #structs::deser(version, payload).map(Self::#enums),)* _ => { Err(ParserError::UnknownMessage { id }) }, @@ -267,7 +267,7 @@ impl MavProfile { quote! { fn message_name(&self) -> &'static str { match self { - #(MavMessage::#enums(..) => #enum_names,)* + #(Self::#enums(..) => #enum_names,)* } } } @@ -278,7 +278,7 @@ impl MavProfile { quote! { fn message_id(&self) -> #id_width { match self { - #(MavMessage::#enums(..) => #ids,)* + #(Self::#enums(..) => #ids,)* } } } @@ -317,9 +317,9 @@ impl MavProfile { }); quote! { - fn default_message_from_id(id: u32) -> Result { + fn default_message_from_id(id: u32) -> Result { match id { - #(#ids => Ok(MavMessage::#enums(#data_name::default())),)* + #(#ids => Ok(Self::#enums(#data_name::default())),)* _ => { Err("Invalid message id.") } @@ -331,8 +331,8 @@ impl MavProfile { fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { quote! { fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { - match *self { - #(MavMessage::#enums(ref body) => body.ser(version, bytes),)* + match self { + #(Self::#enums(body) => body.ser(version, bytes),)* } } } diff --git a/src/connection/udp.rs b/src/connection/udp.rs index adf3b2d4d3..3bcd05a6f2 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -73,10 +73,10 @@ struct PacketBuf { } impl PacketBuf { - pub fn new() -> PacketBuf { + pub fn new() -> Self { let mut v = Vec::new(); v.resize(65536, 0); - PacketBuf { + Self { buf: v, start: 0, end: 0, @@ -123,8 +123,8 @@ pub struct UdpConnection { } impl UdpConnection { - fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { - Ok(UdpConnection { + fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { + Ok(Self { server, reader: Mutex::new(UdpRead { socket: socket.try_clone()?, diff --git a/src/error.rs b/src/error.rs index a8f74f2f65..b60136bd09 100644 --- a/src/error.rs +++ b/src/error.rs @@ -12,15 +12,15 @@ pub enum ParserError { impl Display for ParserError { fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { match self { - ParserError::InvalidFlag { flag_type, value } => write!( + Self::InvalidFlag { flag_type, value } => write!( f, "Invalid flag value for flag type {flag_type:?}, got {value:?}" ), - ParserError::InvalidEnum { enum_type, value } => write!( + Self::InvalidEnum { enum_type, value } => write!( f, "Invalid enum value for enum type {enum_type:?}, got {value:?}" ), - ParserError::UnknownMessage { id } => write!(f, "Unknown message with ID {id:?}"), + Self::UnknownMessage { id } => write!(f, "Unknown message with ID {id:?}"), } } } diff --git a/src/lib.rs b/src/lib.rs index 1337f1f8b6..eb2e3616b5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -20,6 +20,7 @@ //! #![cfg_attr(not(feature = "std"), no_std)] #![deny(clippy::all)] +#![warn(clippy::use_self)] use core::result::Result; From 6dfd5f7ab580ef0e640358963aedf5de4a4a5259 Mon Sep 17 00:00:00 2001 From: "daniel.eades" Date: Thu, 16 Feb 2023 11:28:11 +0000 Subject: [PATCH 044/159] use 'assert' --- build/parser.rs | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 77dcf9ede2..94415d60a9 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -28,12 +28,11 @@ impl MavProfile { fn add_message(&mut self, message: &MavMessage) { match self.messages.entry(message.name.clone()) { Entry::Occupied(entry) => { - if entry.get() != message { - panic!( - "Message '{}' defined twice but definitions are different", - message.name - ); - } + assert!( + entry.get() == message, + "Message '{}' defined twice but definitions are different", + message.name + ); } Entry::Vacant(entry) => { entry.insert(message.clone()); From 2f2392556b9f72652d3853b3c21975eec8e35706 Mon Sep 17 00:00:00 2001 From: Valdemar Erk Date: Mon, 20 Mar 2023 13:42:21 +0100 Subject: [PATCH 045/159] refactor the internals of Bytes and add i/u24 support --- src/bytes.rs | 87 ++++++++++++++++++++---------------------------- src/bytes_mut.rs | 42 +++++++++++++++++++++++ 2 files changed, 79 insertions(+), 50 deletions(-) diff --git a/src/bytes.rs b/src/bytes.rs index d6b7fdcd8b..56597f7f59 100644 --- a/src/bytes.rs +++ b/src/bytes.rs @@ -34,6 +34,17 @@ impl<'a> Bytes<'a> { bytes } + pub fn get_array(&mut self) -> [u8; SIZE] { + let bytes = self.get_bytes(SIZE); + let mut arr = [0u8; SIZE]; + + arr.copy_from_slice(bytes); + + debug_assert_eq!(arr.as_slice(), bytes); + + arr + } + pub fn get_u8(&mut self) -> u8 { self.check_remaining(1); @@ -51,82 +62,58 @@ impl<'a> Bytes<'a> { } pub fn get_u16_le(&mut self) -> u16 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); - - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - u16::from_le_bytes(val) + u16::from_le_bytes(self.get_array()) } pub fn get_i16_le(&mut self) -> i16 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); - - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - i16::from_le_bytes(val) + i16::from_le_bytes(self.get_array()) } - pub fn get_u32_le(&mut self) -> u32 { - const SIZE: usize = core::mem::size_of::(); + pub fn get_u24_le(&mut self) -> u32 { + const SIZE: usize = 3; self.check_remaining(SIZE); - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + let mut val = [0u8; SIZE + 1]; + val[..3].copy_from_slice(&self.data[self.pos..self.pos + SIZE]); self.pos += SIZE; + + debug_assert_eq!(val[3], 0); u32::from_le_bytes(val) } - pub fn get_i32_le(&mut self) -> i32 { - const SIZE: usize = core::mem::size_of::(); + pub fn get_i24_le(&mut self) -> i32 { + const SIZE: usize = 3; self.check_remaining(SIZE); - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); + let mut val = [0u8; SIZE + 1]; + val[..3].copy_from_slice(&self.data[self.pos..self.pos + SIZE]); self.pos += SIZE; + + debug_assert_eq!(val[3], 0); i32::from_le_bytes(val) } - pub fn get_u64_le(&mut self) -> u64 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); + pub fn get_u32_le(&mut self) -> u32 { + u32::from_le_bytes(self.get_array()) + } - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - u64::from_le_bytes(val) + pub fn get_i32_le(&mut self) -> i32 { + i32::from_le_bytes(self.get_array()) } - pub fn get_i64_le(&mut self) -> i64 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); + pub fn get_u64_le(&mut self) -> u64 { + u64::from_le_bytes(self.get_array()) + } - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - i64::from_le_bytes(val) + pub fn get_i64_le(&mut self) -> i64 { + i64::from_le_bytes(self.get_array()) } pub fn get_f32_le(&mut self) -> f32 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); - - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - f32::from_le_bytes(val) + f32::from_le_bytes(self.get_array()) } pub fn get_f64_le(&mut self) -> f64 { - const SIZE: usize = core::mem::size_of::(); - self.check_remaining(SIZE); - - let mut val = [0u8; SIZE]; - val.copy_from_slice(&self.data[self.pos..self.pos + SIZE]); - self.pos += SIZE; - f64::from_le_bytes(val) + f64::from_le_bytes(self.get_array()) } } diff --git a/src/bytes_mut.rs b/src/bytes_mut.rs index dab587f4d1..aa01c351a6 100644 --- a/src/bytes_mut.rs +++ b/src/bytes_mut.rs @@ -74,6 +74,48 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + pub fn put_u24_le(&mut self, val: u32) { + const SIZE: usize = 3; + const MAX: u32 = 2u32.pow(24) - 1; + + assert!( + val <= MAX, + "Attempted to put value that is too large for 24 bits, \ + attempted to push: {}, max allowed: {}", + val, + MAX + ); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..3]); + self.len += SIZE; + } + + pub fn put_i24_le(&mut self, val: i32) { + const SIZE: usize = 3; + const MIN: i32 = 2i32.pow(23); + const MAX: i32 = 2i32.pow(23) - 1; + + assert!( + val <= MAX, + "Attempted to put value that is too large for 24 bits, \ + attempted to push: {}, max allowed: {}", + val, + MAX + ); + assert!( + val >= MIN, + "Attempted to put value that is too negative for 24 bits, \ + attempted to push: {}, min allowed: {}", + val, + MIN + ); + + let src = val.to_le_bytes(); + self.data[self.len..self.len + SIZE].copy_from_slice(&src[..3]); + self.len += SIZE; + } + pub fn put_u32_le(&mut self, val: u32) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); From 240551bda2ab22923badd8a3beb36e8082181cd0 Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Mon, 20 Mar 2023 11:19:51 -0300 Subject: [PATCH 046/159] perf: improve code generation performance Use `BufWriter` and avoid formatting generated code for faster build times --- Cargo.toml | 3 ++- build/main.rs | 14 ++++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index cb34fb1441..6001d7401c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -68,6 +68,7 @@ heapless = "0.7" "common", ] +"format-generated-code" = [] "emit-description" = [] "emit-extensions" = [] "std" = ["byteorder/std"] @@ -81,4 +82,4 @@ default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmeg # 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"] +features = ["default", "all-dialects", "emit-description", "emit-extensions", "format-generated-code"] diff --git a/build/main.rs b/build/main.rs index 0328c5b245..6c14eea2c8 100644 --- a/build/main.rs +++ b/build/main.rs @@ -8,6 +8,7 @@ use crate::util::to_module_name; use std::env; use std::ffi::OsStr; use std::fs::{read_dir, File}; +use std::io::BufWriter; use std::path::{Path, PathBuf}; use std::process::Command; @@ -63,7 +64,7 @@ pub fn main() { modules.push(module_name); let dest_path = Path::new(&out_dir).join(definition_rs); - let mut outf = File::create(&dest_path).unwrap(); + let mut outf = BufWriter::new(File::create(&dest_path).unwrap()); // generate code parser::generate( @@ -71,7 +72,7 @@ pub fn main() { &definition_file.into_string().unwrap(), &mut outf, ); - format_code(&out_dir, &dest_path); + dbg_format_code(&out_dir, &dest_path); // Re-run build if definition file changes println!("cargo:rerun-if-changed={}", entry.path().to_string_lossy()); @@ -84,12 +85,17 @@ pub fn main() { // generate code binder::generate(modules, &mut outf); - format_code(out_dir, dest_path); + dbg_format_code(out_dir, dest_path); } } -fn format_code(cwd: impl AsRef, path: impl AsRef) { +#[cfg(feature = "format-generated-code")] +fn dbg_format_code(cwd: impl AsRef, path: impl AsRef) { if let Err(error) = Command::new("rustfmt").arg(path).current_dir(cwd).status() { eprintln!("{error}"); } } + +// Does nothing +#[cfg(not(feature = "format-generated-code"))] +fn dbg_format_code(_: impl AsRef, _: impl AsRef) {} From 0d99c6510c0c5b73c01d1c1c4a79d39fdb74b1d1 Mon Sep 17 00:00:00 2001 From: Ben Wolsieffer Date: Wed, 29 Mar 2023 21:08:15 -0400 Subject: [PATCH 047/159] Generate bitmask enum arrays as primitive arrays This enables the parser to support the latest mavlink XML. The array fields are only generated as primitive arrays, rather than typed enum arrays, but this matches the existing behavior of non-bitmask enum arrays. Eventually, both of these cases should be fixed. --- build/parser.rs | 47 +++++++++++++++++++++++++++++++---------------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 94415d60a9..ad24c6f7e1 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -66,7 +66,7 @@ impl MavProfile { for mut enm in self.enums.values_mut() { if enm.name == *enum_name { // this is the right enum - enm.bitfield = Some(field.mavtype.rust_type()); + enm.bitfield = Some(field.mavtype.rust_primitive_type()); } } } @@ -674,21 +674,23 @@ impl MavField { fn rust_writer(&self) -> TokenStream { let mut name = "self.".to_string() + &self.name.clone(); if self.enumtype.is_some() { - if let Some(dsp) = &self.display { - // potentially a bitflag - if dsp == "bitmask" { - // it is a bitflag - name += ".bits()"; + // casts are not necessary for arrays, because they are currently + // generated as primitive arrays + if !matches!(self.mavtype, MavType::Array(_, _)) { + if let Some(dsp) = &self.display { + // potentially a bitflag + if dsp == "bitmask" { + // it is a bitflag + name += ".bits()"; + } else { + panic!("Display option not implemented"); + } } else { - panic!("Display option not implemented"); + // an enum, have to use "*foo as u8" cast + name += " as "; + name += &self.mavtype.rust_type(); } } - // cast are not necessary for arrays - else if !matches!(self.mavtype, MavType::Array(_, _)) { - // an enum, have to use "*foo as u8" cast - name += " as "; - name += &self.mavtype.rust_type(); - } } let ts = TokenStream::from_str(&name).unwrap(); let name = quote!(#ts); @@ -703,6 +705,11 @@ impl MavField { let name = quote!(_struct.#_name); let buf = format_ident!("buf"); if let Some(enum_name) = &self.enumtype { + // TODO: handle enum arrays properly, rather than just generating + // primitive arrays + if let MavType::Array(_t, _size) = &self.mavtype { + return self.mavtype.rust_reader(&name, buf); + } if let Some(dsp) = &self.display { if dsp == "bitmask" { // bitflags @@ -717,9 +724,6 @@ impl MavField { panic!("Display option not implemented"); } } else { - if let MavType::Array(_t, _size) = &self.mavtype { - return self.mavtype.rust_reader(&name, buf); - } // handle enum by FromPrimitive let tmp = self.mavtype.rust_reader("e!(let tmp), buf); let val = format_ident!("from_{}", &self.mavtype.rust_type()); @@ -929,6 +933,17 @@ impl MavType { } } + /// Return rust equivalent of the primitive type of a MavType. The primitive + /// type is the type itself for all except arrays, in which case it is the + /// element type. + pub fn rust_primitive_type(&self) -> String { + use self::MavType::*; + match self { + Array(t, _) => t.rust_primitive_type(), + _ => self.rust_type(), + } + } + /// Compare two MavTypes pub fn compare(&self, other: &Self) -> Ordering { let len = self.order_len(); From 4654ee643b5483202722b91ae478b2afd16adae7 Mon Sep 17 00:00:00 2001 From: Ben Wolsieffer Date: Wed, 29 Mar 2023 21:12:40 -0400 Subject: [PATCH 048/159] Update mavlink submodule --- mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink b/mavlink index 6723147f26..d6d86d3f0c 160000 --- a/mavlink +++ b/mavlink @@ -1 +1 @@ -Subproject commit 6723147f26d2625383a588c88633c568c9e9b929 +Subproject commit d6d86d3f0c90a67c275d6833931f7c712b858dba From 19245dd754a95823f4e532e04f50c8a556ec7855 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Fri, 31 Mar 2023 16:12:46 -0300 Subject: [PATCH 049/159] Cargo: Update to 0.11.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 6001d7401c..a7415937b1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.10.1" +version = "0.11.0" 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." From 0254791b2436001c5ca6c8268d5666dd5d0aa03e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 1 Apr 2023 10:57:52 -0300 Subject: [PATCH 050/159] Cargo: Update dialects MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index a7415937b1..cf96b2b7c6 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -36,9 +36,25 @@ nb = { version = "1.0", optional = true } heapless = "0.7" [features] +"all" = [ + "ardupilotmega", + "asluav", + "common", + "development", + "icarous", + "minimal", + "python_array_test", + "standard", + "test", + "ualberta", + "uavionix", + "avssuas", + "cubepilot", +] "ardupilotmega" = ["common", "icarous", "uavionix"] "asluav" = ["common"] -"autoquad" = ["common"] +"avssuas" = ["common"] +"development" = ["common"] "matrixpilot" = ["common"] "minimal" = [] "paparazzi" = ["common"] @@ -50,11 +66,13 @@ heapless = "0.7" "uavionix" = ["common"] "icarous" = [] "common" = [] +"cubepilot" = ["common"] "all-dialects" = [ "ardupilotmega", "asluav", - "autoquad", + "avssuas", + "development", "matrixpilot", "minimal", "paparazzi", @@ -66,6 +84,7 @@ heapless = "0.7" "uavionix", "icarous", "common", + "cubepilot", ] "format-generated-code" = [] From 72a2c4e009a482fc247c620f2c55907bae5a2633 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sat, 1 Apr 2023 13:23:07 -0300 Subject: [PATCH 051/159] Cargo: Update to 0.11.1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index cf96b2b7c6..9aaa757cd8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.11.0" +version = "0.11.1" 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." From a86807c9dcf22914d5408a65b091220a443034a6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 19 Apr 2023 13:32:56 -0300 Subject: [PATCH 052/159] parser: Remove dbg from quote call MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This call was increasing the build log from 20kB to 10MB, that's probably why rust docs are failing to build on their CI Signed-off-by: Patrick José Pereira --- build/parser.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index ad24c6f7e1..a20b7b7a33 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -450,7 +450,7 @@ impl MavEnum { }; } - dbg!(quote! { + quote! { #enum_def impl Default for #enum_name { @@ -458,7 +458,7 @@ impl MavEnum { Self::#default } } - }) + } } } From 87741610bed15e052adfb08f8bfaf998b93244c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Wed, 19 Apr 2023 14:07:47 -0300 Subject: [PATCH 053/159] Cargo: Update to 0.11.2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 9aaa757cd8..7121cf522f 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.11.1" +version = "0.11.2" 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." From bb502ede9fe562d4ac97476544375c98359d7f02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 20 Apr 2023 17:24:28 -0300 Subject: [PATCH 054/159] ci: Add documentation deployment MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/test.yml | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 6b19c12bb3..b6fddab8f1 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -131,3 +131,21 @@ jobs: override: true - name: Build run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options + + docs: + needs: internal-tests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + - uses: actions-rs/toolchain@v1.0.6 + with: + toolchain: stable + override: true + - name: Build docs + run: cargo doc + - name: Deploy + uses: peaceiris/actions-gh-pages@v3 + if: ${{ github.ref == 'refs/heads/master' }} + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_dir: ./target/doc \ No newline at end of file From 2c27462844bed1da2d059a3123a4103f2d6bd562 Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Mon, 20 Mar 2023 10:33:27 -0300 Subject: [PATCH 055/159] remove heapless dependency --- Cargo.toml | 6 +-- build/parser.rs | 118 +++++++++++++++++++++++++++++++++++------------- src/lib.rs | 24 ++++------ 3 files changed, 99 insertions(+), 49 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 7121cf522f..4f61729579 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -33,7 +33,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } embedded-hal = { version = "0.2", optional = true } nb = { version = "1.0", optional = true } -heapless = "0.7" +serde_arrays = { version = "0.1.0", optional = true } [features] "all" = [ @@ -95,8 +95,8 @@ heapless = "0.7" "tcp" = [] "direct-serial" = [] "embedded" = ["embedded-hal", "nb"] -"serde" = ["dep:serde", "heapless/serde"] -default= ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"] +"serde" = ["dep:serde", "dep:serde_arrays"] +default = ["std", "tcp", "udp", "direct-serial", "serial", "serde", "ardupilotmega"] # build with all features on docs.rs so that users viewing documentation # can see everything diff --git a/build/parser.rs b/build/parser.rs index a20b7b7a33..5af014f4fc 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -178,8 +178,6 @@ impl MavProfile { use num_traits::ToPrimitive; #[allow(unused_imports)] use bitflags::bitflags; - #[allow(unused_imports)] - use heapless::Vec; use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut}; @@ -410,10 +408,15 @@ impl MavEnum { quote!(#name) } + fn emit_const_default(&self) -> TokenStream { + let default = format_ident!("{}", self.entries[0].name); + quote!(pub const DEFAULT: Self = Self::#default;) + } + fn emit_rust(&self) -> TokenStream { let defs = self.emit_defs(); - let default = format_ident!("{}", self.entries[0].name); let enum_name = self.emit_name(); + let const_default = self.emit_const_default(); #[cfg(feature = "emit-description")] let description = if let Some(description) = self.description.as_ref() { @@ -453,9 +456,13 @@ impl MavEnum { quote! { #enum_def + impl #enum_name { + #const_default + } + impl Default for #enum_name { fn default() -> Self { - Self::#default + Self::DEFAULT } } } @@ -512,9 +519,16 @@ impl MavMessage { quote!() }; + let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) { + quote!(#[cfg_attr(feature = "serde", serde(with = "serde_arrays"))]) + } else { + quote!() + }; + quote! { #description #serde_default + #serde_with_attr #nametype } }) @@ -582,12 +596,33 @@ impl MavMessage { } } + fn emit_default_impl(&self) -> TokenStream { + let msg_name = self.emit_struct_name(); + quote! { + impl Default for #msg_name { + fn default() -> Self { + Self::DEFAULT.clone() + } + } + } + } + + fn emit_const_default(&self) -> TokenStream { + let initializers = self + .fields + .iter() + .map(|field| field.emit_default_initializer()); + quote!(pub const DEFAULT: Self = Self { #(#initializers)* };) + } + fn emit_rust(&self) -> TokenStream { let msg_name = self.emit_struct_name(); let (name_types, msg_encoded_len) = self.emit_name_types(); let deser_vars = self.emit_deserialize_vars(); let serialize_vars = self.emit_serialize_vars(); + let const_default = self.emit_const_default(); + let default_impl = self.emit_default_impl(); #[cfg(feature = "emit-description")] let description = self.emit_description(); @@ -597,7 +632,7 @@ impl MavMessage { quote! { #description - #[derive(Debug, Clone, PartialEq, Default)] + #[derive(Debug, Clone, PartialEq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct #msg_name { #(#name_types)* @@ -605,6 +640,7 @@ impl MavMessage { impl #msg_name { pub const ENCODED_LEN: usize = #msg_encoded_len; + #const_default pub fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { #deser_vars @@ -614,6 +650,8 @@ impl MavMessage { #serialize_vars } } + + #default_impl } } } @@ -737,6 +775,21 @@ impl MavField { self.mavtype.rust_reader(&name, buf) } } + + fn emit_default_initializer(&self) -> TokenStream { + let field = self.emit_name(); + // FIXME: Is this actually expected behaviour?? + if matches!(self.mavtype, MavType::Array(_, _)) { + let default_value = self.mavtype.emit_default_value(); + quote!(#field: #default_value,) + } else if let Some(ref enumname) = self.enumtype { + let ty = TokenStream::from_str(enumname).unwrap(); + quote!(#field: #ty::DEFAULT,) + } else { + let default_value = self.mavtype.emit_default_value(); + quote!(#field: #default_value,) + } + } } #[derive(Debug, PartialEq, Clone)] @@ -809,24 +862,12 @@ impl MavType { Int64 => quote! {#val = #buf.get_i64_le();}, Float => quote! {#val = #buf.get_f32_le();}, Double => quote! {#val = #buf.get_f64_le();}, - Array(t, size) => { - if size > 32 { - // it is a vector - let r = t.rust_reader("e!(let val), buf); - quote! { - for _ in 0..#size { - #r - #val.push(val).unwrap(); - } - } - } else { - // handle as a slice - let r = t.rust_reader("e!(let val), buf); - quote! { - for idx in 0..#size { - #r - #val[idx] = val; - } + Array(t, _) => { + let r = t.rust_reader("e!(let val), buf); + quote! { + for v in &mut #val { + #r + *v = val; } } } @@ -920,15 +961,28 @@ impl MavType { UInt64 => "u64".into(), Int64 => "i64".into(), Double => "f64".into(), - Array(t, size) => { - // format!("[{};{}]", t.rust_type(), size) - if size > 32 { - // we have to use a vector to make our lives easier - format!("Vec<{}, {}>", t.rust_type(), size) - } else { - // we can use a slice, as Rust derives lot of things for slices <= 32 elements - format!("[{};{}]", t.rust_type(), size) - } + Array(t, size) => format!("[{};{}]", t.rust_type(), size), + } + } + + pub fn emit_default_value(&self) -> TokenStream { + use self::MavType::*; + + match self { + UInt8 | UInt8MavlinkVersion => quote!(0_u8), + Int8 => quote!(0_i8), + Char => quote!(0_u8), + UInt16 => quote!(0_u16), + Int16 => quote!(0_i16), + UInt32 => quote!(0_u32), + Int32 => quote!(0_i32), + Float => quote!(0.0_f32), + UInt64 => quote!(0_u64), + Int64 => quote!(0_i64), + Double => quote!(0.0_f64), + Array(ty, size) => { + let default_value = ty.emit_default_value(); + quote!([#default_value; #size]) } } } diff --git a/src/lib.rs b/src/lib.rs index eb2e3616b5..3ab42ac8af 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -30,8 +30,6 @@ use std::io::{Read, Write}; #[cfg(feature = "std")] use byteorder::ReadBytesExt; -use heapless::Vec; - #[cfg(feature = "std")] mod connection; #[cfg(feature = "std")] @@ -140,32 +138,30 @@ impl MavFrame { // } /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. - pub fn ser(&self) -> Vec { + pub fn ser(&self, buf: &mut [u8]) -> usize { + let mut buf = bytes_mut::BytesMut::new(buf); + // serialize header - let mut v = Vec::from_slice(&[ - self.header.system_id, - self.header.component_id, - self.header.sequence, - ]) - .unwrap(); + buf.put_u8(self.header.system_id); + buf.put_u8(self.header.component_id); + buf.put_u8(self.header.sequence); // message id match self.protocol_version { MavlinkVersion::V2 => { let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); - v.extend_from_slice(&bytes).unwrap(); + buf.put_slice(&bytes); } MavlinkVersion::V1 => { - v.push(self.msg.message_id() as u8).unwrap(); //TODO check + buf.put_u8(self.msg.message_id() as u8); //TODO check } } // serialize message let mut payload_buf = [0u8; 255]; let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); - v.extend_from_slice(&payload_buf[..payload_len]).unwrap(); - - v + buf.put_slice(&payload_buf[..payload_len]); + buf.len() } /// Deserialize MavFrame from a slice that has been received from, for example, a socket. From ed964592c88de7f17bce09d0a075d3f1f1bb309d Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Tue, 21 Mar 2023 15:11:44 -0300 Subject: [PATCH 056/159] Update build/parser.rs Co-authored-by: danieleades <33452915+danieleades@users.noreply.github.com> --- build/parser.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build/parser.rs b/build/parser.rs index 5af014f4fc..090e0b6a2a 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -782,7 +782,7 @@ impl MavField { if matches!(self.mavtype, MavType::Array(_, _)) { let default_value = self.mavtype.emit_default_value(); quote!(#field: #default_value,) - } else if let Some(ref enumname) = self.enumtype { + } else if let Some(enumname) = &self.enumtype { let ty = TokenStream::from_str(enumname).unwrap(); quote!(#field: #ty::DEFAULT,) } else { From 5c70c2b46dace138d4cf83a36bd4194d58d07394 Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Thu, 23 Mar 2023 13:37:17 -0300 Subject: [PATCH 057/159] fix #[serde(default)] --- build/parser.rs | 6 +++- src/lib.rs | 2 +- src/utils.rs | 92 +++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 98 insertions(+), 2 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 090e0b6a2a..2dc2464053 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -514,7 +514,11 @@ impl MavMessage { // If sent by an implementation that doesn't have the extensions fields // then the recipient will see zero values for the extensions fields. let serde_default = if field.is_extension { - quote!(#[cfg_attr(feature = "serde", serde(default))]) + if field.enumtype.is_some() { + quote!(#[cfg_attr(feature = "serde", serde(default))]) + } else { + quote!(#[cfg_attr(feature = "serde", serde(default = "crate::RustDefault::rust_default"))]) + } } else { quote!() }; diff --git a/src/lib.rs b/src/lib.rs index 3ab42ac8af..5186bd8e76 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -37,7 +37,7 @@ pub use self::connection::{connect, MavConnection}; mod utils; #[allow(unused_imports)] -use utils::remove_trailing_zeroes; +use utils::{remove_trailing_zeroes, RustDefault}; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; diff --git a/src/utils.rs b/src/utils.rs index 1b814023bc..8cbd522c7f 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -17,3 +17,95 @@ pub(crate) fn remove_trailing_zeroes(data: &mut [u8]) -> usize { len } + +/// A trait very similar to `Default` but is only implemented for the equivalent Rust types to +/// `MavType`s. This is only needed because rust doesn't currently implement `Default` for arrays +/// of all sizes. In particular this trait is only ever used when the "serde" feature is enabled. +pub(crate) trait RustDefault: Copy { + fn rust_default() -> Self; +} + +impl RustDefault for [T; N] { + #[inline(always)] + fn rust_default() -> Self { + let val: T = RustDefault::rust_default(); + [val; N] + } +} + +impl RustDefault for u8 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i8 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u16 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i16 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u32 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i32 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for u64 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for i64 { + #[inline(always)] + fn rust_default() -> Self { + 0 + } +} + +impl RustDefault for char { + #[inline(always)] + fn rust_default() -> Self { + '\0' + } +} + +impl RustDefault for f32 { + #[inline(always)] + fn rust_default() -> Self { + 0.0 + } +} + +impl RustDefault for f64 { + #[inline(always)] + fn rust_default() -> Self { + 0.0 + } +} From 33edf26205f9a4e1682bcd0ffe86c48759cf2945 Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Wed, 5 Apr 2023 05:07:50 +0000 Subject: [PATCH 058/159] Add link to issue regarding Default const generics impl --- src/utils.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/utils.rs b/src/utils.rs index 8cbd522c7f..58adfc6729 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -21,6 +21,7 @@ pub(crate) fn remove_trailing_zeroes(data: &mut [u8]) -> usize { /// A trait very similar to `Default` but is only implemented for the equivalent Rust types to /// `MavType`s. This is only needed because rust doesn't currently implement `Default` for arrays /// of all sizes. In particular this trait is only ever used when the "serde" feature is enabled. +/// For more information, check out [this issue](https://users.rust-lang.org/t/issue-for-derives-for-arrays-greater-than-size-32/59055/3). pub(crate) trait RustDefault: Copy { fn rust_default() -> Self; } From 6757815e2df4b3f7098ba22e6042cefaa947910f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Tue, 11 Jul 2023 09:06:30 -0300 Subject: [PATCH 059/159] readme: Update links --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0e179fb6d6..37cf297673 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ [![Crate info](https://img.shields.io/crates/v/mavlink.svg)](https://crates.io/crates/mavlink) [![Documentation](https://docs.rs/mavlink/badge.svg)](https://docs.rs/mavlink) -Rust implementation of the [MAVLink](http://qgroundcontrol.org/mavlink/start) UAV messaging protocol, +Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol, with bindings for all message sets. Add to your Cargo.toml: @@ -24,7 +24,7 @@ cargo install mavlink ### Community projects Check some projects built by the community: - [mavlink2rest](https://github.com/patrickelectric/mavlink2rest): A REST server that provides easy and friendly access to mavlink messages. -- [mavlink-camera-manager](https://github.com/patrickelectric/mavlink-camera-manager): Extensible cross-platform camera server. +- [mavlink-camera-manager](https://github.com/mavlink/mavlink-camera-manager): Extensible cross-platform camera server. ## License From b81a2a05f83eee752881f05dc0c360ccd7a4e46c Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Mon, 3 Jul 2023 15:43:27 +0200 Subject: [PATCH 060/159] Update lib.rs comments Fix wrong links formatting --- src/lib.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 5186bd8e76..6ea0d77419 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -209,7 +209,7 @@ pub fn read_versioned_msg( } #[derive(Debug, Copy, Clone, PartialEq, Eq)] -// Follow protocol definition: https://mavlink.io/en/guide/serialization.html#v1_packet_format +// Follow protocol definition: `` pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); impl Default for MAVLinkV1MessageRaw { @@ -319,7 +319,7 @@ impl MAVLinkV1MessageRaw { } /// Return a raw buffer with the mavlink message -/// V1 maximum size is 263 bytes: https://mavlink.io/en/guide/serialization.html +/// V1 maximum size is 263 bytes: `` pub fn read_v1_raw_message( reader: &mut R, ) -> Result { @@ -371,7 +371,7 @@ pub fn read_v1_msg( const MAVLINK_IFLAG_SIGNED: u8 = 0x01; #[derive(Debug, Copy, Clone, PartialEq, Eq)] -// Follow protocol definition: https://mavlink.io/en/guide/serialization.html#mavlink2_packet_format +// Follow protocol definition: `` pub struct MAVLinkV2MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2 + Self::SIGNATURE_SIZE]); impl Default for MAVLinkV2MessageRaw { @@ -505,7 +505,7 @@ impl MAVLinkV2MessageRaw { } /// Return a raw buffer with the mavlink message -/// V2 maximum size is 280 bytes: https://mavlink.io/en/guide/serialization.html +/// V2 maximum size is 280 bytes: `` pub fn read_v2_raw_message( reader: &mut R, ) -> Result { From 12a811d6f1d3d365c9e9c677515a20fb6c66c4f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Wed, 22 Mar 2023 18:03:58 +0400 Subject: [PATCH 061/159] Add MessageData trait to allow serialize ***_DATA structs into MAVLinkV[1|2]MessageRaw --- build/parser.rs | 113 ++++++++--------------- src/lib.rs | 155 ++++++++++++++++++++++++-------- tests/v1_encode_decode_tests.rs | 11 +++ tests/v2_encode_decode_tests.rs | 11 +++ 4 files changed, 175 insertions(+), 115 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 2dc2464053..e797e3882f 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -63,7 +63,7 @@ impl MavProfile { // it is a bitmask if dsp == "bitmask" { // find the corresponding enum - for mut enm in self.enums.values_mut() { + for enm in self.enums.values_mut() { if enm.name == *enum_name { // this is the right enum enm.bitfield = Some(field.mavtype.rust_primitive_type()); @@ -121,28 +121,6 @@ impl MavProfile { .collect() } - /// A list of message IDs - fn emit_msg_ids(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let msg_id = msg.id; - quote!(#msg_id) - }) - .collect() - } - - /// CRC values needed for mavlink parsing - fn emit_msg_crc(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let ec = extra_crc(msg); - quote!(#ec) - }) - .collect() - } - fn emit_rust(&self) -> TokenStream { //TODO verify that id_width of u8 is OK even in mavlink v1 let id_width = format_ident!("u32"); @@ -152,17 +130,15 @@ impl MavProfile { let enum_names = self.emit_enum_names(); let struct_names = self.emit_struct_names(); let enums = self.emit_enums(); - let msg_ids = self.emit_msg_ids(); - let msg_crc = self.emit_msg_crc(); let mav_message = self.emit_mav_message(&enum_names, &struct_names); - let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names, &msg_ids); - let mav_message_crc = self.emit_mav_message_crc(&id_width, &msg_ids, &msg_crc); - let mav_message_name = self.emit_mav_message_name(&enum_names); - let mav_message_id = self.emit_mav_message_id(&enum_names, &msg_ids); - let mav_message_id_from_name = self.emit_mav_message_id_from_name(&enum_names, &msg_ids); + let mav_message_parse = self.emit_mav_message_parse(&enum_names, &struct_names); + let mav_message_crc = self.emit_mav_message_crc(&id_width, &struct_names); + let mav_message_name = self.emit_mav_message_name(&enum_names, &struct_names); + let mav_message_id = self.emit_mav_message_id(&enum_names, &struct_names); + let mav_message_id_from_name = self.emit_mav_message_id_from_name(&struct_names); let mav_message_default_from_id = - self.emit_mav_message_default_from_id(&enum_names, &msg_ids); + self.emit_mav_message_default_from_id(&enum_names, &struct_names); let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); quote! { @@ -179,7 +155,7 @@ impl MavProfile { #[allow(unused_imports)] use bitflags::bitflags; - use crate::{Message, error::*, bytes::Bytes, bytes_mut::BytesMut}; + use crate::{Message, MessageData, error::*, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -203,11 +179,7 @@ impl MavProfile { } } - fn emit_mav_message( - &self, - enums: &Vec, - structs: &Vec, - ) -> TokenStream { + fn emit_mav_message(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] @@ -221,14 +193,13 @@ impl MavProfile { &self, enums: &[TokenStream], structs: &[TokenStream], - ids: &[TokenStream], ) -> TokenStream { let id_width = format_ident!("u32"); quote! { fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { - #(#ids => #structs::deser(version, payload).map(Self::#enums),)* + #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* _ => { Err(ParserError::UnknownMessage { id }) }, @@ -237,16 +208,11 @@ impl MavProfile { } } - fn emit_mav_message_crc( - &self, - id_width: &Ident, - ids: &[TokenStream], - crc: &[TokenStream], - ) -> TokenStream { + fn emit_mav_message_crc(&self, id_width: &Ident, structs: &[TokenStream]) -> TokenStream { quote! { fn extra_crc(id: #id_width) -> u8 { match id { - #(#ids => #crc,)* + #(#structs::ID => #structs::EXTRA_CRC,)* _ => { 0 }, @@ -255,46 +221,32 @@ impl MavProfile { } } - fn emit_mav_message_name(&self, enums: &Vec) -> TokenStream { - let enum_names = enums.iter().map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }); - + fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { quote! { fn message_name(&self) -> &'static str { match self { - #(Self::#enums(..) => #enum_names,)* + #(Self::#enums(..) => #structs::NAME,)* } } } } - fn emit_mav_message_id(&self, enums: &Vec, ids: &Vec) -> TokenStream { + fn emit_mav_message_id(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { let id_width = format_ident!("u32"); quote! { fn message_id(&self) -> #id_width { match self { - #(Self::#enums(..) => #ids,)* + #(Self::#enums(..) => #structs::ID,)* } } } } - fn emit_mav_message_id_from_name( - &self, - enums: &[TokenStream], - ids: &[TokenStream], - ) -> TokenStream { - let enum_names = enums.iter().map(|enum_name| { - let name = enum_name.to_string(); - quote!(#name) - }); - + fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream { quote! { fn message_id_from_name(name: &str) -> Result { match name { - #(#enum_names => Ok(#ids),)* + #(#structs::NAME => Ok(#structs::ID),)* _ => { Err("Invalid message name.") } @@ -306,17 +258,12 @@ impl MavProfile { fn emit_mav_message_default_from_id( &self, enums: &[TokenStream], - ids: &[TokenStream], + structs: &[TokenStream], ) -> TokenStream { - let data_name = enums.iter().map(|enum_name| { - let name = format_ident!("{}_DATA", enum_name.to_string()); - quote!(#name) - }); - quote! { fn default_message_from_id(id: u32) -> Result { match id { - #(#ids => Ok(Self::#enums(#data_name::default())),)* + #(#structs::ID => Ok(Self::#enums(#structs::default())),)* _ => { Err("Invalid message id.") } @@ -621,6 +568,9 @@ impl MavMessage { fn emit_rust(&self) -> TokenStream { let msg_name = self.emit_struct_name(); + let id = self.id; + let name = self.name.clone(); + let extra_crc = extra_crc(self); let (name_types, msg_encoded_len) = self.emit_name_types(); let deser_vars = self.emit_deserialize_vars(); @@ -645,17 +595,26 @@ impl MavMessage { impl #msg_name { pub const ENCODED_LEN: usize = #msg_encoded_len; #const_default + } + + #default_impl - pub fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { + impl MessageData for #msg_name { + type Message = MavMessage; + + const ID: u32 = #id; + const NAME: &'static str = #name; + const EXTRA_CRC: u8 = #extra_crc; + const ENCODED_LEN: usize = #msg_encoded_len; + + fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { #deser_vars } - pub fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { #serialize_vars } } - - #default_impl } } } diff --git a/src/lib.rs b/src/lib.rs index 6ea0d77419..720a0d8a8f 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -81,6 +81,18 @@ where fn extra_crc(id: u32) -> u8; } +pub trait MessageData: Sized { + type Message: Message; + + const ID: u32; + const NAME: &'static str; + const EXTRA_CRC: u8; + const ENCODED_LEN: usize; + + fn ser(&self, version: MavlinkVersion, payload: &mut [u8]) -> usize; + fn deser(version: MavlinkVersion, payload: &[u8]) -> Result; +} + /// Metadata from a MAVLink packet header #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] @@ -198,6 +210,14 @@ impl MavFrame { } } +fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { + let mut crc_calculator = CRCu16::crc16mcrf4cc(); + crc_calculator.digest(data); + + crc_calculator.digest(&[extra_crc]); + crc_calculator.get_crc() +} + pub fn read_versioned_msg( r: &mut R, version: MavlinkVersion, @@ -281,41 +301,67 @@ impl MAVLinkV1MessageRaw { &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2)] } - pub fn calculate_crc(&self) -> u16 { + #[inline] + pub fn has_valid_crc(&self) -> bool { let payload_length: usize = self.payload_length().into(); - let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); - let extra_crc = M::extra_crc(self.message_id().into()); - - crc_calculator.digest(&[extra_crc]); - crc_calculator.get_crc() + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id().into()), + ) } - #[inline] - pub fn has_valid_crc(&self) -> bool { - self.checksum() == self.calculate_crc::() + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + &self.0[..(1 + Self::HEADER_SIZE + payload_length + 2)] } - pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + ) { self.0[0] = MAV_STX; - let msgid = message.message_id(); - - let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; - let payload_len = message.ser(MavlinkVersion::V1, payload_buf); let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ - payload_len as u8, + payload_length as u8, header.sequence, header.system_id, header.component_id, msgid as u8, ]); - let crc = self.calculate_crc::(); - self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] .copy_from_slice(&crc.to_le_bytes()); } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V1, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V1, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + } } /// Return a raw buffer with the mavlink message @@ -452,42 +498,51 @@ impl MAVLinkV2MessageRaw { let payload_length: usize = self.payload_length().into(); // Signature to ensure the link is tamper-proof. - let signature_size = if (self.incompatibility_flags() & 0x01) == MAVLINK_IFLAG_SIGNED { + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { Self::SIGNATURE_SIZE } else { 0 }; &mut self.0 - [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + 2 + signature_size)] + [(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] } - pub fn calculate_crc(&self) -> u16 { + #[inline] + pub fn has_valid_crc(&self) -> bool { let payload_length: usize = self.payload_length().into(); - let mut crc_calculator = CRCu16::crc16mcrf4cc(); - crc_calculator.digest(&self.0[1..(1 + Self::HEADER_SIZE + payload_length)]); - - let extra_crc = M::extra_crc(self.message_id()); - crc_calculator.digest(&[extra_crc]); - crc_calculator.get_crc() + self.checksum() + == calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + M::extra_crc(self.message_id()), + ) } - #[inline] - pub fn has_valid_crc(&self) -> bool { - self.checksum() == self.calculate_crc::() + pub fn raw_bytes(&self) -> &[u8] { + let payload_length = self.payload_length() as usize; + + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { + Self::SIGNATURE_SIZE + } else { + 0 + }; + + &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] } - pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + fn serialize_stx_and_header_and_crc( + &mut self, + header: MavHeader, + msgid: u32, + payload_length: usize, + extra_crc: u8, + ) { self.0[0] = MAV_STX_V2; - let msgid = message.message_id(); let msgid_bytes = msgid.to_le_bytes(); - let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; - let payload_len = message.ser(MavlinkVersion::V2, payload_buf); - let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ - payload_len as u8, + payload_length as u8, 0, //incompat_flags 0, //compat_flags header.sequence, @@ -498,10 +553,34 @@ impl MAVLinkV2MessageRaw { msgid_bytes[2], ]); - let crc = self.calculate_crc::(); - self.0[(1 + Self::HEADER_SIZE + payload_len)..(1 + Self::HEADER_SIZE + payload_len + 2)] + let crc = calculate_crc( + &self.0[1..(1 + Self::HEADER_SIZE + payload_length)], + extra_crc, + ); + self.0[(1 + Self::HEADER_SIZE + payload_length) + ..(1 + Self::HEADER_SIZE + payload_length + 2)] .copy_from_slice(&crc.to_le_bytes()); } + + pub fn serialize_message(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + ); + } + + pub fn serialize_message_data(&mut self, header: MavHeader, message_data: &D) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf); + + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + } } /// Return a raw buffer with the mavlink message diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index 83fab1d2e7..0f17e74426 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -89,4 +89,15 @@ mod test_v1_encode_decode { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV1MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V1); + assert!(raw_msg.has_valid_crc::()); + } } diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index 109fdc9694..db254607ab 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -156,4 +156,15 @@ mod test_v2_encode_decode { panic!("Decoded wrong message type") } } + + #[test] + pub fn test_serialize_to_raw() { + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut raw_msg = mavlink::MAVLinkV2MessageRaw::new(); + + raw_msg.serialize_message_data(crate::test_shared::COMMON_MSG_HEADER, &heartbeat_msg); + + assert_eq!(raw_msg.raw_bytes(), HEARTBEAT_V2); + assert!(raw_msg.has_valid_crc::()); + } } From 370872903265f6ebd9bf951bea9aac80921a6477 Mon Sep 17 00:00:00 2001 From: GabrielDertoni Date: Thu, 16 Feb 2023 22:53:49 -0300 Subject: [PATCH 062/159] fix: correctly deserialize MavFrame This commit addresses two bugs present in the `MavFrame::deser` implementation. It parsed the sequence number after system and component ids, but should instead parse it before. Furthermore the implementation used the `get_u32_le` method to parse the message id in mavlink v2. However the MAVLink spec specifies that this field should have 3 bytes, not 4 (like `u32`). --- src/lib.rs | 6 +++--- tests/mav_frame_tests.rs | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 37 insertions(+), 3 deletions(-) create mode 100644 tests/mav_frame_tests.rs diff --git a/src/lib.rs b/src/lib.rs index 720a0d8a8f..248ea9c1e4 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -180,9 +180,9 @@ impl MavFrame { pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { let mut buf = Bytes::new(input); + let sequence = buf.get_u8(); let system_id = buf.get_u8(); let component_id = buf.get_u8(); - let sequence = buf.get_u8(); let header = MavHeader { system_id, component_id, @@ -190,8 +190,8 @@ impl MavFrame { }; let msg_id = match version { - MavlinkVersion::V2 => buf.get_u32_le(), - MavlinkVersion::V1 => buf.get_u8().into(), + MavlinkVersion::V2 => buf.get_u24_le(), + MavlinkVersion::V1 => buf.get_u8() as u32, }; match M::parse(version, msg_id, buf.remaining_bytes()) { diff --git a/tests/mav_frame_tests.rs b/tests/mav_frame_tests.rs new file mode 100644 index 0000000000..ad4fcfbec5 --- /dev/null +++ b/tests/mav_frame_tests.rs @@ -0,0 +1,34 @@ +pub mod test_shared; + +mod mav_frame_tests { + // NOTE: No header + pub const HEARTBEAT_V2: &[u8] = &[ + 0xef, // seq 239 + 0x01, // sys ID + 0x01, // comp ID + 0x00, 0x00, 0x00, // msg ID + 0x05, 0x00, 0x00, 0x00, 0x02, 0x03, 0x59, 0x03, 0x03, // payload + 16, 240, // checksum + ]; + + #[test] + pub fn test_deser() { + use mavlink::{common::MavMessage, MavFrame, MavlinkVersion}; + let frame = MavFrame::::deser(MavlinkVersion::V2, HEARTBEAT_V2) + .expect("failed to parse message"); + + assert_eq!(frame.header, crate::test_shared::COMMON_MSG_HEADER); + let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + + let msg = match frame.msg { + MavMessage::HEARTBEAT(msg) => msg, + _ => panic!("Decoded wrong message type"), + }; + assert_eq!(msg.custom_mode, heartbeat_msg.custom_mode); + assert_eq!(msg.mavtype, heartbeat_msg.mavtype); + assert_eq!(msg.autopilot, heartbeat_msg.autopilot); + assert_eq!(msg.base_mode, heartbeat_msg.base_mode); + assert_eq!(msg.system_status, heartbeat_msg.system_status); + assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); + } +} From ad9fe3fcfa9ad463b24eb564f6ce4f0cd00d2ea5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 7 Aug 2023 10:13:32 -0300 Subject: [PATCH 063/159] github: test: Fix toolchain version for windows MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index b6fddab8f1..941ec2c289 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -61,7 +61,7 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master with: - toolchain: "1.60.0" + toolchain: stable override: true - uses: actions-rs/cargo@v1 with: From 97bc456357e1ff9ee188a0acce9ee89c5870331f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 18 Sep 2023 16:58:13 -0300 Subject: [PATCH 064/159] Cargo: Update to 0.12.0 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 4f61729579..e07ddb384d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.11.2" +version = "0.12.0" 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." From dd94f630b8c497b819dafeb6322b22be1078420c Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 22 Oct 2023 16:51:13 +0100 Subject: [PATCH 065/159] refactor 'internal tests' --- .github/workflows/test.yml | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 941ec2c289..cd5544e433 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -29,21 +29,14 @@ jobs: internal-tests: runs-on: ubuntu-latest + strategy: + matrix: + dialect: ["ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] steps: - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable + - uses: dtolnay/rust-toolchain@stable - name: Run internal tests - run: | - dialects=("ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common") - for dialect in "${dialects[@]}"; do - echo "::group::Testing $dialect" - if ! cargo test --verbose --features "$dialect" -- --nocapture; then - echo "::error::Tests failed" - fi - echo "::endgroup::" - done + run: cargo test --verbose --features ${{ matrix.dialect }} -- --nocapture mavlink-dump: runs-on: ubuntu-latest From 17c084f74beac5ce1b4ccd13a3e383eb11428947 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 22 Oct 2023 16:59:53 +0100 Subject: [PATCH 066/159] remove non-existent 'autoquad' feature --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index cd5544e433..b24aabdf10 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -31,7 +31,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - dialect: ["ardupilotmega", "asluav", "autoquad", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] + dialect: ["ardupilotmega", "asluav", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] steps: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable From 74243e339a131d8ca08e26f51b900f0d373eae10 Mon Sep 17 00:00:00 2001 From: danieleades <33452915+danieleades@users.noreply.github.com> Date: Sun, 22 Oct 2023 17:48:43 +0100 Subject: [PATCH 067/159] ci: Update Rust version and runner step --- .github/workflows/test.yml | 25 +++++++------------------ Cargo.toml | 2 +- 2 files changed, 8 insertions(+), 19 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index b24aabdf10..dce4f54c07 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -7,9 +7,8 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master + - uses: dtolnay/rust-toolchain@stable with: - toolchain: stable components: rustfmt - name: Run rustfmt run: cargo fmt --all -- --check @@ -42,9 +41,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master - with: - toolchain: stable + - uses: dtolnay/rust-toolchain@stable - name: Build mavlink-dump run: cargo build --verbose --bin mavlink-dump --features ardupilotmega @@ -54,13 +51,12 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master with: - toolchain: stable - override: true + toolchain: 1.65.0 - uses: actions-rs/cargo@v1 with: use-cross: true command: check - args: --all-targets + args: --all --all-targets build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] @@ -101,11 +97,9 @@ jobs: - name: Building ${{ matrix.TARGET }} run: echo "${{ matrix.TARGET }}" - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master + - uses: dtolnay/rust-toolchain@stable with: - toolchain: stable target: ${{ matrix.TARGET }} - override: true - uses: actions-rs/cargo@v1 with: use-cross: true @@ -117,11 +111,9 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@master + - uses: dtolnay/rust-toolchain@nightly with: - toolchain: nightly target: thumbv7em-none-eabihf - override: true - name: Build run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options @@ -130,10 +122,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: actions-rs/toolchain@v1.0.6 - with: - toolchain: stable - override: true + - uses: dtolnay/rust-toolchain@stable - name: Build docs run: cargo doc - name: Deploy diff --git a/Cargo.toml b/Cargo.toml index e07ddb384d..d591a449a2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,7 +9,7 @@ readme = "README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition = "2018" -rust-version = "1.60.0" +rust-version = "1.65.0" [build-dependencies] crc-any = { version = "2.3.0", default-features = false } From 226f2ee62730fa4b295a7a0a2189eb0031e54612 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 20 Mar 2023 06:36:54 +0000 Subject: [PATCH 068/159] update linting, address lints --- .github/workflows/test.yml | 7 ++- build/binder.rs | 1 + build/parser.rs | 87 +++++++++++++++++--------------------- src/bin/mavlink-dump.rs | 2 +- src/connection/udp.rs | 3 +- src/embedded.rs | 8 ++-- src/lib.rs | 14 +++--- src/utils.rs | 2 +- tests/process_log_files.rs | 2 +- 9 files changed, 58 insertions(+), 68 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index dce4f54c07..b439439622 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -19,12 +19,11 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master with: - toolchain: nightly-2022-11-30 + toolchain: nightly-2023-10-21 components: clippy - - uses: actions-rs/clippy-check@v1 + - uses: actions-rs-plus/clippy-check@v2 with: - token: ${{ secrets.GITHUB_TOKEN }} - args: --all --all-targets + args: --all --all-targets --features format-generated-code internal-tests: runs-on: ubuntu-latest diff --git a/build/binder.rs b/build/binder.rs index be68f7a9c3..099ef24e9b 100644 --- a/build/binder.rs +++ b/build/binder.rs @@ -11,6 +11,7 @@ pub fn generate(modules: Vec, out: &mut W) { #[allow(clippy::field_reassign_with_default)] #[allow(non_snake_case)] #[allow(clippy::unnecessary_cast)] + #[allow(clippy::bad_bit_mask)] #[cfg(feature = #module)] pub mod #module_ident; } diff --git a/build/parser.rs b/build/parser.rs index e797e3882f..20a883a2eb 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -57,17 +57,14 @@ impl MavProfile { fn update_enums(mut self) -> Self { for msg in self.messages.values() { for field in &msg.fields { - if let Some(ref enum_name) = field.enumtype { - // it is an enum - if let Some(ref dsp) = field.display { - // it is a bitmask - if dsp == "bitmask" { - // find the corresponding enum - for enm in self.enums.values_mut() { - if enm.name == *enum_name { - // this is the right enum - enm.bitfield = Some(field.mavtype.rust_primitive_type()); - } + if let Some(enum_name) = &field.enumtype { + // it is a bitmask + if let Some("bitmask") = &field.display.as_deref() { + // find the corresponding enum + for enm in self.enums.values_mut() { + if enm.name == *enum_name { + // this is the right enum + enm.bitfield = Some(field.mavtype.rust_primitive_type()); } } } @@ -503,13 +500,13 @@ impl MavMessage { fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self.fields.iter().map(|f| f.rust_writer()); quote! { - let mut _tmp = BytesMut::new(bytes); + let mut __tmp = BytesMut::new(bytes); #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { - let len = _tmp.len(); - crate::remove_trailing_zeroes(&mut bytes[..len]) + let len = __tmp.len(); + crate::remove_trailing_zeroes(&bytes[..len]) } else { - _tmp.len() + __tmp.len() } } } @@ -528,21 +525,21 @@ impl MavMessage { } } else { quote! { - let avail_len = _input.len(); + let avail_len = __input.len(); let mut payload_buf = [0; Self::ENCODED_LEN]; let mut buf = if avail_len < Self::ENCODED_LEN { //copy available bytes into an oversized buffer filled with zeros - payload_buf[0..avail_len].copy_from_slice(_input); + payload_buf[0..avail_len].copy_from_slice(__input); Bytes::new(&payload_buf) } else { // fast zero copy - Bytes::new(_input) + Bytes::new(__input) }; - let mut _struct = Self::default(); + let mut __struct = Self::default(); #(#deser_vars)* - Ok(_struct) + Ok(__struct) } } } @@ -607,7 +604,7 @@ impl MavMessage { const EXTRA_CRC: u8 = #extra_crc; const ENCODED_LEN: usize = #msg_encoded_len; - fn deser(_version: MavlinkVersion, _input: &[u8]) -> Result { + fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { #deser_vars } @@ -643,7 +640,7 @@ impl MavField { if matches!(self.mavtype, MavType::Array(_, _)) { let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); mavtype = quote!(#rt); - } else if let Some(ref enumname) = self.enumtype { + } else if let Some(enumname) = &self.enumtype { let en = TokenStream::from_str(enumname).unwrap(); mavtype = quote!(#en); } else { @@ -695,7 +692,7 @@ impl MavField { } let ts = TokenStream::from_str(&name).unwrap(); let name = quote!(#ts); - let buf = format_ident!("_tmp"); + let buf = format_ident!("__tmp"); self.mavtype.rust_writer(&name, buf) } @@ -703,7 +700,7 @@ impl MavField { fn rust_reader(&self) -> TokenStream { let _name = TokenStream::from_str(&self.name).unwrap(); - let name = quote!(_struct.#_name); + let name = quote!(__struct.#_name); let buf = format_ident!("buf"); if let Some(enum_name) = &self.enumtype { // TODO: handle enum arrays properly, rather than just generating @@ -1122,7 +1119,7 @@ pub fn parse_profile( let attr = attr.unwrap(); match stack.last() { Some(&MavXmlElement::Enum) => { - if let b"name" = attr.key.into_inner() { + if attr.key.into_inner() == b"name" { mavenum.name = attr .value .clone() @@ -1273,7 +1270,7 @@ pub fn parse_profile( entry.description = Some(s.replace('\n', " ")); } (Some(&Param), Some(&Entry)) => { - if let Some(ref mut params) = entry.params { + if let Some(params) = &mut entry.params { // Some messages can jump between values, like: // 0, 1, 2, 7 if params.len() < paramid.unwrap() { @@ -1382,7 +1379,7 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { let mut crc = CRCu16::crc16mcrf4cc(); crc.digest(msg.name.as_bytes()); - crc.digest(" ".as_bytes()); + crc.digest(b" "); let mut f = msg.fields.clone(); // only mavlink 1 fields should be part of the extra_crc @@ -1390,13 +1387,13 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { f.sort_by(|a, b| a.mavtype.compare(&b.mavtype)); for field in &f { crc.digest(field.mavtype.primitive_type().as_bytes()); - crc.digest(" ".as_bytes()); + crc.digest(b" "); if field.name == "mavtype" { - crc.digest("type".as_bytes()); + crc.digest(b"type"); } else { crc.digest(field.name.as_bytes()); } - crc.digest(" ".as_bytes()); + crc.digest(b" "); if let MavType::Array(_, size) = field.mavtype { crc.digest(&[size as u8]); } @@ -1443,31 +1440,25 @@ impl MavXmlFilter { Ok(content) => { match content { Event::Start(bytes) | Event::Empty(bytes) => { - let id = match identify_element(bytes.name().into_inner()) { - None => { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - } - Some(kind) => kind, + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; - if let MavXmlElement::Extensions = id { + if id == MavXmlElement::Extensions { self.extension_filter.is_in = true; } } Event::End(bytes) => { - let id = match identify_element(bytes.name().into_inner()) { - None => { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - } - Some(kind) => kind, + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; - if let MavXmlElement::Message = id { + if id == MavXmlElement::Message { self.extension_filter.is_in = false; } } diff --git a/src/bin/mavlink-dump.rs b/src/bin/mavlink-dump.rs index 2f262af9f9..9d30e84d40 100644 --- a/src/bin/mavlink-dump.rs +++ b/src/bin/mavlink-dump.rs @@ -49,7 +49,7 @@ fn main() { println!("received: {msg:?}"); } Err(MessageReadError::Io(e)) => { - if let std::io::ErrorKind::WouldBlock = e.kind() { + if e.kind() == std::io::ErrorKind::WouldBlock { //no messages currently available to receive -- wait a while thread::sleep(Duration::from_secs(1)); continue; diff --git a/src/connection/udp.rs b/src/connection/udp.rs index 3bcd05a6f2..7357ca99c3 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -74,8 +74,7 @@ struct PacketBuf { impl PacketBuf { pub fn new() -> Self { - let mut v = Vec::new(); - v.resize(65536, 0); + let v = vec![0; 65536]; Self { buf: v, start: 0, diff --git a/src/embedded.rs b/src/embedded.rs index 64138a5b14..574ae2b826 100644 --- a/src/embedded.rs +++ b/src/embedded.rs @@ -5,8 +5,8 @@ pub trait Read { fn read_u8(&mut self) -> Result; fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { - for i in 0..buf.len() { - buf[i] = self.read_u8()?; + for byte in buf { + *byte = self.read_u8()?; } Ok(()) @@ -26,8 +26,8 @@ pub trait Write { impl> Write for W { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { - for i in 0..buf.len() { - nb::block!(self.write(buf[i])).map_err(|_| MessageWriteError::Io)?; + for byte in buf { + nb::block!(self.write(*byte)).map_err(|_| MessageWriteError::Io)?; } Ok(()) diff --git a/src/lib.rs b/src/lib.rs index 248ea9c1e4..8acb6dce89 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -191,7 +191,7 @@ impl MavFrame { let msg_id = match version { MavlinkVersion::V2 => buf.get_u24_le(), - MavlinkVersion::V1 => buf.get_u8() as u32, + MavlinkVersion::V1 => buf.get_u8().into(), }; match M::parse(version, msg_id, buf.remaining_bytes()) { @@ -498,10 +498,10 @@ impl MAVLinkV2MessageRaw { let payload_length: usize = self.payload_length().into(); // Signature to ensure the link is tamper-proof. - let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { - Self::SIGNATURE_SIZE - } else { + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 { 0 + } else { + Self::SIGNATURE_SIZE }; &mut self.0 @@ -521,10 +521,10 @@ impl MAVLinkV2MessageRaw { pub fn raw_bytes(&self) -> &[u8] { let payload_length = self.payload_length() as usize; - let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) != 0 { - Self::SIGNATURE_SIZE - } else { + let signature_size = if (self.incompatibility_flags() & MAVLINK_IFLAG_SIGNED) == 0 { 0 + } else { + Self::SIGNATURE_SIZE }; &self.0[..(1 + Self::HEADER_SIZE + payload_length + signature_size + 2)] diff --git a/src/utils.rs b/src/utils.rs index 58adfc6729..486c2ea0bb 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -4,7 +4,7 @@ /// /// There must always be at least one remaining byte even if it is a /// zero byte. -pub(crate) fn remove_trailing_zeroes(data: &mut [u8]) -> usize { +pub(crate) fn remove_trailing_zeroes(data: &[u8]) -> usize { let mut len = data.len(); for b in data[1..].iter().rev() { diff --git a/tests/process_log_files.rs b/tests/process_log_files.rs index a38665fd8f..0bf71544f8 100644 --- a/tests/process_log_files.rs +++ b/tests/process_log_files.rs @@ -36,7 +36,7 @@ mod process_files { counter += 1; } Err(MessageReadError::Io(e)) => { - if let std::io::ErrorKind::WouldBlock = e.kind() { + if e.kind() == std::io::ErrorKind::WouldBlock { continue; } else { println!("recv error: {e:?}"); From 3a171050583119a6ad3492595ca57200d2b50f6b Mon Sep 17 00:00:00 2001 From: Ihsen Bouallegue <48621967+IhsenBouallegue@users.noreply.github.com> Date: Mon, 20 Nov 2023 14:00:30 +0100 Subject: [PATCH 069/159] Fix sequence order in serialization function (#204) --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 8acb6dce89..871006f13b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -154,9 +154,9 @@ impl MavFrame { let mut buf = bytes_mut::BytesMut::new(buf); // serialize header + buf.put_u8(self.header.sequence); buf.put_u8(self.header.system_id); buf.put_u8(self.header.component_id); - buf.put_u8(self.header.sequence); // message id match self.protocol_version { From ffd39959f1796cbdb3d1b1e431cb5192c8f57961 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 10:22:24 -0300 Subject: [PATCH 070/159] Cargo: Update to 0.12.1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index d591a449a2..ef508f6cfd 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.12.0" +version = "0.12.1" 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." From 892fef8e26dcfc895836f102561c77063f3092e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 10:08:31 -0300 Subject: [PATCH 071/159] tests: Use COMMON_MSG_HEADER values over hardcoded numbers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- tests/mav_frame_tests.rs | 23 +++++++++++++++++------ tests/v1_encode_decode_tests.rs | 6 +++--- tests/v2_encode_decode_tests.rs | 6 +++--- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/tests/mav_frame_tests.rs b/tests/mav_frame_tests.rs index ad4fcfbec5..50e900f8cd 100644 --- a/tests/mav_frame_tests.rs +++ b/tests/mav_frame_tests.rs @@ -3,12 +3,23 @@ pub mod test_shared; mod mav_frame_tests { // NOTE: No header pub const HEARTBEAT_V2: &[u8] = &[ - 0xef, // seq 239 - 0x01, // sys ID - 0x01, // comp ID - 0x00, 0x00, 0x00, // msg ID - 0x05, 0x00, 0x00, 0x00, 0x02, 0x03, 0x59, 0x03, 0x03, // payload - 16, 240, // checksum + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload + 0x00, + 0x00, + 0x00, + 0x02, + 0x03, + 0x59, + 0x03, + 0x03, + 0x10, // checksum + 0xf0, ]; #[test] diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index 0f17e74426..bf0efeda2d 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -6,9 +6,9 @@ mod test_v1_encode_decode { pub const HEARTBEAT_V1: &[u8] = &[ mavlink::MAV_STX, 0x09, - 0xef, - 0x01, - 0x01, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x05, 0x00, diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index db254607ab..46a8c63d02 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -7,9 +7,9 @@ mod test_v2_encode_decode { 0x09, //payload len 0, //incompat flags 0, //compat flags - 0xef, //seq 239 - 0x01, //sys ID - 0x01, //comp ID + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, 0x00, 0x00, 0x00, //msg ID From 2670b89b88462f5918e57db721b0cb1939c9d299 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 10:09:12 -0300 Subject: [PATCH 072/159] tests: Change value of component id to check for correct ser/deser logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- tests/test_shared/mod.rs | 2 +- tests/v1_encode_decode_tests.rs | 4 ++-- tests/v2_encode_decode_tests.rs | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/test_shared/mod.rs b/tests/test_shared/mod.rs index 4532b6bd40..4cba46907a 100644 --- a/tests/test_shared/mod.rs +++ b/tests/test_shared/mod.rs @@ -3,7 +3,7 @@ pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { sequence: 239, system_id: 1, - component_id: 1, + component_id: 2, }; #[cfg(feature = "common")] diff --git a/tests/v1_encode_decode_tests.rs b/tests/v1_encode_decode_tests.rs index bf0efeda2d..db7ea6fcaf 100644 --- a/tests/v1_encode_decode_tests.rs +++ b/tests/v1_encode_decode_tests.rs @@ -19,8 +19,8 @@ mod test_v1_encode_decode { 0x59, 0x03, 0x03, - 0xf1, - 0xd7, + 0x1f, + 0x50, ]; #[test] diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index 46a8c63d02..0aae35bf55 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -22,8 +22,8 @@ mod test_v2_encode_decode { 0x59, 0x03, 0x03, //payload - 16, - 240, //checksum + 46, + 115, //checksum ]; #[test] From 8c63f805a149174add7995d01abf0aaf4dfe434d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 12:42:43 -0300 Subject: [PATCH 073/159] lib: Fix serialization of mavlink v2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Only 3 bytes are used by message id Signed-off-by: Patrick José Pereira --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 871006f13b..e770161cfd 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -162,7 +162,7 @@ impl MavFrame { match self.protocol_version { MavlinkVersion::V2 => { let bytes: [u8; 4] = self.msg.message_id().to_le_bytes(); - buf.put_slice(&bytes); + buf.put_slice(&bytes[..3]); } MavlinkVersion::V1 => { buf.put_u8(self.msg.message_id() as u8); //TODO check From 63a51d87aceee196383345c598f8f95cd054d1ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 12:48:10 -0300 Subject: [PATCH 074/159] tests: Add serialization test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- tests/mav_frame_tests.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tests/mav_frame_tests.rs b/tests/mav_frame_tests.rs index 50e900f8cd..ef20068ffe 100644 --- a/tests/mav_frame_tests.rs +++ b/tests/mav_frame_tests.rs @@ -23,7 +23,7 @@ mod mav_frame_tests { ]; #[test] - pub fn test_deser() { + pub fn test_deser_ser() { use mavlink::{common::MavMessage, MavFrame, MavlinkVersion}; let frame = MavFrame::::deser(MavlinkVersion::V2, HEARTBEAT_V2) .expect("failed to parse message"); @@ -31,6 +31,10 @@ mod mav_frame_tests { assert_eq!(frame.header, crate::test_shared::COMMON_MSG_HEADER); let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); + let mut buffer = [0u8; HEARTBEAT_V2.len()]; + frame.ser(&mut buffer); + assert_eq!(buffer[..buffer.len() - 2], HEARTBEAT_V2[..buffer.len() - 2]); + let msg = match frame.msg { MavMessage::HEARTBEAT(msg) => msg, _ => panic!("Decoded wrong message type"), From 8e58086701f652df6777a041504807a573ccbcb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 13:24:45 -0300 Subject: [PATCH 075/159] tests: Add more ser/deser test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira Co-authored-by: : Ihsen Bouallegue <48621967+IhsenBouallegue@users.noreply.github.com> --- tests/mav_frame_tests.rs | 53 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/tests/mav_frame_tests.rs b/tests/mav_frame_tests.rs index ef20068ffe..d1fac6193f 100644 --- a/tests/mav_frame_tests.rs +++ b/tests/mav_frame_tests.rs @@ -1,6 +1,10 @@ pub mod test_shared; mod mav_frame_tests { + use mavlink::ardupilotmega::MavMessage; + use mavlink::MavFrame; + use mavlink::MavHeader; + // NOTE: No header pub const HEARTBEAT_V2: &[u8] = &[ crate::test_shared::COMMON_MSG_HEADER.sequence, @@ -46,4 +50,53 @@ mod mav_frame_tests { assert_eq!(msg.system_status, heartbeat_msg.system_status); assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); } + + #[test] + pub fn test_deser_ser_message() { + let buf: &mut [u8; 255] = &mut [0; 255]; + + let mavlink_message = mavlink_message(); + let mavlink_frame = new(mavlink_message); + + let _len = mavlink_frame.ser(buf); + + let parsed_mavlink_frame = + MavFrame::::deser(mavlink::MavlinkVersion::V2, buf) + .unwrap(); + + assert_eq!( + format!("{mavlink_frame:?}"), + format!("{parsed_mavlink_frame:?}") + ); + } + + fn mavlink_message() -> mavlink::ardupilotmega::MavMessage { + mavlink::ardupilotmega::MavMessage::LINK_NODE_STATUS( + mavlink::ardupilotmega::LINK_NODE_STATUS_DATA { + timestamp: 92197916, + tx_rate: 0x11223344, + rx_rate: 0x55667788, + messages_sent: 0x99001122, + messages_received: 0x33445566, + messages_lost: 0x77889900, + rx_parse_err: 0x1122, + tx_overflows: 0x3355, + rx_overflows: 0x5566, + tx_buf: 0xff, + rx_buf: 0x11, + }, + ) + } + + fn new(msg: MavMessage) -> MavFrame { + MavFrame { + header: MavHeader { + system_id: 1, + component_id: 2, + sequence: 84, + }, + msg, + protocol_version: mavlink::MavlinkVersion::V2, + } + } } From c4e069eb86ad36ba18d2463d6fbe2e342c082681 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Mon, 20 Nov 2023 13:30:28 -0300 Subject: [PATCH 076/159] Cargo: Update to 0.12.2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index ef508f6cfd..8ae58048c5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.12.1" +version = "0.12.2" 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." From 93534615652c7765e4458ff2c408a745ac337a3e Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Tue, 12 Dec 2023 14:27:36 +0100 Subject: [PATCH 077/159] Update lib.rs lib. Make embedded module public so that traits can be used externally --- src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index e770161cfd..409a96a4b0 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -54,7 +54,7 @@ pub mod bytes_mut; pub mod error; #[cfg(feature = "embedded")] -mod embedded; +pub mod embedded; #[cfg(feature = "embedded")] use embedded::{Read, Write}; From 19154d84d2336a2ce33e529fb1e32b032b51f703 Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Wed, 3 Jan 2024 20:27:53 +0100 Subject: [PATCH 078/159] Cleanup. Remove unnecessary self.clone() calls --- build/parser.rs | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/build/parser.rs b/build/parser.rs index 20a883a2eb..f14c6956a3 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -809,7 +809,7 @@ impl MavType { /// Emit reader of a given type pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; - match self.clone() { + match self { Char => quote! {#val = #buf.get_u8();}, UInt8 => quote! {#val = #buf.get_u8();}, UInt16 => quote! {#val = #buf.get_u16_le();}, @@ -837,7 +837,7 @@ impl MavType { /// Emit writer of a given type pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, UInt8 => quote! {#buf.put_u8(#val);}, Char => quote! {#buf.put_u8(#val);}, @@ -864,7 +864,7 @@ impl MavType { /// Size of a given Mavtype fn len(&self) -> usize { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, @@ -876,7 +876,7 @@ impl MavType { /// Used for ordering of types fn order_len(&self) -> usize { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, @@ -888,7 +888,7 @@ impl MavType { /// Used for crc calculation pub fn primitive_type(&self) -> String { use self::MavType::*; - match self.clone() { + match self { UInt8MavlinkVersion => "uint8_t".into(), UInt8 => "uint8_t".into(), Int8 => "int8_t".into(), @@ -909,7 +909,7 @@ impl MavType { /// Used for generating struct fields. pub fn rust_type(&self) -> String { use self::MavType::*; - match self.clone() { + match self { UInt8 | UInt8MavlinkVersion => "u8".into(), Int8 => "i8".into(), Char => "u8".into(), @@ -927,7 +927,6 @@ impl MavType { pub fn emit_default_value(&self) -> TokenStream { use self::MavType::*; - match self { UInt8 | UInt8MavlinkVersion => quote!(0_u8), Int8 => quote!(0_i8), From 76a28133e2de4220f507dca43072c2e639bfbb3b Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Wed, 3 Jan 2024 22:33:03 +0100 Subject: [PATCH 079/159] Bugfix. Replace default isize enum representation. By default Rust uses isize to represent enum values. isize implementation depends on the `target_pointer_width` attribute. - Mavlink implementation for v2 requires message id with the range 0..16777215 --> u32 - Mavlink implementation requires u32 enum entry values --- README.md | 2 +- build/parser.rs | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 37cf297673..45594840cd 100644 --- a/README.md +++ b/README.md @@ -10,7 +10,7 @@ with bindings for all message sets. Add to your Cargo.toml: ``` -mavlink = "0.10.1" +mavlink = "0.12.2" ``` ## Examples diff --git a/build/parser.rs b/build/parser.rs index f14c6956a3..a0618c5319 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -180,6 +180,7 @@ impl MavProfile { quote! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[repr(u32)] pub enum MavMessage { #(#enums(#structs),)* } @@ -390,6 +391,7 @@ impl MavEnum { #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[repr(u32)] #description pub enum #enum_name { #(#defs)* From aaec6ed88242da3972259d7b9db444f786ce6331 Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Thu, 4 Jan 2024 00:24:11 +0100 Subject: [PATCH 080/159] Refactor mavlink-dump. Change from [[bin]] to [[example]]. Splitting the example has the following benefits: - Faster normal compilation for library use - Easier to retain compilation incremental files (faster re-compilation times) - Cleaner library src/ directory - no_std/std definitions and imports can be removed from the mavlink-dump example - Executable name changes from mavlink -> mavlink-dump. More precise to locate and guess what it is for Disadvantage: - Installation time for the example is increased, but it is something rarely done --- Cargo.toml | 3 ++- examples/mavlink-dump/Cargo.toml | 15 +++++++++++++++ .../mavlink-dump/src/main.rs | 8 -------- 3 files changed, 17 insertions(+), 9 deletions(-) create mode 100644 examples/mavlink-dump/Cargo.toml rename src/bin/mavlink-dump.rs => examples/mavlink-dump/src/main.rs (95%) diff --git a/Cargo.toml b/Cargo.toml index 8ae58048c5..b099521299 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,8 +19,9 @@ proc-macro2 = "1.0.43" lazy_static = "1.2.0" serde = { version = "1.0.115", optional = true, features = ["derive"] } -[[bin]] +[[example]] name = "mavlink-dump" +path = "examples/mavlink-dump/src/main.rs" required-features = ["ardupilotmega"] [dependencies] diff --git a/examples/mavlink-dump/Cargo.toml b/examples/mavlink-dump/Cargo.toml new file mode 100644 index 0000000000..284ab5c890 --- /dev/null +++ b/examples/mavlink-dump/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "mavlink-dump" +authors = [ + "Patrick José Pereira ", +] +license = "MIT/Apache-2.0" +edition = "2018" +version = "0.1.0" + +[profile.release] +opt-level = 3 +lto = true # Performs "fat" LTO which attempts to perform optimizations across all crates within the dependency graph + +[dependencies.mavlink] # MAVLink library +path = "../../" diff --git a/src/bin/mavlink-dump.rs b/examples/mavlink-dump/src/main.rs similarity index 95% rename from src/bin/mavlink-dump.rs rename to examples/mavlink-dump/src/main.rs index 9d30e84d40..f2f1043504 100644 --- a/src/bin/mavlink-dump.rs +++ b/examples/mavlink-dump/src/main.rs @@ -1,11 +1,6 @@ use mavlink::error::MessageReadError; -#[cfg(feature = "std")] use std::{env, sync::Arc, thread, time::Duration}; -#[cfg(not(feature = "std"))] -fn main() {} - -#[cfg(feature = "std")] fn main() { let args: Vec<_> = env::args().collect(); @@ -65,7 +60,6 @@ fn main() { } /// Create a heartbeat message using 'ardupilotmega' dialect -#[cfg(feature = "std")] pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage { mavlink::ardupilotmega::MavMessage::HEARTBEAT(mavlink::ardupilotmega::HEARTBEAT_DATA { custom_mode: 0, @@ -78,7 +72,6 @@ pub fn heartbeat_message() -> mavlink::ardupilotmega::MavMessage { } /// Create a message requesting the parameters list -#[cfg(feature = "std")] pub fn request_parameters() -> mavlink::ardupilotmega::MavMessage { mavlink::ardupilotmega::MavMessage::PARAM_REQUEST_LIST( mavlink::ardupilotmega::PARAM_REQUEST_LIST_DATA { @@ -89,7 +82,6 @@ pub fn request_parameters() -> mavlink::ardupilotmega::MavMessage { } /// Create a message enabling data streaming -#[cfg(feature = "std")] pub fn request_stream() -> mavlink::ardupilotmega::MavMessage { mavlink::ardupilotmega::MavMessage::REQUEST_DATA_STREAM( mavlink::ardupilotmega::REQUEST_DATA_STREAM_DATA { From a7102c9b6c692b501d79cddd7128b7f13ed9c12d Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Thu, 4 Jan 2024 00:29:49 +0100 Subject: [PATCH 081/159] Update README.md --- README.md | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 45594840cd..e7871ae8ed 100644 --- a/README.md +++ b/README.md @@ -14,11 +14,29 @@ mavlink = "0.12.2" ``` ## Examples -See [src/bin/mavlink-dump.rs](src/bin/mavlink-dump.rs) for a usage example. +See [examples/](examples/mavlink-dump/src/main) for different usage examples. + +### mavlink-dump +[examples/mavlink-dump](examples/mavlink-dump/src/main) contains an executable example that can be used to test message reception. + +It can be executed directly by running: +``` +cargo run --example mavlink-dump [options] +``` It's also possible to install the working example via `cargo` command line: ```sh -cargo install mavlink +cargo install --path examples/mavlink-dump +``` + +It can then be executed by running: +``` +mavlink-dump [options] +``` + +Execution call example: +```sh +mavlink-dump udpin:127.0.0.1:14540 ``` ### Community projects From d828e6e67257cb850958648f3f065c59851cc5f2 Mon Sep 17 00:00:00 2001 From: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com> Date: Thu, 4 Jan 2024 00:59:22 +0100 Subject: [PATCH 082/159] Update .github workflow to accommodate for the mavlink-dump refactor --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index b439439622..9011cc0c69 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -42,7 +42,7 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable - name: Build mavlink-dump - run: cargo build --verbose --bin mavlink-dump --features ardupilotmega + run: cargo build --verbose --example mavlink-dump msrv: runs-on: ubuntu-latest From be0bc1ec08ef4784a355f250dc772f0da1d8a79f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Wed, 10 Jan 2024 16:37:50 -0300 Subject: [PATCH 083/159] src: connection: Fix panic when DNS lookup fails --- src/connection/mod.rs | 16 ++++++++++++++++ src/connection/tcp.rs | 15 +++++---------- src/connection/udp.rs | 22 ++++++---------------- 3 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/connection/mod.rs b/src/connection/mod.rs index 10dacbe976..7aa4ab78ec 100644 --- a/src/connection/mod.rs +++ b/src/connection/mod.rs @@ -102,3 +102,19 @@ pub fn connect(address: &str) -> io::Result protocol_err } } + +/// Returns the socket address for the given address. +pub(crate) fn get_socket_addr( + address: T, +) -> Result { + let addr = match address.to_socket_addrs()?.next() { + Some(addr) => addr, + None => { + return Err(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )); + } + }; + Ok(addr) +} diff --git a/src/connection/tcp.rs b/src/connection/tcp.rs index 491dff0129..b34eb5f091 100644 --- a/src/connection/tcp.rs +++ b/src/connection/tcp.rs @@ -6,6 +6,8 @@ use std::net::{TcpListener, TcpStream}; use std::sync::Mutex; use std::time::Duration; +use super::get_socket_addr; + /// TCP MAVLink connection pub fn select_protocol( @@ -26,11 +28,8 @@ pub fn select_protocol( } pub fn tcpout(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Host address lookup failed."); + let addr = get_socket_addr(address)?; + let socket = TcpStream::connect(addr)?; socket.set_read_timeout(Some(Duration::from_millis(100)))?; @@ -45,11 +44,7 @@ pub fn tcpout(address: T) -> io::Result { } pub fn tcpin(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); + let addr = get_socket_addr(address)?; let listener = TcpListener::bind(addr)?; //For now we only accept one incoming stream: this blocks until we get one diff --git a/src/connection/udp.rs b/src/connection/udp.rs index 7357ca99c3..1522ed20df 100644 --- a/src/connection/udp.rs +++ b/src/connection/udp.rs @@ -6,6 +6,8 @@ use std::net::ToSocketAddrs; use std::net::{SocketAddr, UdpSocket}; use std::sync::Mutex; +use super::get_socket_addr; + /// UDP MAVLink connection pub fn select_protocol( @@ -28,12 +30,8 @@ pub fn select_protocol( } pub fn udpbcast(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); - let socket = UdpSocket::bind("0.0.0.0:0").unwrap(); + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0")?; socket .set_broadcast(true) .expect("Couldn't bind to broadcast address."); @@ -41,21 +39,13 @@ pub fn udpbcast(address: T) -> io::Result { } pub fn udpout(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); + let addr = get_socket_addr(address)?; let socket = UdpSocket::bind("0.0.0.0:0")?; UdpConnection::new(socket, false, Some(addr)) } pub fn udpin(address: T) -> io::Result { - let addr = address - .to_socket_addrs() - .unwrap() - .next() - .expect("Invalid address"); + let addr = get_socket_addr(address)?; let socket = UdpSocket::bind(addr)?; UdpConnection::new(socket, true, None) } From 09411e9ae8e26aef0d5b576e4a180ae3819cd13f Mon Sep 17 00:00:00 2001 From: Graham Dennis Date: Mon, 19 Feb 2024 02:41:25 +1100 Subject: [PATCH 084/159] Restructure project to enable custom mavlink bindings generation (#223) * Restructure project to enable custom mavlink bindings generation Split the project into three crates: * mavlink-core: core MAVLink types including TCP, UDP and serial connections * mavlink-bindgen: a library & CLI for generating Rust bindings for MAVLink dialects * mavlink: generated MAVLink bindings for the MAVLink dialects in the mavlink/mavlink repo * run GitHub checks * reformat * attempt to fix cross compile test * attempt to install cross * Downgrade clap to support MSRV 1.65.0 * Downgrade clap_lex to support MSRV 1.65.0 * Another downgrade * downgrade anstyle * Simplify support for MSRV 1.65.0 * Hopefully this works * fix embedded build * Fix build on MSRV * Make cli feature not default * Only build the mavlink package in tests. The mavlink-bindgen crate does not itself need to run on embedded hardware * Attempt to support no_std in mavlink again * Fix for the last GitHub action * Make the embedded example work in the cargo workspace * Remove unused import * Remove accidentally committed binary --- .github/workflows/test.yml | 8 +- .gitmodules | 4 +- .idea/workspace.xml | 128 ++++++++++++++++++ Cargo.toml | 103 +------------- build/main.rs | 101 -------------- mavlink-bindgen/Cargo.toml | 26 ++++ {build => mavlink-bindgen/src}/binder.rs | 2 +- mavlink-bindgen/src/cli.rs | 28 ++++ mavlink-bindgen/src/error.rs | 23 ++++ mavlink-bindgen/src/lib.rs | 125 +++++++++++++++++ mavlink-bindgen/src/main.rs | 11 ++ {build => mavlink-bindgen/src}/parser.rs | 23 ++-- {build => mavlink-bindgen/src}/util.rs | 0 mavlink-core/Cargo.toml | 28 ++++ {src => mavlink-core/src}/bytes.rs | 0 {src => mavlink-core/src}/bytes_mut.rs | 0 .../src}/connection/direct_serial.rs | 0 {src => mavlink-core/src}/connection/file.rs | 0 {src => mavlink-core/src}/connection/mod.rs | 0 {src => mavlink-core/src}/connection/tcp.rs | 0 {src => mavlink-core/src}/connection/udp.rs | 0 {src => mavlink-core/src}/embedded.rs | 0 {src => mavlink-core/src}/error.rs | 0 {src => mavlink-core/src}/lib.rs | 14 +- {src => mavlink-core/src}/utils.rs | 2 +- mavlink/Cargo.toml | 96 +++++++++++++ mavlink/build/main.rs | 53 ++++++++ .../examples}/embedded/.cargo/config.toml | 0 .../examples}/embedded/Cargo.toml | 2 + .../examples}/embedded/README.md | 0 .../examples}/embedded/memory.x | 0 .../examples}/embedded/src/main.rs | 0 .../examples}/mavlink-dump/Cargo.toml | 0 .../examples}/mavlink-dump/src/main.rs | 0 mavlink => mavlink/mavlink | 0 rustfmt.toml => mavlink/rustfmt.toml | 0 mavlink/src/lib.rs | 5 + .../tests}/direct_serial_tests.rs | 0 .../tests}/encode_decode_tests.rs | 0 {tests => mavlink/tests}/helper_tests.rs | 0 {tests => mavlink/tests}/log.tlog | Bin {tests => mavlink/tests}/mav_frame_tests.rs | 0 {tests => mavlink/tests}/process_log_files.rs | 0 .../tests}/tcp_loopback_tests.rs | 0 {tests => mavlink/tests}/test_shared/mod.rs | 0 .../tests}/udp_loopback_tests.rs | 0 .../tests}/v1_encode_decode_tests.rs | 0 .../tests}/v2_encode_decode_tests.rs | 0 48 files changed, 555 insertions(+), 227 deletions(-) create mode 100644 .idea/workspace.xml delete mode 100644 build/main.rs create mode 100644 mavlink-bindgen/Cargo.toml rename {build => mavlink-bindgen/src}/binder.rs (91%) create mode 100644 mavlink-bindgen/src/cli.rs create mode 100644 mavlink-bindgen/src/error.rs create mode 100644 mavlink-bindgen/src/lib.rs create mode 100644 mavlink-bindgen/src/main.rs rename {build => mavlink-bindgen/src}/parser.rs (98%) rename {build => mavlink-bindgen/src}/util.rs (100%) create mode 100644 mavlink-core/Cargo.toml rename {src => mavlink-core/src}/bytes.rs (100%) rename {src => mavlink-core/src}/bytes_mut.rs (100%) rename {src => mavlink-core/src}/connection/direct_serial.rs (100%) rename {src => mavlink-core/src}/connection/file.rs (100%) rename {src => mavlink-core/src}/connection/mod.rs (100%) rename {src => mavlink-core/src}/connection/tcp.rs (100%) rename {src => mavlink-core/src}/connection/udp.rs (100%) rename {src => mavlink-core/src}/embedded.rs (100%) rename {src => mavlink-core/src}/error.rs (100%) rename {src => mavlink-core/src}/lib.rs (99%) rename {src => mavlink-core/src}/utils.rs (97%) create mode 100644 mavlink/Cargo.toml create mode 100644 mavlink/build/main.rs rename {examples => mavlink/examples}/embedded/.cargo/config.toml (100%) rename {examples => mavlink/examples}/embedded/Cargo.toml (98%) rename {examples => mavlink/examples}/embedded/README.md (100%) rename {examples => mavlink/examples}/embedded/memory.x (100%) rename {examples => mavlink/examples}/embedded/src/main.rs (100%) rename {examples => mavlink/examples}/mavlink-dump/Cargo.toml (100%) rename {examples => mavlink/examples}/mavlink-dump/src/main.rs (100%) rename mavlink => mavlink/mavlink (100%) rename rustfmt.toml => mavlink/rustfmt.toml (100%) create mode 100644 mavlink/src/lib.rs rename {tests => mavlink/tests}/direct_serial_tests.rs (100%) rename {tests => mavlink/tests}/encode_decode_tests.rs (100%) rename {tests => mavlink/tests}/helper_tests.rs (100%) rename {tests => mavlink/tests}/log.tlog (100%) rename {tests => mavlink/tests}/mav_frame_tests.rs (100%) rename {tests => mavlink/tests}/process_log_files.rs (100%) rename {tests => mavlink/tests}/tcp_loopback_tests.rs (100%) rename {tests => mavlink/tests}/test_shared/mod.rs (100%) rename {tests => mavlink/tests}/udp_loopback_tests.rs (100%) rename {tests => mavlink/tests}/v1_encode_decode_tests.rs (100%) rename {tests => mavlink/tests}/v2_encode_decode_tests.rs (100%) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 9011cc0c69..313eb75862 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -51,6 +51,10 @@ jobs: - uses: dtolnay/rust-toolchain@master with: toolchain: 1.65.0 + - uses: actions-rs/cargo@v1 + with: + command: install + args: cross --locked - uses: actions-rs/cargo@v1 with: use-cross: true @@ -103,7 +107,7 @@ jobs: with: use-cross: true command: build - args: --verbose --release --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} + args: --verbose --release --package=mavlink --target=${{ matrix.TARGET }} ${{ matrix.FLAGS }} test-embedded-size: needs: build @@ -114,7 +118,7 @@ jobs: with: target: thumbv7em-none-eabihf - name: Build - run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options + run: cargo +nightly build --target thumbv7em-none-eabihf --manifest-path mavlink/examples/embedded/Cargo.toml --out-dir $PWD --release -Z unstable-options docs: needs: internal-tests diff --git a/.gitmodules b/.gitmodules index 3fbebb0955..80bff682ff 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "mavlink"] - path = mavlink +[submodule "mavlink/mavlink"] + path = mavlink/mavlink url = https://github.com/mavlink/mavlink diff --git a/.idea/workspace.xml b/.idea/workspace.xml new file mode 100644 index 0000000000..2a7f278391 --- /dev/null +++ b/.idea/workspace.xml @@ -0,0 +1,128 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1707604355173 + + + + + + \ No newline at end of file diff --git a/Cargo.toml b/Cargo.toml index b099521299..65e9ef8b1d 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,105 +1,10 @@ +[workspace] +members = ["mavlink", "mavlink-bindgen", "mavlink-core"] +resolver = "1" -[package] -name = "mavlink" -version = "0.12.2" -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" -license = "MIT/Apache-2.0" -repository = "https://github.com/mavlink/rust-mavlink" -edition = "2018" -rust-version = "1.65.0" - -[build-dependencies] -crc-any = { version = "2.3.0", default-features = false } -quick-xml = "0.26" -quote = "1" -proc-macro2 = "1.0.43" -lazy_static = "1.2.0" -serde = { version = "1.0.115", optional = true, features = ["derive"] } - -[[example]] -name = "mavlink-dump" -path = "examples/mavlink-dump/src/main.rs" -required-features = ["ardupilotmega"] - -[dependencies] +[workspace.dependencies] crc-any = { version = "2.3.5", default-features = false } num-traits = { version = "0.2", default-features = false } num-derive = "0.3.2" bitflags = "1.2.1" -serial = { version = "0.4", optional = true } -serde = { version = "1.0.115", optional = true, features = ["derive"] } byteorder = { version = "1.3.4", default-features = false } -embedded-hal = { version = "0.2", optional = true } -nb = { version = "1.0", optional = true } -serde_arrays = { version = "0.1.0", optional = true } - -[features] -"all" = [ - "ardupilotmega", - "asluav", - "common", - "development", - "icarous", - "minimal", - "python_array_test", - "standard", - "test", - "ualberta", - "uavionix", - "avssuas", - "cubepilot", -] -"ardupilotmega" = ["common", "icarous", "uavionix"] -"asluav" = ["common"] -"avssuas" = ["common"] -"development" = ["common"] -"matrixpilot" = ["common"] -"minimal" = [] -"paparazzi" = ["common"] -"python_array_test" = ["common"] -"slugs" = ["common"] -"standard" = ["common"] -"test" = [] -"ualberta" = ["common"] -"uavionix" = ["common"] -"icarous" = [] -"common" = [] -"cubepilot" = ["common"] - -"all-dialects" = [ - "ardupilotmega", - "asluav", - "avssuas", - "development", - "matrixpilot", - "minimal", - "paparazzi", - "python_array_test", - "slugs", - "standard", - "test", - "ualberta", - "uavionix", - "icarous", - "common", - "cubepilot", -] - -"format-generated-code" = [] -"emit-description" = [] -"emit-extensions" = [] -"std" = ["byteorder/std"] -"udp" = [] -"tcp" = [] -"direct-serial" = [] -"embedded" = ["embedded-hal", "nb"] -"serde" = ["dep:serde", "dep:serde_arrays"] -default = ["std", "tcp", "udp", "direct-serial", "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"] diff --git a/build/main.rs b/build/main.rs deleted file mode 100644 index 6c14eea2c8..0000000000 --- a/build/main.rs +++ /dev/null @@ -1,101 +0,0 @@ -#![recursion_limit = "256"] - -mod binder; -mod parser; -mod util; - -use crate::util::to_module_name; -use std::env; -use std::ffi::OsStr; -use std::fs::{read_dir, File}; -use std::io::BufWriter; -use std::path::{Path, PathBuf}; -use std::process::Command; - -pub fn main() { - let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); - - // Update and init submodule - if let Err(error) = Command::new("git") - .arg("submodule") - .arg("update") - .arg("--init") - .current_dir(src_dir) - .status() - { - eprintln!("{error}"); - } - - // find & apply patches to XML definitions to avoid crashes - let mut patch_dir = src_dir.to_path_buf(); - patch_dir.push("build/patches"); - let mut mavlink_dir = src_dir.to_path_buf(); - mavlink_dir.push("mavlink"); - - if let Ok(dir) = read_dir(patch_dir) { - for entry in dir.flatten() { - if let Err(error) = Command::new("git") - .arg("apply") - .arg(entry.path().as_os_str()) - .current_dir(&mavlink_dir) - .status() - { - eprintln!("{error}"); - } - } - } - - let mut definitions_dir = src_dir.to_path_buf(); - definitions_dir.push("mavlink/message_definitions/v1.0"); - - let out_dir = env::var("OUT_DIR").unwrap(); - - let mut modules = vec![]; - - for entry in read_dir(&definitions_dir).expect("could not read definitions directory") { - let entry = entry.expect("could not read directory entry"); - - let definition_file = entry.file_name(); - let module_name = to_module_name(&definition_file); - - let mut definition_rs = PathBuf::from(&module_name); - definition_rs.set_extension("rs"); - - modules.push(module_name); - - let dest_path = Path::new(&out_dir).join(definition_rs); - let mut outf = BufWriter::new(File::create(&dest_path).unwrap()); - - // generate code - parser::generate( - &definitions_dir, - &definition_file.into_string().unwrap(), - &mut outf, - ); - dbg_format_code(&out_dir, &dest_path); - - // Re-run build if definition file changes - println!("cargo:rerun-if-changed={}", entry.path().to_string_lossy()); - } - - // output mod.rs - { - let dest_path = Path::new(&out_dir).join("mod.rs"); - let mut outf = File::create(&dest_path).unwrap(); - - // generate code - binder::generate(modules, &mut outf); - dbg_format_code(out_dir, dest_path); - } -} - -#[cfg(feature = "format-generated-code")] -fn dbg_format_code(cwd: impl AsRef, path: impl AsRef) { - if let Err(error) = Command::new("rustfmt").arg(path).current_dir(cwd).status() { - eprintln!("{error}"); - } -} - -// Does nothing -#[cfg(not(feature = "format-generated-code"))] -fn dbg_format_code(_: impl AsRef, _: impl AsRef) {} diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml new file mode 100644 index 0000000000..e5f3ee534b --- /dev/null +++ b/mavlink-bindgen/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "mavlink-bindgen" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +crc-any = { workspace = true, default-features = false } +quick-xml = "0.26" +quote = "1" +proc-macro2 = "1.0.43" +lazy_static = "1.2.0" +serde = { version = "1.0.115", optional = true, features = ["derive"] } +clap = { version = "~4.3.24", optional = true, default-features =false, features = ["derive", "help", "usage", "error-context"] } +thiserror = "1.0.56" + +# Required to support MSRV 1.65.0 +clap_lex = { version = "=0.5.0", optional=true } +clap_builder = { version = "~4.3.24", optional=true} +anstyle = { version = "=1.0.2", optional=true } +anstyle-query = { version = "=1.0.0", optional=true } +anstyle-parse = { version = "=0.2.1", optional=true } + +[features] +cli = ["dep:clap", "dep:clap_lex", "dep:clap_builder", "dep:anstyle", "dep:anstyle-query", "dep:anstyle-parse"] diff --git a/build/binder.rs b/mavlink-bindgen/src/binder.rs similarity index 91% rename from build/binder.rs rename to mavlink-bindgen/src/binder.rs index 099ef24e9b..b9a911cc1c 100644 --- a/build/binder.rs +++ b/mavlink-bindgen/src/binder.rs @@ -1,7 +1,7 @@ use quote::{format_ident, quote}; use std::io::Write; -pub fn generate(modules: Vec, out: &mut W) { +pub fn generate(modules: Vec<&str>, out: &mut W) { let modules_tokens = modules.into_iter().map(|module| { let module_ident = format_ident!("{}", module); diff --git a/mavlink-bindgen/src/cli.rs b/mavlink-bindgen/src/cli.rs new file mode 100644 index 0000000000..02eb59038f --- /dev/null +++ b/mavlink-bindgen/src/cli.rs @@ -0,0 +1,28 @@ +use std::path::PathBuf; + +use clap::Parser; +use mavlink_bindgen::{emit_cargo_build_messages, format_generated_code, generate}; + +#[derive(Parser)] +struct Cli { + definitions_dir: PathBuf, + destination_dir: PathBuf, + #[arg(long)] + format_generated_code: bool, + #[arg(long)] + emit_cargo_build_messages: bool, +} + +pub fn main() { + let args = Cli::parse(); + let result = generate(args.definitions_dir, args.destination_dir) + .expect("failed to generate MAVLink Rust bindings"); + + if args.format_generated_code { + format_generated_code(&result); + } + + if args.emit_cargo_build_messages { + emit_cargo_build_messages(&result); + } +} diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs new file mode 100644 index 0000000000..fec283634b --- /dev/null +++ b/mavlink-bindgen/src/error.rs @@ -0,0 +1,23 @@ +use thiserror::Error; + +#[derive(Error, Debug)] +pub enum BindGenError { + /// Represents a failure to read the MAVLink definitions directory. + #[error("Could not read definitions directory {path}")] + CouldNotReadDefinitionsDirectory { + source: std::io::Error, + path: std::path::PathBuf, + }, + /// Represents a failure to read a directory entry in the MAVLink definitions directory. + #[error("Could not read MAVLink definitions directory entry {path}")] + CouldNotReadDirectoryEntryInDefinitionsDirectory { + source: std::io::Error, + path: std::path::PathBuf, + }, + /// Represents a failure to create a Rust file for the generated MAVLink bindings. + #[error("Could not create Rust bindings file {dest_path}")] + CouldNotCreateRustBindingsFile { + source: std::io::Error, + dest_path: std::path::PathBuf, + }, +} diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs new file mode 100644 index 0000000000..da3145bed3 --- /dev/null +++ b/mavlink-bindgen/src/lib.rs @@ -0,0 +1,125 @@ +use crate::error::BindGenError; +use std::fs::{read_dir, File}; +use std::io::BufWriter; +use std::ops::Deref; +use std::path::{Path, PathBuf}; +use std::process::Command; + +pub mod binder; +pub mod error; +pub mod parser; +mod util; + +#[derive(Debug)] +pub struct GeneratedBinding { + pub module_name: String, + pub mavlink_xml: PathBuf, + pub rust_module: PathBuf, +} + +#[derive(Debug)] +pub struct GeneratedBindings { + pub bindings: Vec, + pub mod_rs: PathBuf, +} + +pub fn generate, P2: AsRef>( + definitions_dir: P1, + destination_dir: P2, +) -> Result { + _generate(definitions_dir.as_ref(), destination_dir.as_ref()) +} + +fn _generate( + definitions_dir: &Path, + destination_dir: &Path, +) -> Result { + let mut bindings = vec![]; + + for entry_maybe in read_dir(&definitions_dir).map_err(|source| { + BindGenError::CouldNotReadDefinitionsDirectory { + source, + path: definitions_dir.to_path_buf(), + } + })? { + let entry = entry_maybe.map_err(|source| { + BindGenError::CouldNotReadDirectoryEntryInDefinitionsDirectory { + source, + path: definitions_dir.to_path_buf(), + } + })?; + + let definition_file = PathBuf::from(entry.file_name()); + let module_name = util::to_module_name(&definition_file); + + let mut definition_rs = PathBuf::from(&module_name); + definition_rs.set_extension("rs"); + + let dest_path = destination_dir.join(definition_rs); + let mut outf = BufWriter::new(File::create(&dest_path).map_err(|source| { + BindGenError::CouldNotCreateRustBindingsFile { + source, + dest_path: dest_path.clone(), + } + })?); + + // generate code + parser::generate(&definitions_dir, &definition_file, &mut outf); + + bindings.push(GeneratedBinding { + module_name, + mavlink_xml: definition_file, + rust_module: dest_path, + }); + } + + // output mod.rs + { + let dest_path = destination_dir.join("mod.rs"); + let mut outf = File::create(&dest_path).map_err(|source| { + BindGenError::CouldNotCreateRustBindingsFile { + source, + dest_path: dest_path.clone(), + } + })?; + + // generate code + binder::generate( + bindings + .iter() + .map(|binding| binding.module_name.deref()) + .collect(), + &mut outf, + ); + + Ok(GeneratedBindings { + bindings, + mod_rs: dest_path, + }) + } +} + +pub fn format_generated_code(result: &GeneratedBindings) { + if let Err(error) = Command::new("rustfmt") + .args( + result + .bindings + .iter() + .map(|binding| binding.rust_module.clone()), + ) + .arg(result.mod_rs.clone()) + .status() + { + eprintln!("{error}"); + } +} + +pub fn emit_cargo_build_messages(result: &GeneratedBindings) { + for binding in &result.bindings { + // Re-run build if definition file changes + println!( + "cargo:rerun-if-changed={}", + binding.rust_module.to_string_lossy() + ); + } +} diff --git a/mavlink-bindgen/src/main.rs b/mavlink-bindgen/src/main.rs new file mode 100644 index 0000000000..660ba5ec7f --- /dev/null +++ b/mavlink-bindgen/src/main.rs @@ -0,0 +1,11 @@ +#![recursion_limit = "256"] + +#[cfg(feature = "cli")] +mod cli; + +pub fn main() { + #[cfg(feature = "cli")] + cli::main(); + #[cfg(not(feature = "cli"))] + panic!("Compiled without cli feature"); +} diff --git a/build/parser.rs b/mavlink-bindgen/src/parser.rs similarity index 98% rename from build/parser.rs rename to mavlink-bindgen/src/parser.rs index a0618c5319..0d273e663d 100644 --- a/build/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -140,7 +140,6 @@ impl MavProfile { quote! { #comment - use crate::MavlinkVersion; #[allow(unused_imports)] use num_derive::FromPrimitive; #[allow(unused_imports)] @@ -152,7 +151,7 @@ impl MavProfile { #[allow(unused_imports)] use bitflags::bitflags; - use crate::{Message, MessageData, error::*, bytes::Bytes, bytes_mut::BytesMut}; + use mavlink_core::{MavlinkVersion, Message, MessageData, bytes::Bytes, bytes_mut::BytesMut}; #[cfg(feature = "serde")] use serde::{Serialize, Deserialize}; @@ -195,11 +194,11 @@ impl MavProfile { let id_width = format_ident!("u32"); quote! { - fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { + fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { match id { #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* _ => { - Err(ParserError::UnknownMessage { id }) + Err(::mavlink_core::error::ParserError::UnknownMessage { id }) }, } } @@ -506,7 +505,7 @@ impl MavMessage { #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { let len = __tmp.len(); - crate::remove_trailing_zeroes(&bytes[..len]) + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) } else { __tmp.len() } @@ -606,7 +605,7 @@ impl MavMessage { const EXTRA_CRC: u8 = #extra_crc; const ENCODED_LEN: usize = #msg_encoded_len; - fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { + fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { #deser_vars } @@ -718,7 +717,7 @@ impl MavField { quote! { #tmp #name = #enum_name_ident::from_bits(tmp & #enum_name_ident::all().bits()) - .ok_or(ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u32 })?; + .ok_or(::mavlink_core::error::ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u32 })?; } } else { panic!("Display option not implemented"); @@ -730,7 +729,7 @@ impl MavField { quote!( #tmp #name = FromPrimitive::#val(tmp) - .ok_or(ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u32 })?; + .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u32 })?; ) } } else { @@ -1032,7 +1031,7 @@ fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { pub fn parse_profile( definitions_dir: &Path, - definition_file: &String, + definition_file: &Path, parsed_files: &mut HashSet, ) -> MavProfile { let in_path = Path::new(&definitions_dir).join(definition_file); @@ -1045,7 +1044,7 @@ pub fn parse_profile( let mut message = MavMessage::default(); let mut mavenum = MavEnum::default(); let mut entry = MavEnumEntry::default(); - let mut include = String::new(); + let mut include = PathBuf::new(); let mut paramid: Option = None; let mut xml_filter = MavXmlFilter::default(); @@ -1283,7 +1282,7 @@ pub fn parse_profile( } } (Some(&Include), Some(&Mavlink)) => { - include = s.replace('\n', ""); + include = PathBuf::from(s.replace('\n', "")); } (Some(&Version), Some(&Mavlink)) => { eprintln!("TODO: version {s:?}"); @@ -1360,7 +1359,7 @@ pub fn parse_profile( /// Generate protobuf represenation of mavlink message set /// Generate rust representation of mavlink message set with appropriate conversion methods -pub fn generate(definitions_dir: &Path, definition_file: &String, output_rust: &mut W) { +pub fn generate(definitions_dir: &Path, definition_file: &Path, output_rust: &mut W) { let mut parsed_files: HashSet = HashSet::new(); let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files); diff --git a/build/util.rs b/mavlink-bindgen/src/util.rs similarity index 100% rename from build/util.rs rename to mavlink-bindgen/src/util.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml new file mode 100644 index 0000000000..17940867be --- /dev/null +++ b/mavlink-core/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "mavlink-core" +version = "0.12.2" +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" +repository = "https://github.com/mavlink/rust-mavlink" +edition = "2018" +rust-version = "1.65.0" + +[dependencies] +crc-any = { workspace = true, default-features = false } +byteorder = { workspace = true, default-features = false } +embedded-hal = { version = "0.2", optional = true } +nb = { version = "1.0", optional = true } +serde = { version = "1.0.115", optional = true, features = ["derive"] } +serde_arrays = { version = "0.1.0", optional = true } +serial = { version = "0.4", optional = true } + +[features] +"std" = ["byteorder/std"] +"udp" = [] +"tcp" = [] +"direct-serial" = ["serial"] +"embedded" = ["embedded-hal", "nb"] +"serde" = ["dep:serde", "dep:serde_arrays"] +default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/src/bytes.rs b/mavlink-core/src/bytes.rs similarity index 100% rename from src/bytes.rs rename to mavlink-core/src/bytes.rs diff --git a/src/bytes_mut.rs b/mavlink-core/src/bytes_mut.rs similarity index 100% rename from src/bytes_mut.rs rename to mavlink-core/src/bytes_mut.rs diff --git a/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs similarity index 100% rename from src/connection/direct_serial.rs rename to mavlink-core/src/connection/direct_serial.rs diff --git a/src/connection/file.rs b/mavlink-core/src/connection/file.rs similarity index 100% rename from src/connection/file.rs rename to mavlink-core/src/connection/file.rs diff --git a/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs similarity index 100% rename from src/connection/mod.rs rename to mavlink-core/src/connection/mod.rs diff --git a/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs similarity index 100% rename from src/connection/tcp.rs rename to mavlink-core/src/connection/tcp.rs diff --git a/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs similarity index 100% rename from src/connection/udp.rs rename to mavlink-core/src/connection/udp.rs diff --git a/src/embedded.rs b/mavlink-core/src/embedded.rs similarity index 100% rename from src/embedded.rs rename to mavlink-core/src/embedded.rs diff --git a/src/error.rs b/mavlink-core/src/error.rs similarity index 100% rename from src/error.rs rename to mavlink-core/src/error.rs diff --git a/src/lib.rs b/mavlink-core/src/lib.rs similarity index 99% rename from src/lib.rs rename to mavlink-core/src/lib.rs index 409a96a4b0..c5f86e4ac1 100644 --- a/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -30,12 +30,7 @@ use std::io::{Read, Write}; #[cfg(feature = "std")] use byteorder::ReadBytesExt; -#[cfg(feature = "std")] -mod connection; -#[cfg(feature = "std")] -pub use self::connection::{connect, MavConnection}; - -mod utils; +pub mod utils; #[allow(unused_imports)] use utils::{remove_trailing_zeroes, RustDefault}; @@ -46,12 +41,13 @@ use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; -// include generate definitions -include!(concat!(env!("OUT_DIR"), "/mod.rs")); - pub mod bytes; pub mod bytes_mut; +#[cfg(feature = "std")] +mod connection; pub mod error; +#[cfg(feature = "std")] +pub use self::connection::{connect, MavConnection}; #[cfg(feature = "embedded")] pub mod embedded; diff --git a/src/utils.rs b/mavlink-core/src/utils.rs similarity index 97% rename from src/utils.rs rename to mavlink-core/src/utils.rs index 486c2ea0bb..c0516c154e 100644 --- a/src/utils.rs +++ b/mavlink-core/src/utils.rs @@ -4,7 +4,7 @@ /// /// There must always be at least one remaining byte even if it is a /// zero byte. -pub(crate) fn remove_trailing_zeroes(data: &[u8]) -> usize { +pub fn remove_trailing_zeroes(data: &[u8]) -> usize { let mut len = data.len(); for b in data[1..].iter().rev() { diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml new file mode 100644 index 0000000000..728e630a37 --- /dev/null +++ b/mavlink/Cargo.toml @@ -0,0 +1,96 @@ + +[package] +name = "mavlink" +version = "0.12.2" +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" +license = "MIT/Apache-2.0" +repository = "https://github.com/mavlink/rust-mavlink" +edition = "2018" +rust-version = "1.65.0" + +[build-dependencies] +mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false} + +[[example]] +name = "mavlink-dump" +path = "examples/mavlink-dump/src/main.rs" +required-features = ["ardupilotmega"] + +[dependencies] +mavlink-core = { path = "../mavlink-core", default-features = false} +num-traits = { workspace = true, default-features = false } +num-derive = { workspace = true } +bitflags = { workspace = true } +serde = { version = "1.0.115", optional = true, features = ["derive"] } +serde_arrays = { version = "0.1.0", optional = true } + +[features] +"all" = [ + "ardupilotmega", + "asluav", + "common", + "development", + "icarous", + "minimal", + "python_array_test", + "standard", + "test", + "ualberta", + "uavionix", + "avssuas", + "cubepilot", +] +"ardupilotmega" = ["common", "icarous", "uavionix"] +"asluav" = ["common"] +"avssuas" = ["common"] +"development" = ["common"] +"matrixpilot" = ["common"] +"minimal" = [] +"paparazzi" = ["common"] +"python_array_test" = ["common"] +"slugs" = ["common"] +"standard" = ["common"] +"test" = [] +"ualberta" = ["common"] +"uavionix" = ["common"] +"icarous" = [] +"common" = [] +"cubepilot" = ["common"] + +"all-dialects" = [ + "ardupilotmega", + "asluav", + "avssuas", + "development", + "matrixpilot", + "minimal", + "paparazzi", + "python_array_test", + "slugs", + "standard", + "test", + "ualberta", + "uavionix", + "icarous", + "common", + "cubepilot", +] + +"format-generated-code" = [] +"emit-description" = [] +"emit-extensions" = [] +"std" = ["mavlink-core/std"] +"udp" = ["mavlink-core/udp"] +"tcp" = ["mavlink-core/tcp"] +"direct-serial" = ["mavlink-core/direct-serial"] +"embedded" = ["mavlink-core/embedded"] +"serde" = ["mavlink-core/serde", "dep:serde", "dep:serde_arrays"] +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"] diff --git a/mavlink/build/main.rs b/mavlink/build/main.rs new file mode 100644 index 0000000000..7206d20658 --- /dev/null +++ b/mavlink/build/main.rs @@ -0,0 +1,53 @@ +#![recursion_limit = "256"] + +use std::env; +use std::fs::read_dir; +use std::path::Path; +use std::process::Command; + +pub fn main() { + let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); + + // Update and init submodule + if let Err(error) = Command::new("git") + .arg("submodule") + .arg("update") + .arg("--init") + .current_dir(src_dir) + .status() + { + eprintln!("{error}"); + } + + // find & apply patches to XML definitions to avoid crashes + let mut patch_dir = src_dir.to_path_buf(); + patch_dir.push("build/patches"); + let mut mavlink_dir = src_dir.to_path_buf(); + mavlink_dir.push("mavlink"); + + if let Ok(dir) = read_dir(patch_dir) { + for entry in dir.flatten() { + if let Err(error) = Command::new("git") + .arg("apply") + .arg(entry.path().as_os_str()) + .current_dir(&mavlink_dir) + .status() + { + eprintln!("{error}"); + } + } + } + + let mut definitions_dir = src_dir.to_path_buf(); + definitions_dir.push("mavlink/message_definitions/v1.0"); + + let out_dir = env::var("OUT_DIR").unwrap(); + + let result = mavlink_bindgen::generate(definitions_dir, out_dir) + .expect("Failed to generate Rust MAVLink bindings"); + + #[cfg(feature = "format-generated-code")] + mavlink_bindgen::format_generated_code(&result); + + mavlink_bindgen::emit_cargo_build_messages(&result); +} diff --git a/examples/embedded/.cargo/config.toml b/mavlink/examples/embedded/.cargo/config.toml similarity index 100% rename from examples/embedded/.cargo/config.toml rename to mavlink/examples/embedded/.cargo/config.toml diff --git a/examples/embedded/Cargo.toml b/mavlink/examples/embedded/Cargo.toml similarity index 98% rename from examples/embedded/Cargo.toml rename to mavlink/examples/embedded/Cargo.toml index 4f9fd15e7e..717a0c94d1 100644 --- a/examples/embedded/Cargo.toml +++ b/mavlink/examples/embedded/Cargo.toml @@ -20,3 +20,5 @@ stm32f3xx-hal = { version = "0.9", features = ["stm32f303xe"] } # HAL for stm32f path = "../../" features = ["ardupilotmega", "embedded"] default-features = false + +[workspace] \ No newline at end of file diff --git a/examples/embedded/README.md b/mavlink/examples/embedded/README.md similarity index 100% rename from examples/embedded/README.md rename to mavlink/examples/embedded/README.md diff --git a/examples/embedded/memory.x b/mavlink/examples/embedded/memory.x similarity index 100% rename from examples/embedded/memory.x rename to mavlink/examples/embedded/memory.x diff --git a/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs similarity index 100% rename from examples/embedded/src/main.rs rename to mavlink/examples/embedded/src/main.rs diff --git a/examples/mavlink-dump/Cargo.toml b/mavlink/examples/mavlink-dump/Cargo.toml similarity index 100% rename from examples/mavlink-dump/Cargo.toml rename to mavlink/examples/mavlink-dump/Cargo.toml diff --git a/examples/mavlink-dump/src/main.rs b/mavlink/examples/mavlink-dump/src/main.rs similarity index 100% rename from examples/mavlink-dump/src/main.rs rename to mavlink/examples/mavlink-dump/src/main.rs diff --git a/mavlink b/mavlink/mavlink similarity index 100% rename from mavlink rename to mavlink/mavlink diff --git a/rustfmt.toml b/mavlink/rustfmt.toml similarity index 100% rename from rustfmt.toml rename to mavlink/rustfmt.toml diff --git a/mavlink/src/lib.rs b/mavlink/src/lib.rs new file mode 100644 index 0000000000..a1b71eaa5a --- /dev/null +++ b/mavlink/src/lib.rs @@ -0,0 +1,5 @@ +#![cfg_attr(not(feature = "std"), no_std)] +// include generate definitions +include!(concat!(env!("OUT_DIR"), "/mod.rs")); + +pub use mavlink_core::*; diff --git a/tests/direct_serial_tests.rs b/mavlink/tests/direct_serial_tests.rs similarity index 100% rename from tests/direct_serial_tests.rs rename to mavlink/tests/direct_serial_tests.rs diff --git a/tests/encode_decode_tests.rs b/mavlink/tests/encode_decode_tests.rs similarity index 100% rename from tests/encode_decode_tests.rs rename to mavlink/tests/encode_decode_tests.rs diff --git a/tests/helper_tests.rs b/mavlink/tests/helper_tests.rs similarity index 100% rename from tests/helper_tests.rs rename to mavlink/tests/helper_tests.rs diff --git a/tests/log.tlog b/mavlink/tests/log.tlog similarity index 100% rename from tests/log.tlog rename to mavlink/tests/log.tlog diff --git a/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs similarity index 100% rename from tests/mav_frame_tests.rs rename to mavlink/tests/mav_frame_tests.rs diff --git a/tests/process_log_files.rs b/mavlink/tests/process_log_files.rs similarity index 100% rename from tests/process_log_files.rs rename to mavlink/tests/process_log_files.rs diff --git a/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs similarity index 100% rename from tests/tcp_loopback_tests.rs rename to mavlink/tests/tcp_loopback_tests.rs diff --git a/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs similarity index 100% rename from tests/test_shared/mod.rs rename to mavlink/tests/test_shared/mod.rs diff --git a/tests/udp_loopback_tests.rs b/mavlink/tests/udp_loopback_tests.rs similarity index 100% rename from tests/udp_loopback_tests.rs rename to mavlink/tests/udp_loopback_tests.rs diff --git a/tests/v1_encode_decode_tests.rs b/mavlink/tests/v1_encode_decode_tests.rs similarity index 100% rename from tests/v1_encode_decode_tests.rs rename to mavlink/tests/v1_encode_decode_tests.rs diff --git a/tests/v2_encode_decode_tests.rs b/mavlink/tests/v2_encode_decode_tests.rs similarity index 100% rename from tests/v2_encode_decode_tests.rs rename to mavlink/tests/v2_encode_decode_tests.rs From 7d6a1c44e23b8a2f3df29d176df4bc8c1584697e Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Tue, 27 Feb 2024 22:03:38 +0000 Subject: [PATCH 085/159] Fixed incorrect message framing by using a buffered reader. This allows the parser to backtrack in case a STX byte in the middle of a message is mistaken for the start-of-frame (checked via CRC) --- mavlink-core/Cargo.toml | 10 +- mavlink-core/src/connection/direct_serial.rs | 47 +++++- mavlink-core/src/connection/file.rs | 9 +- mavlink-core/src/connection/tcp.rs | 13 +- mavlink-core/src/connection/udp.rs | 96 ++++------- mavlink-core/src/lib.rs | 160 +++++++++++-------- mavlink/Cargo.toml | 22 ++- mavlink/tests/encode_decode_tests.rs | 18 +-- mavlink/tests/process_log_files.rs | 2 +- mavlink/tests/v1_encode_decode_tests.rs | 4 +- mavlink/tests/v2_encode_decode_tests.rs | 4 +- 11 files changed, 214 insertions(+), 171 deletions(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 17940867be..38ae99d4c6 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" @@ -17,6 +24,7 @@ nb = { version = "1.0", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } +buffered-reader = { version = "1.3.0", default-features = false } [features] "std" = ["byteorder/std"] diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index c8d226e131..6f604277e6 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -1,13 +1,44 @@ use crate::connection::MavConnection; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; -use std::io; +use core::ops::DerefMut; +use std::io::{self, Read, Write}; use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; -use serial::prelude::*; +use serial::{prelude::*, SystemPort}; /// Serial MAVLINK connection +struct SyncSystemPort(Mutex); + +impl Read for SyncSystemPort { + fn read(&mut self, buf: &mut [u8]) -> io::Result { + let mut port = match self.0.lock() { + Ok(port) => port, + Err(err) => err.into_inner(), + }; + port.read(buf) + } +} + +impl Write for SyncSystemPort { + fn write(&mut self, buf: &[u8]) -> io::Result { + let mut port = match self.0.lock() { + Ok(port) => port, + Err(err) => err.into_inner(), + }; + port.write(buf) + } + + fn flush(&mut self) -> io::Result<()> { + let mut port = match self.0.lock() { + Ok(port) => port, + Err(err) => err.into_inner(), + }; + port.flush() + } +} + pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -40,14 +71,17 @@ pub fn open(settings: &str) -> io::Result { port.configure(&settings)?; Ok(SerialConnection { - port: Mutex::new(port), + port: Mutex::new(buffered_reader::Generic::new( + SyncSystemPort(Mutex::new(port)), + None, + )), sequence: Mutex::new(0), protocol_version: MavlinkVersion::V2, }) } pub struct SerialConnection { - port: Mutex, + port: Mutex>, sequence: Mutex, protocol_version: MavlinkVersion, } @@ -55,9 +89,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 +116,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 5972c4446c..f8db8d4e33 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,8 +1,9 @@ use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; 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 +12,13 @@ pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; Ok(FileConnection { - file: Mutex::new(file), + file: Mutex::new(buffered_reader::Generic::new(file, None)), protocol_version: MavlinkVersion::V2, }) } pub struct FileConnection { - file: Mutex, + file: Mutex>, protocol_version: MavlinkVersion, } @@ -28,7 +29,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 b34eb5f091..6262bd7292 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -1,6 +1,7 @@ use crate::connection::MavConnection; 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 +35,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(buffered_reader::Generic::new(socket.try_clone()?, None)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -52,7 +53,7 @@ pub fn tcpin(address: T) -> io::Result { match incoming { Ok(socket) => { return Ok(TcpConnection { - reader: Mutex::new(socket.try_clone()?), + reader: Mutex::new(buffered_reader::Generic::new(socket.try_clone()?, None)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -73,7 +74,7 @@ pub fn tcpin(address: T) -> io::Result { } pub struct TcpConnection { - reader: Mutex, + reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, } @@ -85,8 +86,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 1522ed20df..4136f796a3 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,7 +1,7 @@ use crate::connection::MavConnection; 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 +31,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 +45,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 +85,13 @@ impl UdpConnection { fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { Ok(Self { server, - reader: Mutex::new(UdpRead { - socket: socket.try_clone()?, - recv_buf: PacketBuf::new(), - }), + reader: Mutex::new(buffered_reader::Generic::new( + UdpRead { + socket: socket.try_clone()?, + last_recv_address: None, + }, + None, + )), writer: Mutex::new(UdpWrite { socket, dest, @@ -131,19 +104,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 c5f86e4ac1..2b2400cbf5 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -25,7 +25,7 @@ use core::result::Result; #[cfg(feature = "std")] -use std::io::{Read, Write}; +use std::io::Write; #[cfg(feature = "std")] use byteorder::ReadBytesExt; @@ -37,6 +37,9 @@ use utils::{remove_trailing_zeroes, RustDefault}; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +#[cfg(feature = "std")] +use buffered_reader::BufferedReader; + use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; @@ -214,7 +217,7 @@ fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { crc_calculator.get_crc() } -pub fn read_versioned_msg( +pub fn read_versioned_msg>( r: &mut R, version: MavlinkVersion, ) -> Result<(MavHeader, M), error::MessageReadError> { @@ -362,52 +365,61 @@ impl MAVLinkV1MessageRaw { /// Return a raw buffer with the mavlink message /// V1 maximum size is 263 bytes: `` -pub fn read_v1_raw_message( +pub fn read_v1_raw_message>( reader: &mut R, -) -> Result { +) -> 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.data_hard(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.data_hard(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( +pub fn read_v1_msg>( r: &mut R, ) -> 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 +593,55 @@ impl MAVLinkV2MessageRaw { /// Return a raw buffer with the mavlink message /// V2 maximum size is 280 bytes: `` -pub fn read_v2_raw_message( +pub fn read_v2_raw_message>( reader: &mut R, -) -> Result { +) -> 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.data_hard(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.data_hard(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( +pub fn read_v2_msg>( read: &mut R, ) -> 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/Cargo.toml b/mavlink/Cargo.toml index 728e630a37..84ea878f49 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,12 +27,13 @@ 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 } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } +buffered-reader = { version = "1.3.0", default-features = false } [features] "all" = [ @@ -93,4 +101,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 f4b380074e..bd910fc1e3 100644 --- a/mavlink/tests/encode_decode_tests.rs +++ b/mavlink/tests/encode_decode_tests.rs @@ -16,7 +16,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = buffered_reader::Memory::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 +34,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = buffered_reader::Memory::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 +56,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = buffered_reader::Memory::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 +85,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 = buffered_reader::Memory::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 +114,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = buffered_reader::Memory::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 +138,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 = buffered_reader::Memory::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 0bf71544f8..5d60db9e38 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 db7ea6fcaf..f76f4355ec 100644 --- a/mavlink/tests/v1_encode_decode_tests.rs +++ b/mavlink/tests/v1_encode_decode_tests.rs @@ -25,7 +25,7 @@ mod test_v1_encode_decode { #[test] pub fn test_read_heartbeat() { - let mut r = HEARTBEAT_V1; + let mut r = buffered_reader::Memory::new(HEARTBEAT_V1); let (header, msg) = mavlink::read_v1_msg(&mut r).expect("Failed to parse message"); //println!("{:?}, {:?}", header, msg); @@ -73,7 +73,7 @@ mod test_v1_encode_decode { ) .expect("Failed to write message"); - let mut c = v.as_slice(); + let mut c = buffered_reader::Memory::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 0aae35bf55..8f07731ec0 100644 --- a/mavlink/tests/v2_encode_decode_tests.rs +++ b/mavlink/tests/v2_encode_decode_tests.rs @@ -28,7 +28,7 @@ mod test_v2_encode_decode { #[test] pub fn test_read_v2_heartbeat() { - let mut r = HEARTBEAT_V2; + let mut r = buffered_reader::Memory::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 +110,7 @@ mod test_v2_encode_decode { #[test] pub fn test_read_truncated_command_long() { - let mut r = COMMAND_LONG_TRUNCATED_V2; + let mut r = buffered_reader::Memory::new(COMMAND_LONG_TRUNCATED_V2); let (_header, recv_msg) = mavlink::read_v2_msg(&mut r).expect("Failed to parse COMMAND_LONG_TRUNCATED_V2"); From c8fb42be973e66513a8a16085c69bb0abafaa7b8 Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Thu, 29 Feb 2024 17:07:19 +0000 Subject: [PATCH 086/159] Replaced `buffered_reader::Generic` with own buffered reader implementation (`PeekReader`) --- mavlink-core/Cargo.toml | 1 - mavlink-core/src/connection/direct_serial.rs | 40 +----- mavlink-core/src/connection/file.rs | 5 +- mavlink-core/src/connection/tcp.rs | 7 +- mavlink-core/src/connection/udp.rs | 14 +- mavlink-core/src/lib.rs | 37 +++-- mavlink-core/src/peek_reader.rs | 139 +++++++++++++++++++ mavlink/tests/encode_decode_tests.rs | 13 +- mavlink/tests/v1_encode_decode_tests.rs | 5 +- mavlink/tests/v2_encode_decode_tests.rs | 8 +- 10 files changed, 189 insertions(+), 80 deletions(-) create mode 100644 mavlink-core/src/peek_reader.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 38ae99d4c6..f5e7d34cab 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -24,7 +24,6 @@ nb = { version = "1.0", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } -buffered-reader = { version = "1.3.0", default-features = false } [features] "std" = ["byteorder/std"] diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index 6f604277e6..341d075ef2 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.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 core::ops::DerefMut; -use std::io::{self, Read, Write}; +use std::io; use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; @@ -9,36 +10,6 @@ use serial::{prelude::*, SystemPort}; /// Serial MAVLINK connection -struct SyncSystemPort(Mutex); - -impl Read for SyncSystemPort { - fn read(&mut self, buf: &mut [u8]) -> io::Result { - let mut port = match self.0.lock() { - Ok(port) => port, - Err(err) => err.into_inner(), - }; - port.read(buf) - } -} - -impl Write for SyncSystemPort { - fn write(&mut self, buf: &[u8]) -> io::Result { - let mut port = match self.0.lock() { - Ok(port) => port, - Err(err) => err.into_inner(), - }; - port.write(buf) - } - - fn flush(&mut self) -> io::Result<()> { - let mut port = match self.0.lock() { - Ok(port) => port, - Err(err) => err.into_inner(), - }; - port.flush() - } -} - pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -71,17 +42,14 @@ pub fn open(settings: &str) -> io::Result { port.configure(&settings)?; Ok(SerialConnection { - port: Mutex::new(buffered_reader::Generic::new( - SyncSystemPort(Mutex::new(port)), - None, - )), + 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, } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index f8db8d4e33..d0fcdc9875 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,5 +1,6 @@ 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; @@ -12,13 +13,13 @@ pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; Ok(FileConnection { - file: Mutex::new(buffered_reader::Generic::new(file, None)), + file: Mutex::new(PeekReader::new(file)), protocol_version: MavlinkVersion::V2, }) } pub struct FileConnection { - file: Mutex>, + file: Mutex>, protocol_version: MavlinkVersion, } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 6262bd7292..44d3778777 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -1,4 +1,5 @@ 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; @@ -35,7 +36,7 @@ pub fn tcpout(address: T) -> io::Result { socket.set_read_timeout(Some(Duration::from_millis(100)))?; Ok(TcpConnection { - reader: Mutex::new(buffered_reader::Generic::new(socket.try_clone()?, None)), + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -53,7 +54,7 @@ pub fn tcpin(address: T) -> io::Result { match incoming { Ok(socket) => { return Ok(TcpConnection { - reader: Mutex::new(buffered_reader::Generic::new(socket.try_clone()?, None)), + reader: Mutex::new(PeekReader::new(socket.try_clone()?)), writer: Mutex::new(TcpWrite { socket, sequence: 0, @@ -74,7 +75,7 @@ pub fn tcpin(address: T) -> io::Result { } pub struct TcpConnection { - reader: Mutex>, + reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 4136f796a3..c0db512da7 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,4 +1,5 @@ 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::{self, Read}; @@ -75,7 +76,7 @@ struct UdpWrite { } pub struct UdpConnection { - reader: Mutex>, + reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, server: bool, @@ -85,13 +86,10 @@ impl UdpConnection { fn new(socket: UdpSocket, server: bool, dest: Option) -> io::Result { Ok(Self { server, - reader: Mutex::new(buffered_reader::Generic::new( - UdpRead { - socket: socket.try_clone()?, - last_recv_address: None, - }, - None, - )), + reader: Mutex::new(PeekReader::new(UdpRead { + socket: socket.try_clone()?, + last_recv_address: None, + })), writer: Mutex::new(UdpWrite { socket, dest, diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 2b2400cbf5..2bcfc68599 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -25,10 +25,7 @@ use core::result::Result; #[cfg(feature = "std")] -use std::io::Write; - -#[cfg(feature = "std")] -use byteorder::ReadBytesExt; +use std::io::{Read, Write}; pub mod utils; #[allow(unused_imports)] @@ -38,7 +35,9 @@ use utils::{remove_trailing_zeroes, RustDefault}; use serde::{Deserialize, Serialize}; #[cfg(feature = "std")] -use buffered_reader::BufferedReader; +pub mod peek_reader; +#[cfg(feature = "std")] +use peek_reader::PeekReader; use crate::{bytes::Bytes, error::ParserError}; @@ -217,8 +216,8 @@ fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { crc_calculator.get_crc() } -pub fn read_versioned_msg>( - r: &mut R, +pub fn read_versioned_msg( + r: &mut PeekReader, version: MavlinkVersion, ) -> Result<(MavHeader, M), error::MessageReadError> { match version { @@ -365,8 +364,8 @@ 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, +pub fn read_v1_raw_message( + reader: &mut PeekReader, ) -> Result { loop { loop { @@ -379,12 +378,12 @@ pub fn read_v1_raw_message>( let mut message = MAVLinkV1MessageRaw::new(); message.0[0] = MAV_STX; - let header = &reader.data_hard(MAVLinkV1MessageRaw::HEADER_SIZE)? + 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.data_hard(packet_length)?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length]; + &reader.peek_exact(packet_length)?[MAVLinkV1MessageRaw::HEADER_SIZE..packet_length]; message .mut_payload_and_checksum() .copy_from_slice(payload_and_checksum); @@ -399,8 +398,8 @@ pub fn read_v1_raw_message>( } /// Read a MAVLink v1 message from a Read stream. -pub fn read_v1_msg>( - r: &mut R, +pub fn read_v1_msg( + r: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v1_raw_message::(r)?; @@ -593,8 +592,8 @@ 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, +pub fn read_v2_raw_message( + reader: &mut PeekReader, ) -> Result { loop { loop { @@ -607,12 +606,12 @@ pub fn read_v2_raw_message>( let mut message = MAVLinkV2MessageRaw::new(); message.0[0] = MAV_STX_V2; - let header = &reader.data_hard(MAVLinkV2MessageRaw::HEADER_SIZE)? + 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.data_hard(packet_length)?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length]; + &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); @@ -625,8 +624,8 @@ pub fn read_v2_raw_message>( } /// Read a MAVLink v2 message from a Read stream. -pub fn read_v2_msg>( - read: &mut R, +pub fn read_v2_msg( + read: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message::(read)?; diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs new file mode 100644 index 0000000000..7ed14ff73c --- /dev/null +++ b/mavlink-core/src/peek_reader.rs @@ -0,0 +1,139 @@ +use std::io::{self, ErrorKind, Read}; + +const DEFAULT_CHUNK_SIZE: usize = 1024; + +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 { + pub fn new(reader: R) -> Self { + Self::with_chunk_size(reader, DEFAULT_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, + } + } + + pub fn peek(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, false, false) + } + + pub fn peek_exact(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, true, false) + } + + pub fn consume(&mut self, amount: usize) -> usize { + let amount = amount.min(self.buffer.len() - self.cursor); + self.cursor += amount; + amount + } + + pub fn read(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, false, true) + } + + pub fn read_exact(&mut self, amount: usize) -> io::Result<&[u8]> { + self.fetch(amount, true, true) + } + + pub fn read_u8(&mut self) -> io::Result { + let buf = self.read_exact(1)?; + Ok(buf[0]) + } + + pub fn reader_ref(&mut self) -> &R { + &self.reader + } + + pub fn reader_mut(&mut self) -> &mut R { + &mut self.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 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/tests/encode_decode_tests.rs b/mavlink/tests/encode_decode_tests.rs index bd910fc1e3..0cc290eb34 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 = buffered_reader::Memory::new(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 = buffered_reader::Memory::new(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 = buffered_reader::Memory::new(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,7 +86,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = buffered_reader::Memory::new(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"); match &recv_msg { @@ -114,7 +115,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = buffered_reader::Memory::new(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); @@ -138,7 +139,7 @@ mod test_encode_decode { ) .expect("Failed to write message"); - let mut c = buffered_reader::Memory::new(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"); match &recv_msg { diff --git a/mavlink/tests/v1_encode_decode_tests.rs b/mavlink/tests/v1_encode_decode_tests.rs index f76f4355ec..048e97c8e3 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 = buffered_reader::Memory::new(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 = buffered_reader::Memory::new(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 8f07731ec0..f83eb127a1 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 = buffered_reader::Memory::new(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 = buffered_reader::Memory::new(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"); From 5679e268b3c5d60d3711b1bd1c15149ccca5b59d Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Thu, 29 Feb 2024 18:39:48 +0000 Subject: [PATCH 087/159] Removed extraneous buffered-reader dependency --- mavlink/Cargo.toml | 1 - 1 file changed, 1 deletion(-) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 84ea878f49..08cf4e1ab1 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -33,7 +33,6 @@ num-derive = { workspace = true } bitflags = { workspace = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } -buffered-reader = { version = "1.3.0", default-features = false } [features] "all" = [ From ad39b2847e84ec2ec17fa5c1b551b8db05f28243 Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Thu, 29 Feb 2024 21:35:28 +0000 Subject: [PATCH 088/159] added documentation to `PeekReader --- mavlink-core/src/peek_reader.rs | 81 ++++++++++++++++++++++++++++++++- 1 file changed, 80 insertions(+), 1 deletion(-) diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs index 7ed14ff73c..8541881764 100644 --- a/mavlink-core/src/peek_reader.rs +++ b/mavlink-core/src/peek_reader.rs @@ -1,7 +1,29 @@ +//! 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, @@ -13,15 +35,17 @@ pub struct PeekReader { reader: R, // Stashed error, if any. error: Option, - /// Whether we hit EOF on the underlying reader. + // 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), @@ -33,41 +57,95 @@ impl PeekReader { } } + /// 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; @@ -109,6 +187,7 @@ impl PeekReader { } } } + // if some bytes were read, add them to the buffer if read > 0 { if self.buffer.capacity() - previous_len < read { // reallocate From 033b069a2d540866e4f5c80b4f8569696fb77a30 Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Wed, 20 Mar 2024 22:57:21 +0000 Subject: [PATCH 089/159] Simplified PeekReader for MAVLink and made it friendly --- mavlink-core/src/lib.rs | 6 +- mavlink-core/src/peek_reader.rs | 169 ++++++++++---------------------- 2 files changed, 53 insertions(+), 122 deletions(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 2bcfc68599..9ef9a06de8 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -34,9 +34,7 @@ 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}; @@ -366,7 +364,7 @@ impl MAVLinkV1MessageRaw { /// V1 maximum size is 263 bytes: `` pub fn read_v1_raw_message( reader: &mut PeekReader, -) -> Result { +) -> Result { loop { loop { // search for the magic framing value indicating start of mavlink message @@ -594,7 +592,7 @@ impl MAVLinkV2MessageRaw { /// V2 maximum size is 280 bytes: `` pub fn read_v2_raw_message( reader: &mut PeekReader, -) -> Result { +) -> Result { loop { loop { // search for the magic framing value indicating start of mavlink message diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs index 8541881764..b760b2a8a3 100644 --- a/mavlink-core/src/peek_reader.rs +++ b/mavlink-core/src/peek_reader.rs @@ -2,7 +2,7 @@ //! //! 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 +//! A reader implementing the standard library'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. //! @@ -11,11 +11,13 @@ //! The main type `PeekReader`does not implement [`std::io::Read`] itself, as there is no added benefit //! in doing so. //! +#[cfg(feature = "embedded")] +use crate::embedded::Read; -use std::io::{self, ErrorKind, Read}; +#[cfg(feature = "std")] +use std::io::Read; -// The default chunk size to read from the underlying reader -const DEFAULT_CHUNK_SIZE: usize = 1024; +use crate::error::MessageReadError; /// A buffered/peekable reader /// @@ -24,51 +26,31 @@ const DEFAULT_CHUNK_SIZE: usize = 1024; /// 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 { +/// NOTE: This reader is generic over the size of the buffer, defaulting to MAVLink's current largest +/// possible message size of 280 bytes +/// +pub struct PeekReader { // Internal buffer - buffer: Vec, - // The position of the next byte to read in the buffer. + buffer: [u8; BUFFER_SIZE], + // The position of the next byte to read from the buffer. cursor: usize, - // The preferred chunk size. This is just a hint. - preferred_chunk_size: usize, + // The position of the next byte to read into the buffer. + top: 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 +impl PeekReader { + /// Instantiates 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), + buffer: [0; BUFFER_SIZE], cursor: 0, - preferred_chunk_size, + top: 0, 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 @@ -79,29 +61,9 @@ impl PeekReader { /// 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) + pub fn peek_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + let result = self.fetch(amount, false); + result } /// Reads a specified amount of bytes from the internal buffer @@ -113,8 +75,8 @@ impl PeekReader { /// /// 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) + pub fn read_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, true) } /// Reads a byte from the internal buffer @@ -126,11 +88,21 @@ impl PeekReader { /// /// 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 { + pub fn read_u8(&mut self) -> Result { let buf = self.read_exact(1)?; Ok(buf[0]) } + /// 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.top - self.cursor); + self.cursor += amount; + amount + } + /// Returns an immutable reference to the underlying [`std::io::Read`]er /// /// Reading directly from the underlying stream will cause data loss @@ -146,72 +118,33 @@ impl PeekReader { } /// 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; + fn fetch(&mut self, amount: usize, consume: bool) -> Result<&[u8], MessageReadError> { + let buffered = self.top - self.cursor; - // the caller requested more bytes tha we have buffered, fetch them from the reader + // the caller requested more bytes than 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]; + let bytes_read = amount - buffered; + assert!(bytes_read < BUFFER_SIZE); + let mut buf = [0u8; BUFFER_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; - } - } - } + self.reader.read_exact(&mut buf[..bytes_read])?; + // 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 self.buffer.len() - self.top < bytes_read { + // reallocate + self.buffer.copy_within(self.cursor..self.top, 0); + self.cursor = 0; + self.top = buffered; } - } - if exact && buffered < amount { - return Err(io::Error::new(ErrorKind::UnexpectedEof, "Unexpected EOF")); + self.buffer[self.top..self.top + bytes_read].copy_from_slice(&buf[..bytes_read]); + self.top += bytes_read; } - let result_len = amount.min(buffered); - let result = &self.buffer[self.cursor..self.cursor + result_len]; + let result = &self.buffer[self.cursor..self.cursor + amount]; if consume { - self.cursor += result_len; + self.cursor += amount; } Ok(result) } From 8d61f75ac4a5bf04cd9401d9073244118117db5f Mon Sep 17 00:00:00 2001 From: jbrown <64260615+JorgeCastanho@users.noreply.github.com> Date: Wed, 27 Mar 2024 10:50:51 +0000 Subject: [PATCH 090/159] Changed rerun-if-changed to full xml path of message definitions --- mavlink-bindgen/src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index da3145bed3..f5e63da800 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -68,7 +68,7 @@ fn _generate( bindings.push(GeneratedBinding { module_name, - mavlink_xml: definition_file, + mavlink_xml: entry.path(), rust_module: dest_path, }); } @@ -119,7 +119,7 @@ pub fn emit_cargo_build_messages(result: &GeneratedBindings) { // Re-run build if definition file changes println!( "cargo:rerun-if-changed={}", - binding.rust_module.to_string_lossy() + binding.mavlink_xml.to_string_lossy() ); } } From 1208960092e6fd942d240d67670f6dc9cb3eb73b Mon Sep 17 00:00:00 2001 From: Philipp Borgers Date: Thu, 28 Mar 2024 19:11:30 +0100 Subject: [PATCH 091/159] Fix URLs in README pointing to examples --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index e7871ae8ed..45ca6868f7 100644 --- a/README.md +++ b/README.md @@ -14,10 +14,10 @@ mavlink = "0.12.2" ``` ## Examples -See [examples/](examples/mavlink-dump/src/main) for different usage examples. +See [examples/](mavlink/examples/mavlink-dump/src/main) for different usage examples. ### mavlink-dump -[examples/mavlink-dump](examples/mavlink-dump/src/main) contains an executable example that can be used to test message reception. +[examples/mavlink-dump](mavlink/examples/mavlink-dump/src/main) contains an executable example that can be used to test message reception. It can be executed directly by running: ``` From 96079f49c65939e8da377baf1cda5a63729fd339 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3oFRF?= Date: Thu, 2 May 2024 15:02:48 +0100 Subject: [PATCH 092/159] changed fn_calculate_crc to pub QOL --- mavlink-core/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 9ef9a06de8..0e6b653157 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -206,7 +206,7 @@ impl MavFrame { } } -fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { +pub fn calculate_crc(data: &[u8], extra_crc: u8) -> u16 { let mut crc_calculator = CRCu16::crc16mcrf4cc(); crc_calculator.digest(data); From cdd585f14dcadcc90d3655ca69e2c8cbc7ddd751 Mon Sep 17 00:00:00 2001 From: Tiago Marques Date: Wed, 24 Apr 2024 12:13:45 +0100 Subject: [PATCH 093/159] Added (back) buffering to UDP's Read implementation --- mavlink-core/src/connection/udp.rs | 62 ++++++++++++++++++++++++++++-- 1 file changed, 58 insertions(+), 4 deletions(-) diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index c0db512da7..b353ea1ba1 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,3 +1,5 @@ +use std::collections::VecDeque; + use crate::connection::MavConnection; use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; @@ -57,15 +59,24 @@ pub fn udpin(address: T) -> io::Result { struct UdpRead { socket: UdpSocket, + buffer: VecDeque, last_recv_address: Option, } +const MTU_SIZE: usize = 1500; impl Read for UdpRead { fn read(&mut self, buf: &mut [u8]) -> io::Result { - self.socket.recv_from(buf).map(|(n, addr)| { - self.last_recv_address = Some(addr); - n - }) + if !self.buffer.is_empty() { + self.buffer.read(buf) + } else { + let mut read_buffer = [0u8; MTU_SIZE]; + let (n_buffer, address) = self.socket.recv_from(&mut read_buffer)?; + let n = (&read_buffer[0..n_buffer]).read(buf)?; + self.buffer.extend(&read_buffer[n..n_buffer]); + + self.last_recv_address = Some(address); + Ok(n) + } } } @@ -88,6 +99,7 @@ impl UdpConnection { server, reader: Mutex::new(PeekReader::new(UdpRead { socket: socket.try_clone()?, + buffer: VecDeque::new(), last_recv_address: None, })), writer: Mutex::new(UdpWrite { @@ -148,3 +160,45 @@ impl MavConnection for UdpConnection { self.protocol_version } } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_datagram_buffering() { + let receiver_socket = UdpSocket::bind("127.0.0.1:5000").unwrap(); + let mut udp_reader = UdpRead { + socket: receiver_socket.try_clone().unwrap(), + buffer: VecDeque::new(), + last_recv_address: None, + }; + let sender_socket = UdpSocket::bind("0.0.0.0:0").unwrap(); + sender_socket.connect("127.0.0.1:5000").unwrap(); + + let datagram: Vec = (0..50).collect::>(); + + let mut n_sent = sender_socket.send(&datagram).unwrap(); + assert_eq!(n_sent, datagram.len()); + n_sent = sender_socket.send(&datagram).unwrap(); + assert_eq!(n_sent, datagram.len()); + + let mut buf = [0u8; 30]; + + let mut n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + } +} From a67f1d888e1676e109d75b6652a47dffafa00cb4 Mon Sep 17 00:00:00 2001 From: TB-ME Date: Fri, 19 Apr 2024 12:40:48 +1000 Subject: [PATCH 094/159] changed `main` to `main.rs` for markdown hyperlinks --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 45ca6868f7..689c6bf1f5 100644 --- a/README.md +++ b/README.md @@ -14,10 +14,10 @@ mavlink = "0.12.2" ``` ## Examples -See [examples/](mavlink/examples/mavlink-dump/src/main) for different usage examples. +See [examples/](mavlink/examples/mavlink-dump/src/main.rs) for different usage examples. ### mavlink-dump -[examples/mavlink-dump](mavlink/examples/mavlink-dump/src/main) contains an executable example that can be used to test message reception. +[examples/mavlink-dump](mavlink/examples/mavlink-dump/src/main.rs) contains an executable example that can be used to test message reception. It can be executed directly by running: ``` From f47c61f74b7255bd7e37d82c56410812e1dd9551 Mon Sep 17 00:00:00 2001 From: Salman Abuhaimed Date: Sun, 31 Mar 2024 15:31:57 +0300 Subject: [PATCH 095/159] print bindgen cli error --- mavlink-bindgen/src/cli.rs | 9 +++++---- mavlink-bindgen/src/error.rs | 12 +++++++++--- mavlink-bindgen/src/lib.rs | 4 ++-- mavlink-bindgen/src/main.rs | 13 +++++++++++-- mavlink-bindgen/src/parser.rs | 24 ++++++++++++++++++------ mavlink/build/main.rs | 17 +++++++++++++---- 6 files changed, 58 insertions(+), 21 deletions(-) diff --git a/mavlink-bindgen/src/cli.rs b/mavlink-bindgen/src/cli.rs index 02eb59038f..30fee8a843 100644 --- a/mavlink-bindgen/src/cli.rs +++ b/mavlink-bindgen/src/cli.rs @@ -1,7 +1,7 @@ use std::path::PathBuf; use clap::Parser; -use mavlink_bindgen::{emit_cargo_build_messages, format_generated_code, generate}; +use mavlink_bindgen::{emit_cargo_build_messages, format_generated_code, generate, BindGenError}; #[derive(Parser)] struct Cli { @@ -13,10 +13,9 @@ struct Cli { emit_cargo_build_messages: bool, } -pub fn main() { +pub fn main() -> Result<(), BindGenError> { let args = Cli::parse(); - let result = generate(args.definitions_dir, args.destination_dir) - .expect("failed to generate MAVLink Rust bindings"); + let result = generate(args.definitions_dir, args.destination_dir)?; if args.format_generated_code { format_generated_code(&result); @@ -25,4 +24,6 @@ pub fn main() { if args.emit_cargo_build_messages { emit_cargo_build_messages(&result); } + + Ok(()) } diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs index fec283634b..957ec5506d 100644 --- a/mavlink-bindgen/src/error.rs +++ b/mavlink-bindgen/src/error.rs @@ -3,19 +3,25 @@ use thiserror::Error; #[derive(Error, Debug)] pub enum BindGenError { /// Represents a failure to read the MAVLink definitions directory. - #[error("Could not read definitions directory {path}")] + #[error("Could not read definitions directory {path}: {source}")] CouldNotReadDefinitionsDirectory { source: std::io::Error, path: std::path::PathBuf, }, + /// Represents a failure to read the MAVLink definitions directory. + #[error("Could not read definition file {path}: {source}")] + CouldNotReadDefinitionFile { + source: std::io::Error, + path: std::path::PathBuf, + }, /// Represents a failure to read a directory entry in the MAVLink definitions directory. - #[error("Could not read MAVLink definitions directory entry {path}")] + #[error("Could not read MAVLink definitions directory entry {path}: {source}")] CouldNotReadDirectoryEntryInDefinitionsDirectory { source: std::io::Error, path: std::path::PathBuf, }, /// Represents a failure to create a Rust file for the generated MAVLink bindings. - #[error("Could not create Rust bindings file {dest_path}")] + #[error("Could not create Rust bindings file {dest_path}: {source}")] CouldNotCreateRustBindingsFile { source: std::io::Error, dest_path: std::path::PathBuf, diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index f5e63da800..3448fe88da 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -1,4 +1,4 @@ -use crate::error::BindGenError; +pub use crate::error::BindGenError; use std::fs::{read_dir, File}; use std::io::BufWriter; use std::ops::Deref; @@ -64,7 +64,7 @@ fn _generate( })?); // generate code - parser::generate(&definitions_dir, &definition_file, &mut outf); + parser::generate(&definitions_dir, &definition_file, &mut outf)?; bindings.push(GeneratedBinding { module_name, diff --git a/mavlink-bindgen/src/main.rs b/mavlink-bindgen/src/main.rs index 660ba5ec7f..f2c68ba787 100644 --- a/mavlink-bindgen/src/main.rs +++ b/mavlink-bindgen/src/main.rs @@ -1,11 +1,20 @@ #![recursion_limit = "256"] +use std::process::ExitCode; + #[cfg(feature = "cli")] mod cli; -pub fn main() { +fn main() -> ExitCode { #[cfg(feature = "cli")] - cli::main(); + if let Err(e) = cli::main() { + eprintln!("{e}"); + return ExitCode::FAILURE; + } + #[cfg(not(feature = "cli"))] panic!("Compiled without cli feature"); + + #[cfg(feature = "cli")] + ExitCode::SUCCESS } diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 0d273e663d..1a6eaba7e3 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -17,6 +17,8 @@ use quote::{format_ident, quote}; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; +use crate::error::BindGenError; + #[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavProfile { @@ -1033,7 +1035,7 @@ pub fn parse_profile( definitions_dir: &Path, definition_file: &Path, parsed_files: &mut HashSet, -) -> MavProfile { +) -> Result { let in_path = Path::new(&definitions_dir).join(definition_file); parsed_files.insert(in_path.clone()); // Keep track of which files have been parsed @@ -1049,7 +1051,11 @@ pub fn parse_profile( let mut xml_filter = MavXmlFilter::default(); let mut events: Vec> = Vec::new(); - let mut reader = Reader::from_reader(BufReader::new(File::open(in_path).unwrap())); + let file = File::open(&in_path).map_err(|e| BindGenError::CouldNotReadDefinitionFile { + source: e, + path: in_path.to_path_buf(), + })?; + let mut reader = Reader::from_reader(BufReader::new(file)); reader.trim_text(true); reader.trim_text_end(true); @@ -1331,7 +1337,7 @@ pub fn parse_profile( let include_file = Path::new(&definitions_dir).join(include.clone()); if !parsed_files.contains(&include_file) { let included_profile = - parse_profile(definitions_dir, &include, parsed_files); + parse_profile(definitions_dir, &include, parsed_files)?; for message in included_profile.messages.values() { profile.add_message(message); } @@ -1354,18 +1360,24 @@ pub fn parse_profile( } //let profile = profile.update_messages(); //TODO verify no longer needed - profile.update_enums() + Ok(profile.update_enums()) } /// Generate protobuf represenation of mavlink message set /// Generate rust representation of mavlink message set with appropriate conversion methods -pub fn generate(definitions_dir: &Path, definition_file: &Path, output_rust: &mut W) { +pub fn generate( + definitions_dir: &Path, + definition_file: &Path, + output_rust: &mut W, +) -> Result<(), BindGenError> { let mut parsed_files: HashSet = HashSet::new(); - let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files); + let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?; // rust file let rust_tokens = profile.emit_rust(); writeln!(output_rust, "{rust_tokens}").unwrap(); + + Ok(()) } /// CRC operates over names of the message and names of its fields diff --git a/mavlink/build/main.rs b/mavlink/build/main.rs index 7206d20658..4d1cfd4c73 100644 --- a/mavlink/build/main.rs +++ b/mavlink/build/main.rs @@ -3,9 +3,9 @@ use std::env; use std::fs::read_dir; use std::path::Path; -use std::process::Command; +use std::process::{Command, ExitCode}; -pub fn main() { +fn main() -> ExitCode { let src_dir = Path::new(env!("CARGO_MANIFEST_DIR")); // Update and init submodule @@ -17,6 +17,7 @@ pub fn main() { .status() { eprintln!("{error}"); + return ExitCode::FAILURE; } // find & apply patches to XML definitions to avoid crashes @@ -34,6 +35,7 @@ pub fn main() { .status() { eprintln!("{error}"); + return ExitCode::FAILURE; } } } @@ -43,11 +45,18 @@ pub fn main() { let out_dir = env::var("OUT_DIR").unwrap(); - let result = mavlink_bindgen::generate(definitions_dir, out_dir) - .expect("Failed to generate Rust MAVLink bindings"); + let result = match mavlink_bindgen::generate(definitions_dir, out_dir) { + Ok(r) => r, + Err(e) => { + eprintln!("{e}"); + return ExitCode::FAILURE; + } + }; #[cfg(feature = "format-generated-code")] mavlink_bindgen::format_generated_code(&result); mavlink_bindgen::emit_cargo_build_messages(&result); + + ExitCode::SUCCESS } From 8c342c11ff07a68c34eee8253e43cac8a5286308 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=A0=D0=BE=D0=BC=D0=B0=D0=BD=20=D0=9A=D1=80=D0=B8=D0=B2?= =?UTF-8?q?=D0=B5=D0=BD=D0=BA=D0=BE=D0=B2?= Date: Mon, 17 Jun 2024 18:21:52 +0400 Subject: [PATCH 096/159] Update embedded-hal to 1.0. Add async methods. Add async example (Embassy executor). --- mavlink-core/Cargo.toml | 10 +- mavlink-core/src/embedded.rs | 33 ++- mavlink-core/src/error.rs | 8 +- mavlink-core/src/lib.rs | 209 +++++++++++++++++- mavlink-core/src/peek_reader.rs | 2 +- mavlink/Cargo.toml | 4 + .../embedded-async-read/.cargo/config.toml | 7 + .../examples/embedded-async-read/Cargo.toml | 44 ++++ .../examples/embedded-async-read/README.md | 11 + .../examples/embedded-async-read/src/main.rs | 93 ++++++++ mavlink/examples/embedded/Cargo.toml | 2 +- 11 files changed, 403 insertions(+), 20 deletions(-) create mode 100644 mavlink/examples/embedded-async-read/.cargo/config.toml create mode 100644 mavlink/examples/embedded-async-read/Cargo.toml create mode 100644 mavlink/examples/embedded-async-read/README.md create mode 100644 mavlink/examples/embedded-async-read/src/main.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index f5e7d34cab..82b2410b00 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -19,8 +19,10 @@ rust-version = "1.65.0" [dependencies] crc-any = { workspace = true, default-features = false } byteorder = { workspace = true, default-features = false } -embedded-hal = { version = "0.2", optional = true } nb = { version = "1.0", optional = true } +embedded-hal-02 = { version = "0.2", optional = true, package = "embedded-hal" } +embedded-io = { version = "0.6.1", optional = true } +embedded-io-async = { version = "0.6.1", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } @@ -30,6 +32,10 @@ serial = { version = "0.4", optional = true } "udp" = [] "tcp" = [] "direct-serial" = ["serial"] -"embedded" = ["embedded-hal", "nb"] +# NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. +# Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). +# Use 'embedded-hal-0.2' feature to enable deprecated embedded-hal=0.2.3 (some hals is not supports embedded-hal=1.0 yet). +"embedded" = ["dep:embedded-io", "dep:embedded-io-async"] +"embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink-core/src/embedded.rs b/mavlink-core/src/embedded.rs index 574ae2b826..96917e506c 100644 --- a/mavlink-core/src/embedded.rs +++ b/mavlink-core/src/embedded.rs @@ -1,30 +1,45 @@ use crate::error::*; +#[cfg(all(feature = "embedded", feature = "embedded-hal-02"))] +const _: () = panic!("Only one of 'embedded' and 'embedded-hal-02' features can be enabled."); + /// Replacement for std::io::Read + byteorder::ReadBytesExt in no_std envs pub trait Read { - fn read_u8(&mut self) -> Result; + fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError>; +} +#[cfg(all(feature = "embedded", not(feature = "embedded-hal-02")))] +impl Read for R { + fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { + embedded_io::Read::read_exact(self, buf).map_err(|_| MessageReadError::Io) + } +} + +#[cfg(all(feature = "embedded-hal-02", not(feature = "embedded")))] +impl> Read for R { fn read_exact(&mut self, buf: &mut [u8]) -> Result<(), MessageReadError> { for byte in buf { - *byte = self.read_u8()?; + *byte = nb::block!(self.read()).map_err(|_| MessageReadError::Io)?; } Ok(()) } } -impl> Read for R { - fn read_u8(&mut self) -> Result { - nb::block!(self.read()).map_err(|_| MessageReadError::Io) - } -} - /// Replacement for std::io::Write + byteorder::WriteBytesExt in no_std envs pub trait Write { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError>; } -impl> Write for W { +#[cfg(all(feature = "embedded", not(feature = "embedded-hal-02")))] +impl Write for W { + fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { + embedded_io::Write::write_all(self, buf).map_err(|_| MessageWriteError::Io) + } +} + +#[cfg(all(feature = "embedded-hal-02", not(feature = "embedded")))] +impl> Write for W { fn write_all(&mut self, buf: &[u8]) -> Result<(), MessageWriteError> { for byte in buf { nb::block!(self.write(*byte)).map_err(|_| MessageWriteError::Io)?; diff --git a/mavlink-core/src/error.rs b/mavlink-core/src/error.rs index b60136bd09..49394f8d64 100644 --- a/mavlink-core/src/error.rs +++ b/mavlink-core/src/error.rs @@ -32,7 +32,7 @@ impl Error for ParserError {} pub enum MessageReadError { #[cfg(feature = "std")] Io(std::io::Error), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Io, Parse(ParserError), } @@ -42,7 +42,7 @@ impl Display for MessageReadError { match self { #[cfg(feature = "std")] Self::Io(e) => write!(f, "Failed to read message: {e:#?}"), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Self::Io => write!(f, "Failed to read message"), Self::Parse(e) => write!(f, "Failed to read message: {e:#?}"), } @@ -69,7 +69,7 @@ impl From for MessageReadError { pub enum MessageWriteError { #[cfg(feature = "std")] Io(std::io::Error), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Io, } @@ -78,7 +78,7 @@ impl Display for MessageWriteError { match self { #[cfg(feature = "std")] Self::Io(e) => write!(f, "Failed to write message: {e:#?}"), - #[cfg(feature = "embedded")] + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] Self::Io => write!(f, "Failed to write message"), } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 0e6b653157..ef18bfa994 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -49,9 +49,9 @@ pub mod error; #[cfg(feature = "std")] pub use self::connection::{connect, MavConnection}; -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] pub mod embedded; -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; pub const MAX_FRAME_SIZE: usize = 280; @@ -395,7 +395,49 @@ pub fn read_v1_raw_message( } } -/// Read a MAVLink v1 message from a Read stream. +/// Async read a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: `` +/// +/// # Example +/// See mavlink/examples/embedded-async-read full example for details. +#[cfg(feature = "embedded")] +pub async fn read_v1_raw_message_async( + reader: &mut impl embedded_io_async::Read, +) -> Result { + loop { + // search for the magic framing value indicating start of mavlink message + let mut byte = [0u8]; + loop { + reader + .read_exact(&mut byte) + .await + .map_err(|_| error::MessageReadError::Io)?; + if byte[0] == MAV_STX { + break; + } + } + + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + reader + .read_exact(message.mut_header()) + .await + .map_err(|_| error::MessageReadError::Io)?; + reader + .read_exact(message.mut_payload_and_checksum()) + .await + .map_err(|_| error::MessageReadError::Io)?; + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + return Ok(message); + } + } +} + +/// Read a MAVLink v1 message from a Read stream. pub fn read_v1_msg( r: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { @@ -419,6 +461,34 @@ pub fn read_v1_msg( .map_err(|err| err.into()) } +/// Async read a MAVLink v1 message from a Read stream. +/// +/// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn read_v1_msg_async( + r: &mut impl embedded_io_async::Read, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v1_raw_message_async::(r).await?; + + 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_err(|err| err.into()) +} + const MAVLINK_IFLAG_SIGNED: u8 = 0x01; #[derive(Debug, Copy, Clone, PartialEq, Eq)] @@ -621,6 +691,48 @@ pub fn read_v2_raw_message( } } +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +/// +/// # Example +/// See mavlink/examples/embedded-async-read full example for details. +#[cfg(feature = "embedded")] +pub async fn read_v2_raw_message_async( + reader: &mut impl embedded_io_async::Read, +) -> Result { + loop { + // search for the magic framing value indicating start of mavlink message + let mut byte = [0u8]; + loop { + reader + .read_exact(&mut byte) + .await + .map_err(|_| error::MessageReadError::Io)?; + if byte[0] == MAV_STX_V2 { + break; + } + } + + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX; + reader + .read_exact(message.mut_header()) + .await + .map_err(|_| error::MessageReadError::Io)?; + reader + .read_exact(message.mut_payload_and_checksum_and_sign()) + .await + .map_err(|_| error::MessageReadError::Io)?; + + // retry if CRC failed after previous STX + // (an STX byte may appear in the middle of a message) + if message.has_valid_crc::() { + return Ok(message); + } + } +} + /// Read a MAVLink v2 message from a Read stream. pub fn read_v2_msg( read: &mut PeekReader, @@ -641,6 +753,34 @@ pub fn read_v2_msg( .map_err(|err| err.into()) } +/// Async read a MAVLink v2 message from a Read stream. +/// +/// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. +/// Use `*_DATA::deser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn read_v2_msg_async( + r: &mut R, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_async::(r).await?; + + M::parse( + MavlinkVersion::V2, + 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_err(|err| err.into()) +} + /// Write a message using the given mavlink version pub fn write_versioned_msg( w: &mut W, @@ -654,6 +794,23 @@ pub fn write_versioned_msg( } } +/// Async write a message using the given mavlink version +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_versioned_msg_async( + w: &mut impl embedded_io_async::Write, + version: MavlinkVersion, + header: MavHeader, + data: &M, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await, + MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await, + } +} + /// Write a MAVLink v2 message to a Write stream. pub fn write_v2_msg( w: &mut W, @@ -671,6 +828,29 @@ pub fn write_v2_msg( Ok(len) } +/// Async write a MAVLink v2 message to a Write stream. +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_v2_msg_async( + w: &mut impl embedded_io_async::Write, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]) + .await + .map_err(|_| error::MessageWriteError::Io)?; + + Ok(len) +} + /// Write a MAVLink v1 message to a Write stream. pub fn write_v1_msg( w: &mut W, @@ -687,3 +867,26 @@ pub fn write_v1_msg( Ok(len) } + +/// Write a MAVLink v1 message to a Write stream. +/// +/// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. +/// Use `*_DATA::ser` methods manually to prevent it. +#[cfg(feature = "embedded")] +pub async fn write_v1_msg_async( + w: &mut impl embedded_io_async::Write, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV1MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]) + .await + .map_err(|_| error::MessageWriteError::Io)?; + + Ok(len) +} diff --git a/mavlink-core/src/peek_reader.rs b/mavlink-core/src/peek_reader.rs index b760b2a8a3..5a5a8a11c5 100644 --- a/mavlink-core/src/peek_reader.rs +++ b/mavlink-core/src/peek_reader.rs @@ -11,7 +11,7 @@ //! The main type `PeekReader`does not implement [`std::io::Read`] itself, as there is no added benefit //! in doing so. //! -#[cfg(feature = "embedded")] +#[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use crate::embedded::Read; #[cfg(feature = "std")] diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 08cf4e1ab1..c21c797b0d 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -93,7 +93,11 @@ serde_arrays = { version = "0.1.0", optional = true } "udp" = ["mavlink-core/udp"] "tcp" = ["mavlink-core/tcp"] "direct-serial" = ["mavlink-core/direct-serial"] +# NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. +# Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). +# Use 'embedded-hal-0.2' feature to enable deprecated embedded-hal=0.2.3 (some hals is not supports embedded-hal=1.0 yet). "embedded" = ["mavlink-core/embedded"] +"embedded-hal-02" = ["mavlink-core/embedded-hal-02"] "serde" = ["mavlink-core/serde", "dep:serde", "dep:serde_arrays"] default = ["std", "tcp", "udp", "direct-serial", "serde", "ardupilotmega"] diff --git a/mavlink/examples/embedded-async-read/.cargo/config.toml b/mavlink/examples/embedded-async-read/.cargo/config.toml new file mode 100644 index 0000000000..2c0f3e9d15 --- /dev/null +++ b/mavlink/examples/embedded-async-read/.cargo/config.toml @@ -0,0 +1,7 @@ +[target.thumbv7em-none-eabihf] +rustflags = [ + "-C", "link-arg=-Tlink.x", +] + +[build] +target = "thumbv7em-none-eabihf" diff --git a/mavlink/examples/embedded-async-read/Cargo.toml b/mavlink/examples/embedded-async-read/Cargo.toml new file mode 100644 index 0000000000..20712a4307 --- /dev/null +++ b/mavlink/examples/embedded-async-read/Cargo.toml @@ -0,0 +1,44 @@ +[package] +name = "mavlink-embedded-async-read" +edition = "2021" +authors = ["Patrick José Pereira "] +version = "0.1.0" + +[profile.release] +opt-level = 'z' # Optimize for binary size, but also turn off loop vectorization. +lto = true # Performs "fat" LTO which attempts to perform optimizations across all crates within the dependency graph + +[dependencies] +cortex-m = { version = "0.7", features = [ + "inline-asm", + "critical-section-single-core", +] } # Low level access to Cortex-M processors +cortex-m-rt = "0.7" # Startup code and minimal runtime for Cortex-M microcontrollers +rtt-target = "0.5" +panic-rtt-target = "0.1" # Panic handler +static_cell = "2.1" + +embassy-time = { version = "0.3", features = ["tick-hz-32_768"] } +embassy-executor = { version = "0.5", features = [ + "arch-cortex-m", + "executor-thread", + "executor-interrupt", + "integrated-timers", +] } +embassy-stm32 = { version = "0.1", features = [ + "memory-x", + "stm32f446re", + "time-driver-any", +] } + +[dependencies.mavlink] # MAVLink library (wait for 0.9.0 version) +path = "../../" +features = ["common", "embedded"] +default-features = false + +[patch.crates-io] +embassy-time-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } +embassy-time-queue-driver = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } +embassy-stm32 = { git = "https://github.com/embassy-rs/embassy.git", rev = "86c48dde4192cabcad22faa10cabb4dc5f035c0a" } + +[workspace] diff --git a/mavlink/examples/embedded-async-read/README.md b/mavlink/examples/embedded-async-read/README.md new file mode 100644 index 0000000000..8bbf41c145 --- /dev/null +++ b/mavlink/examples/embedded-async-read/README.md @@ -0,0 +1,11 @@ +# rust-MAVLink Embedded async example (with reading loop) +### How to run: +- Install cargo flash: + - cargo install cargo-flash +- Install target + - rustup target add thumbv7em-none-eabihf +- Check if we can build the project + - cargo build +- Connect your STM32f446re board +- Flash it! + - cargo flash --chip stm32f446RETx --release --log info diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs new file mode 100644 index 0000000000..af76a2640a --- /dev/null +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -0,0 +1,93 @@ +//! Target board: stm32f446RETx (stm32nucleo) +//! Manual: https://www.st.com/resource/en/reference_manual/dm00043574-stm32f303xb-c-d-e-stm32f303x6-8-stm32f328x8-stm32f358xc-stm32f398xe-advanced-arm-based-mcus-stmicroelectronics.pdf +#![no_main] +#![no_std] + +// Panic handler +use panic_rtt_target as _; + +use embassy_executor::Spawner; +use embassy_stm32::{bind_interrupts, mode::Async, peripherals::*, usart}; +use embassy_time::Timer; +use mavlink; +use mavlink::common::{MavMessage, HEARTBEAT_DATA}; +use mavlink::{read_v2_raw_message_async, MAVLinkV2MessageRaw, MavlinkVersion, MessageData}; +use rtt_target::{rprintln, rtt_init_print}; +use static_cell::ConstStaticCell; + +bind_interrupts!(struct Irqs { + USART1 => usart::InterruptHandler; +}); + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + rtt_init_print!(); + + // Peripherals access + let p = embassy_stm32::init(embassy_stm32::Config::default()); + + // Create an interface USART2 with 115200 baudrate + let mut config = usart::Config::default(); + config.baudrate = 115200; + let serial = usart::Uart::new( + p.USART1, p.PA10, p.PA9, Irqs, p.DMA2_CH7, p.DMA2_CH2, config, + ) + .unwrap(); + + // Break serial in TX and RX (not used) + let (mut tx, rx) = serial.split(); + + // Create our mavlink header and heartbeat message + let header = mavlink::MavHeader { + system_id: 1, + component_id: 1, + sequence: 42, + }; + let heartbeat = mavlink::common::HEARTBEAT_DATA { + custom_mode: 0, + mavtype: mavlink::common::MavType::MAV_TYPE_SUBMARINE, + autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_ARDUPILOTMEGA, + base_mode: mavlink::common::MavModeFlag::empty(), + system_status: mavlink::common::MavState::MAV_STATE_STANDBY, + mavlink_version: 0x3, + }; + + // Spawn Rx loop + spawner.spawn(rx_task(rx)).unwrap(); + + // Main loop + loop { + // Write the raw heartbeat message to reduce firmware flash size (using Message::ser will be add ~70KB because + // all *_DATA::ser methods will be add to firmware). + let mut raw = MAVLinkV2MessageRaw::new(); + raw.serialize_message_data(header, &heartbeat); + tx.write(raw.raw_bytes()).await.unwrap(); + + // Delay for 1 second + Timer::after_millis(1000).await; + } +} + +#[embassy_executor::task] +pub async fn rx_task(rx: usart::UartRx<'static, Async>) { + // Make ring-buffered RX (over DMA) + static BUF_MEMORY: ConstStaticCell<[u8; 1024]> = ConstStaticCell::new([0; 1024]); + let mut rx_buffered = rx.into_ring_buffered(BUF_MEMORY.take()); + + loop { + // Read raw message to reduce firmware flash size (using read_v2_msg_async will be add ~80KB because + // all *_DATA::deser methods will be add to firmware). + let raw = read_v2_raw_message_async::(&mut rx_buffered) + .await + .unwrap(); + rprintln!("Read raw message: msg_id={}", raw.message_id()); + + match raw.message_id() { + HEARTBEAT_DATA::ID => { + let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); + rprintln!("heartbeat: {:?}", heartbeat); + } + _ => {} + } + } +} diff --git a/mavlink/examples/embedded/Cargo.toml b/mavlink/examples/embedded/Cargo.toml index 717a0c94d1..4b0801d66d 100644 --- a/mavlink/examples/embedded/Cargo.toml +++ b/mavlink/examples/embedded/Cargo.toml @@ -18,7 +18,7 @@ stm32f3xx-hal = { version = "0.9", features = ["stm32f303xe"] } # HAL for stm32f [dependencies.mavlink] # MAVLink library (wait for 0.9.0 version) path = "../../" -features = ["ardupilotmega", "embedded"] +features = ["ardupilotmega", "embedded-hal-02"] default-features = false [workspace] \ No newline at end of file From 249e4c8629048db50dc03bded0ae0f9aa647081f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 06:02:26 -0300 Subject: [PATCH 097/159] mavlink-core: Update to 0.13.1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- mavlink-core/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 82b2410b00..3726398515 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-core" -version = "0.12.2" +version = "0.13.1" authors = [ "Todd Stellanova", "Michal Podhradsky", From 19ad62d18b12cf123d1107a51a1ee8f441c9d924 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 06:04:25 -0300 Subject: [PATCH 098/159] mavlink-core: Fix README path MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- mavlink-core/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 3726398515..e4df6bbab6 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -10,7 +10,7 @@ authors = [ "Ibiyemi Abiodun", ] description = "Implements the MAVLink data interchange format for UAVs." -readme = "README.md" +readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition = "2018" From 66588afb083c4be955acd65e97274eceb9ec2091 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 06:06:20 -0300 Subject: [PATCH 099/159] mavlink-bindgen: Update Cargo description MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- mavlink-bindgen/Cargo.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index e5f3ee534b..7a59e26781 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -2,6 +2,8 @@ name = "mavlink-bindgen" version = "0.1.0" edition = "2021" +license = "MIT/Apache-2.0" +description = "Library used by rust-mavlink." # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html From 594be0c7936e32f41dd933a9d2b9bf61c9d6860a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 06:39:58 -0300 Subject: [PATCH 100/159] Release 0.13.1 mavlink@0.13.1 mavlink-bindgen@0.13.1 mavlink-core@0.13.1 Generated by cargo-workspaces --- mavlink-bindgen/Cargo.toml | 2 +- mavlink/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index 7a59e26781..1d8cf1ffd6 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-bindgen" -version = "0.1.0" +version = "0.13.1" edition = "2021" license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index c21c797b0d..280393172f 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.12.2" +version = "0.13.1" authors = [ "Todd Stellanova", "Michal Podhradsky", From eea939b3fd6aa4f1c3e6f3d43f2a9bdc1f362830 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 10:33:37 -0300 Subject: [PATCH 101/159] Add new release flow MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/deploy.yml | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 177e7ee22a..f27431787e 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -16,6 +16,18 @@ jobs: override: true - name: Build run: cargo build - - uses: katyo/publish-crates@v1 + - name: Extract version from tag + id: get_version + run: echo "::set-output name=version::${GITHUB_REF/refs\/tags\//}" + - name: Login to crates.io + uses: actions-rs/cargo@v1 with: - registry-token: ${{ secrets.CARGO }} + command: login + args: ${{ secrets.CARGO }} + - name: Set and publish workspace crates + run: | + cargo install cargo-workspaces + cargo workspaces version custom ${{ steps.get_version.outputs.version }} \ + --exact --yes --no-individual-tags \ + -m "Commit new release ${{ steps.get_version.outputs.version }}" + cargo workspaces publish --yes --no-verify From 26ecf38099736a0bfb92c15d5c65a2800f36ffdd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 10:49:18 -0300 Subject: [PATCH 102/159] workflows: Fix checkout MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/deploy.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index f27431787e..8472042984 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -10,6 +10,9 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master + with: + fetch-depth: 0 + ref: ${{ github.event.pull_request.head.ref }} - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable From 9cd7bf0b8c720d255232a389051fb60f5fc92c2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Thu, 18 Jul 2024 14:58:10 -0300 Subject: [PATCH 103/159] deploy: Fix action MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- .github/workflows/deploy.yml | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/.github/workflows/deploy.yml b/.github/workflows/deploy.yml index 8472042984..89244914c3 100644 --- a/.github/workflows/deploy.yml +++ b/.github/workflows/deploy.yml @@ -12,7 +12,7 @@ jobs: - uses: actions/checkout@master with: fetch-depth: 0 - ref: ${{ github.event.pull_request.head.ref }} + ref: ${{ github.event.repository.default_branch }} - uses: actions-rs/toolchain@v1.0.7 with: toolchain: stable @@ -22,15 +22,19 @@ jobs: - name: Extract version from tag id: get_version run: echo "::set-output name=version::${GITHUB_REF/refs\/tags\//}" - - name: Login to crates.io - uses: actions-rs/cargo@v1 - with: - command: login - args: ${{ secrets.CARGO }} + - name: Commit version changes + run: | + git config --global user.name 'github-actions[bot]' + git config --global user.email '41898282+github-actions[bot]@users.noreply.github.com' - name: Set and publish workspace crates + env: + CARGO_REGISTRY_TOKEN: ${{ secrets.CARGO }} run: | cargo install cargo-workspaces cargo workspaces version custom ${{ steps.get_version.outputs.version }} \ - --exact --yes --no-individual-tags \ + --exact --yes --no-git-tag --no-git-push \ -m "Commit new release ${{ steps.get_version.outputs.version }}" - cargo workspaces publish --yes --no-verify + cargo workspaces publish --yes --no-verify --publish-as-is + - name: Push commit + run: | + git push origin ${{ github.event.repository.default_branch }} From 2e40341e6d1a0f76568defb39e334e8f883a3a12 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 21 Jul 2024 20:23:39 -0300 Subject: [PATCH 104/159] Use versions over path MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- mavlink/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 280393172f..fe9364a1b7 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -19,7 +19,7 @@ edition = "2018" rust-version = "1.65.0" [build-dependencies] -mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false } +mavlink-bindgen = { version = "0.13.1", default-features = false } [[example]] name = "mavlink-dump" @@ -27,7 +27,7 @@ path = "examples/mavlink-dump/src/main.rs" required-features = ["ardupilotmega"] [dependencies] -mavlink-core = { path = "../mavlink-core", default-features = false } +mavlink-core = { version = "0.13.1", default-features = false } num-traits = { workspace = true, default-features = false } num-derive = { workspace = true } bitflags = { workspace = true } From 0b399396f840eca71ee0d63da11a68d0bf5f280c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Patrick=20Jos=C3=A9=20Pereira?= Date: Sun, 21 Jul 2024 20:47:11 -0300 Subject: [PATCH 105/159] Fix and add missing links to README MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Patrick José Pereira --- mavlink-bindgen/Cargo.toml | 1 + mavlink/Cargo.toml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index 1d8cf1ffd6..0a5ee88567 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -4,6 +4,7 @@ version = "0.13.1" edition = "2021" license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." +readme = "../README.md" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index fe9364a1b7..4fd09da02d 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -12,7 +12,7 @@ authors = [ ] build = "build/main.rs" description = "Implements the MAVLink data interchange format for UAVs." -readme = "README.md" +readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition = "2018" From ab925ad8469a1f5668166e0dd23d99883cbc86b7 Mon Sep 17 00:00:00 2001 From: Nazar Serhiichuk <43041209+G1gg1L3s@users.noreply.github.com> Date: Mon, 22 Jul 2024 02:56:52 +0300 Subject: [PATCH 106/159] Improve final binary size --- mavlink-bindgen/src/parser.rs | 19 +++++++++++++++++++ mavlink-core/src/bytes.rs | 16 ++++++++++++++++ mavlink-core/src/bytes_mut.rs | 13 +++++++++++++ 3 files changed, 48 insertions(+) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 1a6eaba7e3..1ed238b815 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -502,8 +502,27 @@ impl MavMessage { fn emit_serialize_vars(&self) -> TokenStream { let ser_vars = self.fields.iter().map(|f| f.rust_writer()); + quote! { let mut __tmp = BytesMut::new(bytes); + + // TODO: these lints are produced on a couple of cubepilot messages + // because they are generated as empty structs with no fields and + // therefore Self::ENCODED_LEN is 0. This itself is a bug because + // cubepilot.xml has unclosed tags in fields, which the parser has + // bad time handling. It should probably be fixed in both the parser + // and mavlink message definitions. However, until it's done, let's + // skip the lints. + #[allow(clippy::absurd_extreme_comparisons)] + #[allow(unused_comparisons)] + if __tmp.remaining() < Self::ENCODED_LEN { + panic!( + "buffer is too small (need {} bytes, but got {})", + Self::ENCODED_LEN, + __tmp.remaining(), + ) + } + #(#ser_vars)* if matches!(version, MavlinkVersion::V2) { let len = __tmp.len(); diff --git a/mavlink-core/src/bytes.rs b/mavlink-core/src/bytes.rs index 56597f7f59..712ca36623 100644 --- a/mavlink-core/src/bytes.rs +++ b/mavlink-core/src/bytes.rs @@ -13,10 +13,12 @@ impl<'a> Bytes<'a> { self.data.len() - self.pos } + #[inline] pub fn remaining_bytes(&self) -> &'a [u8] { &self.data[self.pos..] } + #[inline] fn check_remaining(&self, count: usize) { assert!( self.remaining() >= count, @@ -26,6 +28,7 @@ impl<'a> Bytes<'a> { ); } + #[inline] pub fn get_bytes(&mut self, count: usize) -> &[u8] { self.check_remaining(count); @@ -34,6 +37,7 @@ impl<'a> Bytes<'a> { bytes } + #[inline] pub fn get_array(&mut self) -> [u8; SIZE] { let bytes = self.get_bytes(SIZE); let mut arr = [0u8; SIZE]; @@ -45,6 +49,7 @@ impl<'a> Bytes<'a> { arr } + #[inline] pub fn get_u8(&mut self) -> u8 { self.check_remaining(1); @@ -53,6 +58,7 @@ impl<'a> Bytes<'a> { val } + #[inline] pub fn get_i8(&mut self) -> i8 { self.check_remaining(1); @@ -61,14 +67,17 @@ impl<'a> Bytes<'a> { val } + #[inline] pub fn get_u16_le(&mut self) -> u16 { u16::from_le_bytes(self.get_array()) } + #[inline] pub fn get_i16_le(&mut self) -> i16 { i16::from_le_bytes(self.get_array()) } + #[inline] pub fn get_u24_le(&mut self) -> u32 { const SIZE: usize = 3; self.check_remaining(SIZE); @@ -81,6 +90,7 @@ impl<'a> Bytes<'a> { u32::from_le_bytes(val) } + #[inline] pub fn get_i24_le(&mut self) -> i32 { const SIZE: usize = 3; self.check_remaining(SIZE); @@ -93,26 +103,32 @@ impl<'a> Bytes<'a> { i32::from_le_bytes(val) } + #[inline] pub fn get_u32_le(&mut self) -> u32 { u32::from_le_bytes(self.get_array()) } + #[inline] pub fn get_i32_le(&mut self) -> i32 { i32::from_le_bytes(self.get_array()) } + #[inline] pub fn get_u64_le(&mut self) -> u64 { u64::from_le_bytes(self.get_array()) } + #[inline] pub fn get_i64_le(&mut self) -> i64 { i64::from_le_bytes(self.get_array()) } + #[inline] pub fn get_f32_le(&mut self) -> f32 { f32::from_le_bytes(self.get_array()) } + #[inline] pub fn get_f64_le(&mut self) -> f64 { f64::from_le_bytes(self.get_array()) } diff --git a/mavlink-core/src/bytes_mut.rs b/mavlink-core/src/bytes_mut.rs index aa01c351a6..1fc22b9cf6 100644 --- a/mavlink-core/src/bytes_mut.rs +++ b/mavlink-core/src/bytes_mut.rs @@ -33,6 +33,7 @@ impl<'a> BytesMut<'a> { ); } + #[inline] pub fn put_slice(&mut self, src: &[u8]) { self.check_remaining(src.len()); @@ -42,6 +43,7 @@ impl<'a> BytesMut<'a> { self.len += src.len(); } + #[inline] pub fn put_u8(&mut self, val: u8) { self.check_remaining(1); @@ -49,6 +51,7 @@ impl<'a> BytesMut<'a> { self.len += 1; } + #[inline] pub fn put_i8(&mut self, val: i8) { self.check_remaining(1); @@ -56,6 +59,7 @@ impl<'a> BytesMut<'a> { self.len += 1; } + #[inline] pub fn put_u16_le(&mut self, val: u16) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -65,6 +69,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_i16_le(&mut self, val: i16) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -74,6 +79,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_u24_le(&mut self, val: u32) { const SIZE: usize = 3; const MAX: u32 = 2u32.pow(24) - 1; @@ -91,6 +97,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_i24_le(&mut self, val: i32) { const SIZE: usize = 3; const MIN: i32 = 2i32.pow(23); @@ -116,6 +123,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_u32_le(&mut self, val: u32) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -125,6 +133,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_i32_le(&mut self, val: i32) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -134,6 +143,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_u64_le(&mut self, val: u64) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -143,6 +153,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_i64_le(&mut self, val: i64) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -152,6 +163,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_f32_le(&mut self, val: f32) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); @@ -161,6 +173,7 @@ impl<'a> BytesMut<'a> { self.len += SIZE; } + #[inline] pub fn put_f64_le(&mut self, val: f64) { const SIZE: usize = core::mem::size_of::(); self.check_remaining(SIZE); From 63b6af9fe59eb5cdea6aa385d0c3f4f3d2f2303b Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 31 Jul 2024 09:40:30 +0200 Subject: [PATCH 107/159] fix: Enum entries without value are generated as isize --- mavlink-bindgen/src/parser.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 1ed238b815..e6ee66f3ab 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -308,7 +308,7 @@ impl MavEnum { } fn emit_defs(&self) -> Vec { - let mut cnt = 0isize; + let mut cnt = 0u32; self.entries .iter() .map(|enum_entry| { @@ -330,7 +330,7 @@ impl MavEnum { value = quote!(#cnt); } else { let tmp_value = enum_entry.value.unwrap(); - cnt = cnt.max(tmp_value as isize); + cnt = cnt.max(tmp_value as u32); let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); value = quote!(#tmp); }; From 9e9ef0929c07d20d2681dac6ecced58335bf1895 Mon Sep 17 00:00:00 2001 From: pv42 Date: Tue, 6 Aug 2024 18:33:57 +0200 Subject: [PATCH 108/159] Feat: add cli argument documentation --- mavlink-bindgen/src/cli.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mavlink-bindgen/src/cli.rs b/mavlink-bindgen/src/cli.rs index 30fee8a843..5ff37b26f1 100644 --- a/mavlink-bindgen/src/cli.rs +++ b/mavlink-bindgen/src/cli.rs @@ -4,11 +4,16 @@ use clap::Parser; use mavlink_bindgen::{emit_cargo_build_messages, format_generated_code, generate, BindGenError}; #[derive(Parser)] +/// Generate Rust bindings from MAVLink message dialect XML files. struct Cli { + /// Path to the directory containing the MAVLink dialect definitions. definitions_dir: PathBuf, + /// Path to the directory where the code is generated into, must already exist. destination_dir: PathBuf, + /// format code generated code #[arg(long)] format_generated_code: bool, + /// prints cargo build messages indicating when the code has to be rebuild #[arg(long)] emit_cargo_build_messages: bool, } From d788b66f3637028513a40ef69c0a7fc20fe9af94 Mon Sep 17 00:00:00 2001 From: pv42 Date: Tue, 6 Aug 2024 18:34:27 +0200 Subject: [PATCH 109/159] Feat: add mavlink-bindgen readme --- mavlink-bindgen/README.md | 94 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 94 insertions(+) create mode 100644 mavlink-bindgen/README.md diff --git a/mavlink-bindgen/README.md b/mavlink-bindgen/README.md new file mode 100644 index 0000000000..7e678a70cb --- /dev/null +++ b/mavlink-bindgen/README.md @@ -0,0 +1,94 @@ +# rust-mavlink + +[![Build status](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml/badge.svg)](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml) + +[![Crate info](https://img.shields.io/crates/v/mavlink-bindgen.svg)](https://crates.io/crates/mavlink-bindgen) +[![Documentation](https://docs.rs/mavlink-bindgen/badge.svg)](https://docs.rs/mavlink-bindgen) + +Library and CLI for code generator of the Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol. + +`mavlink-bindgen` can be used to create MAVLink bindings for Rust. This is used from `build.rs` in the [mavlink](https://crates.io/crates/mavlink) crate to create bindings from the standard MAVLink dialects in https://github.com/mavlink/mavlink. + +## Usage + +`mavlink-bindgen` can be used as a code generator from `build.rs` as done is the `mavlink` crate for a custom MAVLink dialect or as a CLI tool to generate rust binding from XML dialect definitions. The generated code will depend on the [mavlink-core](https://crates.io/crates/mavlink-core) crate in both use cases. + +### CLI + +Build using cargo with `cli` feature enabled: + +```shell +cargo build --features cli +``` + +Alternatively you can build and install `mavlink-bindgen` to you locally installed crates: + +```shell +cargo install mavlink-bindgen --features cli +``` + +Generate code using the resulting binary: + +```shell +mavlink-bindgen --format-generated-code message_definitions mavlink_dialects +``` + +The full options are shown below. + +```shell +Usage: mavlink-bindgen [OPTIONS] + +Arguments: + Path to the directory containing the MAVLink dialect definitions + Path to the directory where the code is generated into + +Options: + --format-generated-code format code generated code + --emit-cargo-build-messages prints cargo build message indicating when the code has to be rebuild + -h, --help Print help +``` + +### Library as build dependency + +Add to your Cargo.toml: + +```toml +mavlink-bindgen = "0.13.1" +``` + +Add a `build/main.rs` or `build.rs` to your project if it does not already exist. Then add the following to the `main` function to generate the code: + +```rs +let out_dir = env::var("OUT_DIR").unwrap(); +let result = match mavlink_bindgen::generate(definitions_dir, out_dir) { + Ok(r) => r, + Err(e) => { + eprintln!("{e}"); + return ExitCode::FAILURE; + } +}; +``` + +If the generated code should be formated use + +```rs + mavlink_bindgen::format_generated_code(&result); +``` + +To tell cargo when to regenerate code from the definitions use: + +```rs + mavlink_bindgen::emit_cargo_build_messages(&result); +``` + +Finally include the generated code into the `lib.rs` or `main.rs` : + +```rs +#![cfg_attr(not(feature = "std"), no_std)] +// include generate definitions +include!(concat!(env!("OUT_DIR"), "/mod.rs")); + +pub use mavlink_core::*; +``` + +This approach is used by the mavlink crate see its build script for an example. From 69703a7892f773a197d20a3ea3fec365e1f83c59 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 14:08:36 +0200 Subject: [PATCH 110/159] Fix: CouldNotReadDefinitionFile doc is copy pasted --- mavlink-bindgen/src/error.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs index 957ec5506d..fe32023389 100644 --- a/mavlink-bindgen/src/error.rs +++ b/mavlink-bindgen/src/error.rs @@ -8,7 +8,7 @@ pub enum BindGenError { source: std::io::Error, path: std::path::PathBuf, }, - /// Represents a failure to read the MAVLink definitions directory. + /// Represents a failure to read a MAVLink definition file. #[error("Could not read definition file {path}: {source}")] CouldNotReadDefinitionFile { source: std::io::Error, From 47058869f0030310c70031573f5a50809c717f3d Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 14:22:49 +0200 Subject: [PATCH 111/159] Feat: add doc to public functions in mavlink-bindgen's lib.rs --- mavlink-bindgen/src/lib.rs | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index 3448fe88da..22f1efa2ce 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -23,6 +23,9 @@ pub struct GeneratedBindings { pub mod_rs: PathBuf, } +/// Generate Rust MAVLink dialect binding for dialects present in `definitions_dir` into `destination_dir`. +/// +/// If successful returns paths of generated bindings linked to their dialect definitions files. pub fn generate, P2: AsRef>( definitions_dir: P1, destination_dir: P2, @@ -99,6 +102,7 @@ fn _generate( } } +/// Formats generated code using `rustfmt`. pub fn format_generated_code(result: &GeneratedBindings) { if let Err(error) = Command::new("rustfmt") .args( @@ -114,6 +118,7 @@ pub fn format_generated_code(result: &GeneratedBindings) { } } +/// Prints definitions for cargo that describe which files the generated code depends on, indicating when it has to be regenerated. pub fn emit_cargo_build_messages(result: &GeneratedBindings) { for binding in &result.bindings { // Re-run build if definition file changes From d16896264308d96a34632be46a7bcfb39c43091a Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 14:25:42 +0200 Subject: [PATCH 112/159] Fix: improve mavlink-bindgen readme --- mavlink-bindgen/README.md | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/mavlink-bindgen/README.md b/mavlink-bindgen/README.md index 7e678a70cb..c813b5fc3a 100644 --- a/mavlink-bindgen/README.md +++ b/mavlink-bindgen/README.md @@ -1,46 +1,46 @@ -# rust-mavlink +# mavlink-bindgen [![Build status](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml/badge.svg)](https://github.com/mavlink/rust-mavlink/actions/workflows/test.yml) - [![Crate info](https://img.shields.io/crates/v/mavlink-bindgen.svg)](https://crates.io/crates/mavlink-bindgen) [![Documentation](https://docs.rs/mavlink-bindgen/badge.svg)](https://docs.rs/mavlink-bindgen) -Library and CLI for code generator of the Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol. +Library and CLI for generating code for the Rust implementation of the [MAVLink](https://mavlink.io/en) UAV messaging protocol. -`mavlink-bindgen` can be used to create MAVLink bindings for Rust. This is used from `build.rs` in the [mavlink](https://crates.io/crates/mavlink) crate to create bindings from the standard MAVLink dialects in https://github.com/mavlink/mavlink. +`mavlink-bindgen` can be used to create MAVLink bindings for Rust. This is used from `build.rs` in the [mavlink](https://crates.io/crates/mavlink) crate to create bindings from the standard MAVLink dialects in . ## Usage -`mavlink-bindgen` can be used as a code generator from `build.rs` as done is the `mavlink` crate for a custom MAVLink dialect or as a CLI tool to generate rust binding from XML dialect definitions. The generated code will depend on the [mavlink-core](https://crates.io/crates/mavlink-core) crate in both use cases. +`mavlink-bindgen` can be used as a code generator from `build.rs` as done is the `mavlink` crate for a custom MAVLink dialect or as a CLI tool to generate rust binding from XML dialect definitions. The generated code will depend on the [mavlink-core](https://crates.io/crates/mavlink-core) crate in both use cases. Each dialect generated will be locked behind a feature flag of the same name, that must be enabled when using the generated code. ### CLI -Build using cargo with `cli` feature enabled: +Build the binary using cargo with `cli` feature enabled: ```shell +cd mavlink-bindgen cargo build --features cli ``` -Alternatively you can build and install `mavlink-bindgen` to you locally installed crates: +Alternatively you can build and install `mavlink-bindgen` to your locally installed crates: ```shell cargo install mavlink-bindgen --features cli ``` -Generate code using the resulting binary: +To generate code using the resulting binary: ```shell mavlink-bindgen --format-generated-code message_definitions mavlink_dialects ``` -The full options are shown below. +The full command line options are shown below. ```shell Usage: mavlink-bindgen [OPTIONS] Arguments: Path to the directory containing the MAVLink dialect definitions - Path to the directory where the code is generated into + Path to the directory where the code is generated into, must already exist Options: --format-generated-code format code generated code @@ -48,6 +48,8 @@ Options: -h, --help Print help ``` +The output dir will contain a `mod.rs` file with each dialect in its own file locked behind a feature flag. + ### Library as build dependency Add to your Cargo.toml: @@ -91,4 +93,6 @@ include!(concat!(env!("OUT_DIR"), "/mod.rs")); pub use mavlink_core::*; ``` -This approach is used by the mavlink crate see its build script for an example. +Since each dialect is locked behind a feature flag these need to be enabled for the dialects to become available when using the generated code. + +This approach is used by the `mavlink` crate see its build script for an example. From a5edd295a491e43d857dbdc8d2371cf9321c6738 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 14:39:29 +0200 Subject: [PATCH 113/159] Fix: set cargo toml link to readme in mavlink-bindgen, add missing repo link to mavlink-bindgen --- mavlink-bindgen/Cargo.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index 0a5ee88567..32403c9a2f 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -4,7 +4,8 @@ version = "0.13.1" edition = "2021" license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." -readme = "../README.md" +readme = "README.md" +repository = "https://github.com/mavlink/rust-mavlink" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html From c93fd578d7bca76a0644b49b22622e9774c7ab15 Mon Sep 17 00:00:00 2001 From: pv42 Date: Thu, 8 Aug 2024 13:54:03 +0200 Subject: [PATCH 114/159] fix: incompatibility flags of v2 messages are checked --- mavlink-core/src/lib.rs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index ef18bfa994..eac97934b7 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -490,6 +490,7 @@ pub async fn read_v1_msg_async( } const MAVLINK_IFLAG_SIGNED: u8 = 0x01; +const MAVLINK_SUPPORTED_IFLAGS: u8 = MAVLINK_IFLAG_SIGNED; #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: `` @@ -677,6 +678,12 @@ pub fn read_v2_raw_message( let header = &reader.peek_exact(MAVLinkV2MessageRaw::HEADER_SIZE)? [..MAVLinkV2MessageRaw::HEADER_SIZE]; message.mut_header().copy_from_slice(header); + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + let packet_length = message.raw_bytes().len() - 1; let payload_and_checksum_and_sign = &reader.peek_exact(packet_length)?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length]; @@ -720,6 +727,12 @@ pub async fn read_v2_raw_message_async( .read_exact(message.mut_header()) .await .map_err(|_| error::MessageReadError::Io)?; + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + reader .read_exact(message.mut_payload_and_checksum_and_sign()) .await From c10d36e9a4fff04588d87f2435a6cd515393fbfd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Thu, 22 Aug 2024 11:24:36 -0300 Subject: [PATCH 115/159] mavlink-core: src: read shouldn't be mut --- mavlink-core/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index eac97934b7..3be0db377a 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -511,7 +511,7 @@ impl MAVLinkV2MessageRaw { } #[inline] - pub fn header(&mut self) -> &[u8] { + pub fn header(&self) -> &[u8] { &self.0[1..=Self::HEADER_SIZE] } From 5a1cd30fd9f9e9f002ddf7968be60f0be229d4b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Thu, 22 Aug 2024 11:27:27 -0300 Subject: [PATCH 116/159] cargo: Add tokio-1 feature for tokio-based async-io --- mavlink-core/Cargo.toml | 2 ++ mavlink/Cargo.toml | 1 + 2 files changed, 3 insertions(+) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index e4df6bbab6..db3504e1cc 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -26,6 +26,7 @@ embedded-io-async = { version = "0.6.1", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } +tokio = { version = "1.0", default-features = false, features = ["io-util"], optional = true } [features] "std" = ["byteorder/std"] @@ -38,4 +39,5 @@ serial = { version = "0.4", optional = true } "embedded" = ["dep:embedded-io", "dep:embedded-io-async"] "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] +"tokio-1" = ["dep:tokio"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 4fd09da02d..714d79cd52 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -99,6 +99,7 @@ serde_arrays = { version = "0.1.0", optional = true } "embedded" = ["mavlink-core/embedded"] "embedded-hal-02" = ["mavlink-core/embedded-hal-02"] "serde" = ["mavlink-core/serde", "dep:serde", "dep:serde_arrays"] +"tokio-1" = ["mavlink-core/tokio-1"] default = ["std", "tcp", "udp", "direct-serial", "serde", "ardupilotmega"] # build with all features on docs.rs so that users viewing documentation From 12b1e2b8afa63eb78dfea45d9878287043b8d8ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Thu, 22 Aug 2024 11:28:04 -0300 Subject: [PATCH 117/159] mavlink-core: src: Add tokio-based async read/write --- mavlink-core/src/lib.rs | 106 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 3be0db377a..342d7297b9 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -698,6 +698,41 @@ pub fn read_v2_raw_message( } } +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "tokio-1")] +pub async fn read_v2_raw_message_async( + reader: &mut R, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8().await? == MAV_STX_V2 { + break; + } + } + + let mut message = MAVLinkV2MessageRaw::new(); + + message.0[0] = MAV_STX_V2; + let header_len = reader.read_exact(message.mut_header()).await?; + assert_eq!(header_len, MAVLinkV2MessageRaw::HEADER_SIZE); + + if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 { + // if there are incompatibility flags set that we do not know discard the message + continue; + } + + reader + .read_exact(message.mut_payload_and_checksum_and_sign()) + .await?; + + if message.has_valid_crc::() { + return Ok(message); + } + } +} + /// Async read a raw buffer with the mavlink message /// V2 maximum size is 280 bytes: `` /// @@ -766,6 +801,27 @@ pub fn read_v2_msg( .map_err(|err| err.into()) } +/// Async read a MAVLink v2 message from a Read stream. +#[cfg(feature = "tokio-1")] +pub async fn read_v2_msg_async( + read: &mut R, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_async::(read).await?; + + 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()) +} + /// Async read a MAVLink v2 message from a Read stream. /// /// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. @@ -807,6 +863,20 @@ pub fn write_versioned_msg( } } +/// Async write a message using the given mavlink version +#[cfg(feature = "tokio-1")] +pub async fn write_versioned_msg_async( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_async(w, header, data).await, + MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await, + } +} + /// Async write a message using the given mavlink version /// /// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. @@ -841,6 +911,24 @@ pub fn write_v2_msg( Ok(len) } +/// Async write a MAVLink v2 message to a Write stream. +#[cfg(feature = "tokio-1")] +pub async fn write_v2_msg_async( + w: &mut W, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]).await?; + + Ok(len) +} + /// Async write a MAVLink v2 message to a Write stream. /// /// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. @@ -881,6 +969,24 @@ pub fn write_v1_msg( Ok(len) } +/// Async write a MAVLink v1 message to a Write stream. +#[cfg(feature = "tokio-1")] +pub async fn write_v1_msg_async( + w: &mut W, + header: MavHeader, + data: &M, +) -> Result { + let mut message_raw = MAVLinkV1MessageRaw::new(); + message_raw.serialize_message(header, data); + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV1MessageRaw::HEADER_SIZE + payload_length + 2; + + w.write_all(&message_raw.0[..len]).await?; + + Ok(len) +} + /// Write a MAVLink v1 message to a Write stream. /// /// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. From 311cd65d429d8dfe8400b41d76bd46b4a70be3fe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Thu, 22 Aug 2024 11:40:46 -0300 Subject: [PATCH 118/159] Revert "Use versions over path" This reverts commit 2e40341e6d1a0f76568defb39e334e8f883a3a12. --- mavlink/Cargo.toml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 714d79cd52..476c80b0b8 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -19,7 +19,7 @@ edition = "2018" rust-version = "1.65.0" [build-dependencies] -mavlink-bindgen = { version = "0.13.1", default-features = false } +mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false } [[example]] name = "mavlink-dump" @@ -27,7 +27,7 @@ path = "examples/mavlink-dump/src/main.rs" required-features = ["ardupilotmega"] [dependencies] -mavlink-core = { version = "0.13.1", 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 } From 4faca0ec695c5598ed3421466f8c57139d77d5de Mon Sep 17 00:00:00 2001 From: "Alex M. Smith" Date: Wed, 7 Aug 2024 18:05:01 +0200 Subject: [PATCH 119/159] fix: add MavFrame deser & ser warning, fixes #250 --- mavlink-core/src/lib.rs | 39 ++++++++++++++++++++++++++++---- mavlink/tests/mav_frame_tests.rs | 7 +++++- 2 files changed, 41 insertions(+), 5 deletions(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 342d7297b9..5583793a42 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -127,7 +127,7 @@ impl Default for MavHeader { /// Encapsulation of the Mavlink message and the header, /// important to preserve information about the sender system -/// and component id +/// and component id. #[derive(Debug, Clone)] #[cfg_attr(feature = "serde", derive(Serialize))] pub struct MavFrame { @@ -146,9 +146,32 @@ impl MavFrame { // } /// Serialize MavFrame into a vector, so it can be sent over a socket, for example. + /// The resulting buffer will start with the sequence field of the Mavlink frame + /// and will not include the initial packet marker, length field, and flags. pub fn ser(&self, buf: &mut [u8]) -> usize { let mut buf = bytes_mut::BytesMut::new(buf); + // serialize message + let mut payload_buf = [0u8; 255]; + let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); + + // Currently expects a buffer with the sequence field at the start. + // If this is updated to include the initial packet marker, length field, and flags, + // uncomment. + // + // match self.protocol_version { + // MavlinkVersion::V2 => { + // buf.put_u8(MAV_STX_V2); + // buf.put_u8(payload_len as u8); + // but.put_u8(0); // incompatibility flags + // buf.put_u8(0); // compatibility flags + // } + // MavlinkVersion::V1 => { + // buf.put_u8(MAV_STX); + // buf.put_u8(payload_len as u8); + // } + // } + // serialize header buf.put_u8(self.header.sequence); buf.put_u8(self.header.system_id); @@ -164,18 +187,26 @@ impl MavFrame { buf.put_u8(self.msg.message_id() as u8); //TODO check } } - // serialize message - let mut payload_buf = [0u8; 255]; - let payload_len = self.msg.ser(self.protocol_version, &mut payload_buf); buf.put_slice(&payload_buf[..payload_len]); buf.len() } /// Deserialize MavFrame from a slice that has been received from, for example, a socket. + /// The input buffer should start with the sequence field of the Mavlink frame. The + /// initial packet marker, length field, and flag fields should be excluded. pub fn deser(version: MavlinkVersion, input: &[u8]) -> Result { let mut buf = Bytes::new(input); + // Currently expects a buffer with the sequence field at the start. + // If this is updated to include the initial packet marker, length field, and flags, + // uncomment. + // + // match version { + // MavlinkVersion::V2 => buf.get_u32_le(), + // MavlinkVersion::V1 => buf.get_u16_le().into(), + // }; + let sequence = buf.get_u8(); let system_id = buf.get_u8(); let component_id = buf.get_u8(); diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs index d1fac6193f..1c225efd09 100644 --- a/mavlink/tests/mav_frame_tests.rs +++ b/mavlink/tests/mav_frame_tests.rs @@ -5,8 +5,13 @@ mod mav_frame_tests { use mavlink::MavFrame; use mavlink::MavHeader; - // NOTE: No header + // NOTE: No STX, length, or flag fields in the header pub const HEARTBEAT_V2: &[u8] = &[ + // Currently [`MavFrame::deser`] and [`MavFrame::ser`] does not account for the first four fields. + // 0xfd, // STX V2 + // 0x09, // len + // 0x00, // incompat_flags + // 0x00, // compat_flags crate::test_shared::COMMON_MSG_HEADER.sequence, crate::test_shared::COMMON_MSG_HEADER.system_id, crate::test_shared::COMMON_MSG_HEADER.component_id, From 06f1eef42a36accc3c7a4e8c0bbda25e52a67724 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Thu, 22 Aug 2024 14:18:03 -0300 Subject: [PATCH 120/159] mavlink-core: src: Fix embedded v2 read header --- mavlink-core/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 5583793a42..29e0d20345 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -788,7 +788,7 @@ pub async fn read_v2_raw_message_async( let mut message = MAVLinkV2MessageRaw::new(); - message.0[0] = MAV_STX; + message.0[0] = MAV_STX_V2; reader .read_exact(message.mut_header()) .await From 8fb58753f96c456cc84fca086df3adfa3e4779af Mon Sep 17 00:00:00 2001 From: pv42 Date: Thu, 22 Aug 2024 22:24:20 +0200 Subject: [PATCH 121/159] Emit extensions --- mavlink-bindgen/Cargo.toml | 4 +++- mavlink-core/Cargo.toml | 2 +- mavlink-core/src/utils.rs | 2 +- mavlink/Cargo.toml | 6 +++--- mavlink/src/lib.rs | 4 ++++ 5 files changed, 12 insertions(+), 6 deletions(-) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index 32403c9a2f..4a6e365f7d 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-bindgen" -version = "0.13.1" +version = "0.13.2" edition = "2021" license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." @@ -28,3 +28,5 @@ anstyle-parse = { version = "=0.2.1", optional=true } [features] cli = ["dep:clap", "dep:clap_lex", "dep:clap_builder", "dep:anstyle", "dep:anstyle-query", "dep:anstyle-parse"] +emit-extensions = [] +emit-description = [] diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index db3504e1cc..8883a15ded 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-core" -version = "0.13.1" +version = "0.13.2" authors = [ "Todd Stellanova", "Michal Podhradsky", diff --git a/mavlink-core/src/utils.rs b/mavlink-core/src/utils.rs index c0516c154e..efefcbadb8 100644 --- a/mavlink-core/src/utils.rs +++ b/mavlink-core/src/utils.rs @@ -22,7 +22,7 @@ pub fn remove_trailing_zeroes(data: &[u8]) -> usize { /// `MavType`s. This is only needed because rust doesn't currently implement `Default` for arrays /// of all sizes. In particular this trait is only ever used when the "serde" feature is enabled. /// For more information, check out [this issue](https://users.rust-lang.org/t/issue-for-derives-for-arrays-greater-than-size-32/59055/3). -pub(crate) trait RustDefault: Copy { +pub trait RustDefault: Copy { fn rust_default() -> Self; } diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 476c80b0b8..597a6c756b 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink" -version = "0.13.1" +version = "0.13.2" authors = [ "Todd Stellanova", "Michal Podhradsky", @@ -87,8 +87,8 @@ serde_arrays = { version = "0.1.0", optional = true } ] "format-generated-code" = [] -"emit-description" = [] -"emit-extensions" = [] +"emit-description" = ["mavlink-bindgen/emit-description"] +"emit-extensions" = ["mavlink-bindgen/emit-extensions"] "std" = ["mavlink-core/std"] "udp" = ["mavlink-core/udp"] "tcp" = ["mavlink-core/tcp"] diff --git a/mavlink/src/lib.rs b/mavlink/src/lib.rs index a1b71eaa5a..3690dd954c 100644 --- a/mavlink/src/lib.rs +++ b/mavlink/src/lib.rs @@ -3,3 +3,7 @@ include!(concat!(env!("OUT_DIR"), "/mod.rs")); pub use mavlink_core::*; + +#[cfg(feature = "emit-extensions")] +#[allow(unused_imports)] +pub(crate) use mavlink_core::utils::RustDefault; From 2ab67858a184ca74dca6d5e788c5cf0f910f4c18 Mon Sep 17 00:00:00 2001 From: danieleades <33452915+danieleades@users.noreply.github.com> Date: Sun, 25 Aug 2024 16:23:09 +0100 Subject: [PATCH 122/159] move to 2021 edition (#262) --- Cargo.toml | 3 +++ mavlink-bindgen/Cargo.toml | 2 +- mavlink-bindgen/src/parser.rs | 3 +-- mavlink-core/Cargo.toml | 2 +- mavlink-core/src/bytes.rs | 3 +-- mavlink-core/src/bytes_mut.rs | 15 ++++----------- mavlink/Cargo.toml | 2 +- 7 files changed, 12 insertions(+), 18 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 65e9ef8b1d..064abc84b4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -8,3 +8,6 @@ num-traits = { version = "0.2", default-features = false } num-derive = "0.3.2" bitflags = "1.2.1" byteorder = { version = "1.3.4", default-features = false } + +[workspace.package] +edition = "2021" diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index 4a6e365f7d..f34d44ab1d 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "mavlink-bindgen" version = "0.13.2" -edition = "2021" +edition.workspace = true license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." readme = "README.md" diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index e6ee66f3ab..2b45e5d17b 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1107,9 +1107,8 @@ pub fn parse_profile( assert!( is_valid_parent(stack.last().copied(), id), - "not valid parent {:?} of {:?}", + "not valid parent {:?} of {id:?}", stack.last(), - id ); match id { diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 8883a15ded..246c657537 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -13,7 +13,7 @@ description = "Implements the MAVLink data interchange format for UAVs." readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" -edition = "2018" +edition.workspace = true rust-version = "1.65.0" [dependencies] diff --git a/mavlink-core/src/bytes.rs b/mavlink-core/src/bytes.rs index 712ca36623..e1b11ff433 100644 --- a/mavlink-core/src/bytes.rs +++ b/mavlink-core/src/bytes.rs @@ -22,9 +22,8 @@ impl<'a> Bytes<'a> { fn check_remaining(&self, count: usize) { assert!( self.remaining() >= count, - "read buffer exhausted; remaining {} bytes, try read {} bytes", + "read buffer exhausted; remaining {} bytes, try read {count} bytes", self.remaining(), - count ); } diff --git a/mavlink-core/src/bytes_mut.rs b/mavlink-core/src/bytes_mut.rs index 1fc22b9cf6..df607c83c8 100644 --- a/mavlink-core/src/bytes_mut.rs +++ b/mavlink-core/src/bytes_mut.rs @@ -27,9 +27,8 @@ impl<'a> BytesMut<'a> { fn check_remaining(&self, count: usize) { assert!( self.remaining() >= count, - "write buffer overflow; remaining {} bytes, try add {} bytes", + "write buffer overflow; remaining {} bytes, try add {count} bytes", self.remaining(), - count ); } @@ -87,9 +86,7 @@ impl<'a> BytesMut<'a> { assert!( val <= MAX, "Attempted to put value that is too large for 24 bits, \ - attempted to push: {}, max allowed: {}", - val, - MAX + attempted to push: {val}, max allowed: {MAX}", ); let src = val.to_le_bytes(); @@ -106,16 +103,12 @@ impl<'a> BytesMut<'a> { assert!( val <= MAX, "Attempted to put value that is too large for 24 bits, \ - attempted to push: {}, max allowed: {}", - val, - MAX + attempted to push: {val}, max allowed: {MAX}", ); assert!( val >= MIN, "Attempted to put value that is too negative for 24 bits, \ - attempted to push: {}, min allowed: {}", - val, - MIN + attempted to push: {val}, min allowed: {MIN}", ); let src = val.to_le_bytes(); diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 597a6c756b..46896b017c 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -15,7 +15,7 @@ description = "Implements the MAVLink data interchange format for UAVs." readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" -edition = "2018" +edition.workspace = true rust-version = "1.65.0" [build-dependencies] From 908e6547c0283028a4784d12368f112a5e036e86 Mon Sep 17 00:00:00 2001 From: danieleades <33452915+danieleades@users.noreply.github.com> Date: Sun, 25 Aug 2024 16:44:43 +0100 Subject: [PATCH 123/159] Minor refactoring --- mavlink-bindgen/src/lib.rs | 7 +- mavlink-bindgen/src/parser.rs | 12 +- mavlink-core/src/connection/direct_serial.rs | 16 +- mavlink-core/src/connection/file.rs | 6 +- mavlink-core/src/connection/mod.rs | 18 +-- mavlink-core/src/connection/tcp.rs | 6 +- mavlink-core/src/connection/udp.rs | 6 +- mavlink-core/src/lib.rs | 137 ++++++++---------- .../examples/embedded-async-read/src/main.rs | 9 +- 9 files changed, 89 insertions(+), 128 deletions(-) diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index 22f1efa2ce..23aae4b1d0 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -39,7 +39,7 @@ fn _generate( ) -> Result { let mut bindings = vec![]; - for entry_maybe in read_dir(&definitions_dir).map_err(|source| { + for entry_maybe in read_dir(definitions_dir).map_err(|source| { BindGenError::CouldNotReadDefinitionsDirectory { source, path: definitions_dir.to_path_buf(), @@ -55,8 +55,7 @@ fn _generate( let definition_file = PathBuf::from(entry.file_name()); let module_name = util::to_module_name(&definition_file); - let mut definition_rs = PathBuf::from(&module_name); - definition_rs.set_extension("rs"); + let definition_rs = PathBuf::from(&module_name).with_extension("rs"); let dest_path = destination_dir.join(definition_rs); let mut outf = BufWriter::new(File::create(&dest_path).map_err(|source| { @@ -67,7 +66,7 @@ fn _generate( })?); // generate code - parser::generate(&definitions_dir, &definition_file, &mut outf)?; + parser::generate(definitions_dir, &definition_file, &mut outf)?; bindings.push(GeneratedBinding { module_name, diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 2b45e5d17b..dd8e389ea1 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -7,7 +7,6 @@ use std::fs::File; use std::io::{BufReader, Write}; use std::path::{Path, PathBuf}; use std::str::FromStr; -use std::u32; use quick_xml::{events::Event, Reader}; @@ -330,7 +329,7 @@ impl MavEnum { value = quote!(#cnt); } else { let tmp_value = enum_entry.value.unwrap(); - cnt = cnt.max(tmp_value as u32); + cnt = cnt.max(tmp_value); let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); value = quote!(#tmp); }; @@ -774,10 +773,11 @@ impl MavField { } } -#[derive(Debug, PartialEq, Clone)] +#[derive(Debug, PartialEq, Clone, Default)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub enum MavType { UInt8MavlinkVersion, + #[default] UInt8, UInt16, UInt32, @@ -792,12 +792,6 @@ pub enum MavType { Array(Box, usize), } -impl Default for MavType { - fn default() -> Self { - Self::UInt8 - } -} - impl MavType { fn parse_type(s: &str) -> Option { use self::MavType::*; diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index 341d075ef2..a3a33a0fa6 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -1,3 +1,5 @@ +//! Serial MAVLINK connection + use crate::connection::MavConnection; use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; @@ -8,8 +10,6 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; -/// Serial MAVLINK connection - pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -19,15 +19,15 @@ pub fn open(settings: &str) -> io::Result { )); } - let baud_opt = settings_toks[1].parse::(); - if baud_opt.is_err() { + let Ok(baud) = settings_toks[1] + .parse::() + .map(serial::core::BaudRate::from_speed) + else { return Err(io::Error::new( io::ErrorKind::AddrNotAvailable, "Invalid baud rate", )); - } - - let baud = serial::core::BaudRate::from_speed(baud_opt.unwrap()); + }; let settings = serial::core::PortSettings { baud_rate: baud, @@ -91,7 +91,7 @@ impl MavConnection for SerialConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index d0fcdc9875..85444bb6ae 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -1,3 +1,5 @@ +//! File MAVLINK connection + use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; use crate::peek_reader::PeekReader; @@ -7,8 +9,6 @@ use std::fs::File; use std::io; use std::sync::Mutex; -/// File MAVLINK connection - pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; @@ -52,7 +52,7 @@ impl MavConnection for FileConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index 7aa4ab78ec..035f63eb28 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -24,7 +24,7 @@ pub trait MavConnection { fn send(&self, header: &MavHeader, data: &M) -> Result; fn set_protocol_version(&mut self, version: MavlinkVersion); - fn get_protocol_version(&self) -> MavlinkVersion; + fn protocol_version(&self) -> MavlinkVersion; /// Write whole frame fn send_frame(&self, frame: &MavFrame) -> Result { @@ -34,7 +34,7 @@ pub trait MavConnection { /// Read whole frame fn recv_frame(&self) -> Result, crate::error::MessageReadError> { let (header, msg) = self.recv()?; - let protocol_version = self.get_protocol_version(); + let protocol_version = self.protocol_version(); Ok(MavFrame { header, msg, @@ -107,14 +107,8 @@ pub fn connect(address: &str) -> io::Result pub(crate) fn get_socket_addr( address: T, ) -> Result { - let addr = match address.to_socket_addrs()?.next() { - Some(addr) => addr, - None => { - return Err(io::Error::new( - io::ErrorKind::Other, - "Host address lookup failed", - )); - } - }; - Ok(addr) + address.to_socket_addrs()?.next().ok_or(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )) } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 44d3778777..35b7f09084 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -1,3 +1,5 @@ +//! TCP MAVLink connection + use crate::connection::MavConnection; use crate::peek_reader::PeekReader; use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; @@ -10,8 +12,6 @@ use std::time::Duration; use super::get_socket_addr; -/// TCP MAVLink connection - pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -108,7 +108,7 @@ impl MavConnection for TcpConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index b353ea1ba1..3ec739d3f5 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -1,3 +1,5 @@ +//! UDP MAVLink connection + use std::collections::VecDeque; use crate::connection::MavConnection; @@ -11,8 +13,6 @@ use std::sync::Mutex; use super::get_socket_addr; -/// UDP MAVLink connection - pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -156,7 +156,7 @@ impl MavConnection for UdpConnection { self.protocol_version = version; } - fn get_protocol_version(&self) -> MavlinkVersion { + fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 29e0d20345..e6e7aec97a 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -221,14 +221,11 @@ impl MavFrame { MavlinkVersion::V1 => buf.get_u8().into(), }; - match M::parse(version, msg_id, buf.remaining_bytes()) { - Ok(msg) => Ok(Self { - header, - msg, - protocol_version: version, - }), - Err(err) => Err(err), - } + M::parse(version, msg_id, buf.remaining_bytes()).map(|msg| Self { + header, + msg, + protocol_version: version, + }) } /// Return the frame header @@ -474,22 +471,18 @@ pub fn read_v1_msg( ) -> Result<(MavHeader, M), error::MessageReadError> { 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_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) } /// Async read a MAVLink v1 message from a Read stream. @@ -502,22 +495,18 @@ pub async fn read_v1_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v1_raw_message_async::(r).await?; - 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_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) } const MAVLINK_IFLAG_SIGNED: u8 = 0x01; @@ -818,18 +807,14 @@ pub fn read_v2_msg( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message::(read)?; - 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()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) } /// Async read a MAVLink v2 message from a Read stream. @@ -839,18 +824,14 @@ pub async fn read_v2_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message_async::(read).await?; - 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()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse(MavlinkVersion::V2, message.message_id(), message.payload())?, + )) } /// Async read a MAVLink v2 message from a Read stream. @@ -863,22 +844,18 @@ pub async fn read_v2_msg_async( ) -> Result<(MavHeader, M), error::MessageReadError> { let message = read_v2_raw_message_async::(r).await?; - M::parse( - MavlinkVersion::V2, - 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_err(|err| err.into()) + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V2, + u32::from(message.message_id()), + message.payload(), + )?, + )) } /// Write a message using the given mavlink version diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index af76a2640a..7f53335de3 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -82,12 +82,9 @@ pub async fn rx_task(rx: usart::UartRx<'static, Async>) { .unwrap(); rprintln!("Read raw message: msg_id={}", raw.message_id()); - match raw.message_id() { - HEARTBEAT_DATA::ID => { - let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); - rprintln!("heartbeat: {:?}", heartbeat); - } - _ => {} + if raw.message_id() == HEARTBEAT_DATA::ID { + let heartbeat = HEARTBEAT_DATA::deser(MavlinkVersion::V2, raw.payload()).unwrap(); + rprintln!("heartbeat: {:?}", heartbeat); } } } From 9a657c048cf8f7f800c07e9242ec06e790b90905 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 23:50:56 +0200 Subject: [PATCH 124/159] feat: add signing feature with sha2 dependency --- mavlink-core/Cargo.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 246c657537..d131be3c11 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -27,6 +27,7 @@ serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } tokio = { version = "1.0", default-features = false, features = ["io-util"], optional = true } +sha2 = { version = "0.10", optional = true } [features] "std" = ["byteorder/std"] @@ -40,4 +41,5 @@ tokio = { version = "1.0", default-features = false, features = ["io-util"], opt "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] "tokio-1" = ["dep:tokio"] +"signing" = ["dep:sha2", "std"] default = ["std", "tcp", "udp", "direct-serial", "serde"] From ab82e274fd6a6ed7b4ec82fc3ed49d40d37b5351 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 7 Aug 2024 23:57:47 +0200 Subject: [PATCH 125/159] feat: Add incompat flag field to MavHeader --- mavlink-core/src/connection/direct_serial.rs | 1 + mavlink-core/src/connection/tcp.rs | 1 + mavlink-core/src/connection/udp.rs | 1 + mavlink-core/src/lib.rs | 5 ++++- mavlink/examples/embedded-async-read/src/main.rs | 1 + mavlink/examples/embedded/src/main.rs | 1 + mavlink/tests/mav_frame_tests.rs | 1 + mavlink/tests/test_shared/mod.rs | 1 + 8 files changed, 11 insertions(+), 1 deletion(-) diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index a3a33a0fa6..2e31bda56d 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -77,6 +77,7 @@ impl MavConnection for SerialConnection { let mut sequence = self.sequence.lock().unwrap(); let header = MavHeader { + incompat_flags: 0, sequence: *sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 35b7f09084..73d456aa3b 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -95,6 +95,7 @@ impl MavConnection for TcpConnection { let mut lock = self.writer.lock().unwrap(); let header = MavHeader { + incompat_flags: 0, sequence: lock.sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 3ec739d3f5..91bbfaef45 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -134,6 +134,7 @@ impl MavConnection for UdpConnection { let state = &mut *guard; let header = MavHeader { + incompat_flags: 0, sequence: state.sequence, system_id: header.system_id, component_id: header.component_id, diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index e6e7aec97a..fa6bea9c48 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -93,6 +93,7 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { +pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -118,6 +119,7 @@ pub const MAV_STX_V2: u8 = 0xFD; impl Default for MavHeader { fn default() -> Self { Self { + incompat_flags: 0, system_id: 255, component_id: 0, sequence: 0, @@ -211,6 +213,7 @@ impl MavFrame { let system_id = buf.get_u8(); let component_id = buf.get_u8(); let header = MavHeader { + incompat_flags: 0, system_id, component_id, sequence, @@ -639,7 +642,7 @@ impl MAVLinkV2MessageRaw { let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ payload_length as u8, - 0, //incompat_flags + header.incompat_flags, 0, //compat_flags header.sequence, header.system_id, diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index 7f53335de3..6b4e5c1dc1 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -39,6 +39,7 @@ async fn main(spawner: Spawner) { // Create our mavlink header and heartbeat message let header = mavlink::MavHeader { + incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs index 4a71ca8fcf..21b70396b1 100644 --- a/mavlink/examples/embedded/src/main.rs +++ b/mavlink/examples/embedded/src/main.rs @@ -82,6 +82,7 @@ fn main() -> ! { fn mavlink_header() -> mavlink::MavHeader { mavlink::MavHeader { + incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs index 1c225efd09..5472dbb8c1 100644 --- a/mavlink/tests/mav_frame_tests.rs +++ b/mavlink/tests/mav_frame_tests.rs @@ -96,6 +96,7 @@ mod mav_frame_tests { fn new(msg: MavMessage) -> MavFrame { MavFrame { header: MavHeader { + incompat_flags: 0, system_id: 1, component_id: 2, sequence: 84, diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 4cba46907a..332e9a1f02 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -1,6 +1,7 @@ #![allow(unused)] pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { + incompat_flags: 0, sequence: 239, system_id: 1, component_id: 2, From 6e15048c916c84bc674d4076038d08f38294b52e Mon Sep 17 00:00:00 2001 From: pv42 Date: Thu, 8 Aug 2024 00:30:36 +0200 Subject: [PATCH 126/159] feat: add MavConnection::setup_signing(), signing functions to MAVLinkV2MessageRaw add calculate_signature, checksum_bytes, signature_link_id[_mut], signature_timestamp[_bytes[_mut]], signature_value[_mut] to MAVLinkV2MessageRaw --- mavlink-core/src/connection/direct_serial.rs | 12 ++ mavlink-core/src/connection/file.rs | 12 ++ mavlink-core/src/connection/mod.rs | 9 ++ mavlink-core/src/connection/signing.rs | 118 +++++++++++++++++++ mavlink-core/src/connection/tcp.rs | 14 +++ mavlink-core/src/connection/udp.rs | 12 ++ mavlink-core/src/lib.rs | 88 +++++++++++++- 7 files changed, 264 insertions(+), 1 deletion(-) create mode 100644 mavlink-core/src/connection/signing.rs diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index 2e31bda56d..daea3a0b77 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -10,6 +10,9 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); if settings_toks.len() < 2 { @@ -45,6 +48,8 @@ pub fn open(settings: &str) -> io::Result { port: Mutex::new(PeekReader::new(port)), sequence: Mutex::new(0), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } @@ -52,6 +57,8 @@ pub struct SerialConnection { port: Mutex>, sequence: Mutex, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for SerialConnection { @@ -95,4 +102,9 @@ impl MavConnection for SerialConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index 85444bb6ae..8209392abc 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -9,18 +9,25 @@ use std::fs::File; use std::io; use std::sync::Mutex; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; Ok(FileConnection { file: Mutex::new(PeekReader::new(file)), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } pub struct FileConnection { file: Mutex>, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } impl MavConnection for FileConnection { @@ -55,4 +62,9 @@ impl MavConnection for FileConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index 035f63eb28..f4d6e6e332 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -11,6 +11,11 @@ mod udp; #[cfg(feature = "direct-serial")] mod direct_serial; +#[cfg(feature = "signing")] +pub(crate) mod signing; +#[cfg(feature = "signing")] +pub use signing::SigningConfig; + mod file; /// A MAVLink connection @@ -47,6 +52,10 @@ pub trait MavConnection { let header = MavHeader::default(); self.send(&header, data) } + + /// Setup secret key used for message signing, or disable message signing + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option); } /// Connect to a MAVLink node by address string. diff --git a/mavlink-core/src/connection/signing.rs b/mavlink-core/src/connection/signing.rs new file mode 100644 index 0000000000..fe43120e64 --- /dev/null +++ b/mavlink-core/src/connection/signing.rs @@ -0,0 +1,118 @@ +use crate::MAVLinkV2MessageRaw; + +use std::{collections::HashMap, sync::Mutex}; + +// signing configuration +pub struct SigningConfig { + secret_key: [u8; 32], + pub(crate) sign_outgoing: bool, + allow_unsigned: bool, +} + +// mutable state of signing per connection +pub(crate) struct SigningState { + timestamp: u64, + // does never really change but is definitely not part of the setup + link_id: u8, + stream_timestamps: HashMap<(u8, u8, u8), u64>, // TODO unsigned callback +} + +pub struct SigningData { + pub(crate) config: SigningConfig, + pub(crate) state: Mutex, +} + +impl SigningConfig { + pub fn new(secret_key: [u8; 32], sign_outgoing: bool, allow_unsigned: bool) -> Self { + SigningConfig { + secret_key, + sign_outgoing, + allow_unsigned, + } + } +} + +impl SigningData { + pub fn from_config(config: SigningConfig) -> Self { + Self { + config, + state: Mutex::new(SigningState { + timestamp: 0, + link_id: 0, + stream_timestamps: HashMap::new(), + }), + } + } + + pub(crate) fn verify_signature(&self, message: MAVLinkV2MessageRaw) -> bool { + use crate::MAVLINK_IFLAG_SIGNED; + // TODO: fix that unwrap poison + let mut state = self.state.lock().unwrap(); + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let timestamp = message.signature_timestamp(); + let src_system = message.system_id(); + let src_component = message.component_id(); + let stream_key = (message.signature_link_id(), src_system, src_component); + match state.stream_timestamps.get(&stream_key) { + Some(stream_timestamp) => { + if timestamp <= *stream_timestamp { + // reject old timestamp + return false; + } + } + None => { + if timestamp + 60 * 1000 * 100 < state.timestamp { + // bad new stream, more then a minute older the the last one + return false; + } + } + } + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + let result = signature_buffer == message.signature_value(); + if result { + state.stream_timestamps.insert(stream_key, timestamp); + state.timestamp = u64::max(state.timestamp, timestamp) + } + result + } else { + self.config.allow_unsigned + } + } + + pub(crate) fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { + // TODO: fix that unwrap poison + let mut state = self.state.lock().unwrap(); + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let ts_bytes = u64::to_le_bytes(state.timestamp); + message + .signature_timestamp_bytes_mut() + .copy_from_slice(&ts_bytes[0..6]); + // TODO link id set + *message.signature_link_id_mut() = state.link_id; + + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + + message + .signature_value_mut() + .copy_from_slice(&signature_buffer); + state.timestamp += 1; + } + + fn get_current_timestamp() -> u64 { + use std::time::SystemTime; + // fallback to 0 if the system time appears to be before epoch + let now = SystemTime::now() + .duration_since(SystemTime::UNIX_EPOCH) + .map(|n| n.as_micros()) + .unwrap_or(0); + // use 1st January 2015 GMT as offset, fallback to 0 if before that date, this will overflow in April 2104 + ((now + .checked_sub(1420070400u128 * 1000000u128) + .unwrap_or_default()) + / 10u128) as u64 + } +} diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 73d456aa3b..2f92d36bae 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -12,6 +12,9 @@ use std::time::Duration; use super::get_socket_addr; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -42,6 +45,8 @@ pub fn tcpout(address: T) -> io::Result { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } @@ -60,6 +65,8 @@ pub fn tcpin(address: T) -> io::Result { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } Err(e) => { @@ -78,6 +85,8 @@ pub struct TcpConnection { reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, } struct TcpWrite { @@ -112,4 +121,9 @@ impl MavConnection for TcpConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 91bbfaef45..80f09adb6b 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -13,6 +13,9 @@ use std::sync::Mutex; use super::get_socket_addr; +#[cfg(feature = "signing")] +use super::signing::{SigningConfig, SigningData}; + pub fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -91,6 +94,8 @@ pub struct UdpConnection { writer: Mutex, protocol_version: MavlinkVersion, server: bool, + #[cfg(feature = "signing")] + signing_data: Option, } impl UdpConnection { @@ -108,6 +113,8 @@ impl UdpConnection { sequence: 0, }), protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, }) } } @@ -160,6 +167,11 @@ impl MavConnection for UdpConnection { fn protocol_version(&self) -> MavlinkVersion { self.protocol_version } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + } } #[cfg(test)] diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index fa6bea9c48..3b8402b858 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -54,6 +54,13 @@ pub mod embedded; #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; +#[cfg(feature = "signing")] +use self::connection::signing::SigningData; +#[cfg(feature = "signing")] +pub use self::connection::SigningConfig; +#[cfg(feature = "signing")] +use sha2::{Digest, Sha256}; + pub const MAX_FRAME_SIZE: usize = 280; pub trait Message @@ -93,7 +100,7 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { -pub incompat_flags: u8, + pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -553,6 +560,11 @@ impl MAVLinkV2MessageRaw { self.0[2] } + #[inline] + pub fn incompatibility_flags_mut(&mut self) -> &mut u8 { + &mut self.0[2] + } + #[inline] pub fn compatibility_flags(&self) -> u8 { self.0[3] @@ -593,6 +605,67 @@ impl MAVLinkV2MessageRaw { ]) } + #[cfg(feature = "signing")] + #[inline] + pub fn checksum_bytes(&self) -> &[u8] { + let checksum_offset = 1 + Self::HEADER_SIZE + self.payload_length() as usize; + &self.0[checksum_offset..(checksum_offset + 2)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id(&self) -> u8 { + let payload_length: usize = self.payload_length().into(); + self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_link_id_mut(&mut self) -> &mut u8 { + let payload_length: usize = self.payload_length().into(); + &mut self.0[1 + Self::HEADER_SIZE + payload_length + 2] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp_bytes_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let timestamp_start = 1 + Self::HEADER_SIZE + payload_length + 3; + &mut self.0[timestamp_start..(timestamp_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_timestamp(&self) -> u64 { + let mut timestamp_bytes = [0u8; 8]; + timestamp_bytes[0..6].copy_from_slice(self.signature_timestamp_bytes()); + u64::from_le_bytes(timestamp_bytes) + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value(&self) -> &[u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &self.0[signature_start..(signature_start + 6)] + } + + #[cfg(feature = "signing")] + #[inline] + pub fn signature_value_mut(&mut self) -> &mut [u8] { + let payload_length: usize = self.payload_length().into(); + let signature_start = 1 + Self::HEADER_SIZE + payload_length + 3 + 6; + &mut self.0[signature_start..(signature_start + 6)] + } + fn mut_payload_and_checksum_and_sign(&mut self) -> &mut [u8] { let payload_length: usize = self.payload_length().into(); @@ -617,6 +690,19 @@ impl MAVLinkV2MessageRaw { ) } + #[cfg(feature = "signing")] + pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) { + let mut hasher = Sha256::new(); + hasher.update(secret_key); + hasher.update(&[MAV_STX_V2]); + hasher.update(self.header()); + hasher.update(self.payload()); + hasher.update(self.checksum_bytes()); + hasher.update(&[self.signature_link_id()]); + hasher.update(self.signature_timestamp_bytes()); + target_buffer.copy_from_slice(&hasher.finalize()[0..6]); + } + pub fn raw_bytes(&self) -> &[u8] { let payload_length = self.payload_length() as usize; From 33d05648bd7d9d84186a88635cc718784ff51ba5 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 11 Aug 2024 19:13:57 +0200 Subject: [PATCH 127/159] fix: move signing mod, remove incompat_flags since it is breaking feat: add distinct pub fn for signing, signing in MavConnection send, add signing feature to mavlink test: add --features signing to all test, add signing test, add Debug, Clone to SigningConfig --- .github/workflows/test.yml | 2 +- mavlink-core/src/connection/direct_serial.rs | 29 +++- mavlink-core/src/connection/file.rs | 16 +- mavlink-core/src/connection/mod.rs | 4 +- mavlink-core/src/connection/tcp.rs | 30 +++- mavlink-core/src/connection/udp.rs | 34 ++++- mavlink-core/src/lib.rs | 143 ++++++++++++++++-- mavlink-core/src/{connection => }/signing.rs | 67 +++++--- mavlink/Cargo.toml | 1 + .../examples/embedded-async-read/src/main.rs | 1 - mavlink/examples/embedded/src/main.rs | 1 - mavlink/tests/mav_frame_tests.rs | 1 - mavlink/tests/signing.rs | 100 ++++++++++++ mavlink/tests/tcp_loopback_tests.rs | 23 ++- mavlink/tests/test_shared/mod.rs | 8 +- 15 files changed, 398 insertions(+), 62 deletions(-) rename mavlink-core/src/{connection => }/signing.rs (55%) create mode 100644 mavlink/tests/signing.rs diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 313eb75862..45d144f3c0 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -34,7 +34,7 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable - name: Run internal tests - run: cargo test --verbose --features ${{ matrix.dialect }} -- --nocapture + run: cargo test --verbose --features ${{ matrix.dialect }} --features signing -- --nocapture mavlink-dump: runs-on: ubuntu-latest diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index daea3a0b77..d2f376aa14 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -2,7 +2,7 @@ use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io; use std::sync::Mutex; @@ -10,8 +10,10 @@ use std::sync::Mutex; use crate::error::{MessageReadError, MessageWriteError}; use serial::{prelude::*, SystemPort}; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(settings: &str) -> io::Result { let settings_toks: Vec<&str> = settings.split(':').collect(); @@ -65,7 +67,15 @@ impl MavConnection for SerialConnection { fn recv(&self) -> Result<(MavHeader, M), MessageReadError> { let mut port = self.port.lock().unwrap(); loop { - match read_versioned_msg(port.deref_mut(), self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(port.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + port.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } @@ -84,7 +94,6 @@ impl MavConnection for SerialConnection { let mut sequence = self.sequence.lock().unwrap(); let header = MavHeader { - incompat_flags: 0, sequence: *sequence, system_id: header.system_id, component_id: header.component_id, @@ -92,7 +101,17 @@ impl MavConnection for SerialConnection { *sequence = sequence.wrapping_add(1); - write_versioned_msg(port.reader_mut(), self.protocol_version, header, data) + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(port.reader_mut(), self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + port.reader_mut(), + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result } 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 8209392abc..c635d2be2d 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -3,14 +3,16 @@ use crate::connection::MavConnection; use crate::error::{MessageReadError, MessageWriteError}; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::fs::File; use std::io; use std::sync::Mutex; +#[cfg(not(feature = "signing"))] +use crate::read_versioned_msg; #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, SigningConfig, SigningData}; pub fn open(file_path: &str) -> io::Result { let file = File::open(file_path)?; @@ -37,7 +39,15 @@ impl MavConnection for FileConnection { let mut file = self.file.lock().unwrap(); loop { - match read_versioned_msg(file.deref_mut(), self.protocol_version) { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(file.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + file.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + match result { ok @ Ok(..) => { return ok; } diff --git a/mavlink-core/src/connection/mod.rs b/mavlink-core/src/connection/mod.rs index f4d6e6e332..e60386a6aa 100644 --- a/mavlink-core/src/connection/mod.rs +++ b/mavlink-core/src/connection/mod.rs @@ -12,9 +12,7 @@ mod udp; mod direct_serial; #[cfg(feature = "signing")] -pub(crate) mod signing; -#[cfg(feature = "signing")] -pub use signing::SigningConfig; +use crate::SigningConfig; mod file; diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 2f92d36bae..04761ff6af 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -2,7 +2,7 @@ use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io; use std::net::ToSocketAddrs; @@ -12,8 +12,11 @@ use std::time::Duration; use super::get_socket_addr; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn select_protocol( address: &str, @@ -97,21 +100,38 @@ struct TcpWrite { impl MavConnection for TcpConnection { fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut reader = self.reader.lock().unwrap(); - read_versioned_msg(reader.deref_mut(), self.protocol_version) + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); + result } fn send(&self, header: &MavHeader, data: &M) -> Result { let mut lock = self.writer.lock().unwrap(); let header = MavHeader { - incompat_flags: 0, sequence: lock.sequence, system_id: header.system_id, component_id: header.component_id, }; lock.sequence = lock.sequence.wrapping_add(1); - write_versioned_msg(&mut lock.socket, self.protocol_version, header, data) + #[cfg(not(feature = "signing"))] + let result = write_versioned_msg(&mut lock.socket, self.protocol_version, header, data); + #[cfg(feature = "signing")] + let result = write_versioned_msg_signed( + &mut lock.socket, + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ); + result } fn set_protocol_version(&mut self, version: MavlinkVersion) { diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 80f09adb6b..9481684167 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -4,7 +4,7 @@ use std::collections::VecDeque; use crate::connection::MavConnection; use crate::peek_reader::PeekReader; -use crate::{read_versioned_msg, write_versioned_msg, MavHeader, MavlinkVersion, Message}; +use crate::{MavHeader, MavlinkVersion, Message}; use core::ops::DerefMut; use std::io::{self, Read}; use std::net::ToSocketAddrs; @@ -13,8 +13,11 @@ use std::sync::Mutex; use super::get_socket_addr; +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg, write_versioned_msg}; + #[cfg(feature = "signing")] -use super::signing::{SigningConfig, SigningData}; +use crate::{read_versioned_msg_signed, write_versioned_msg_signed, SigningConfig, SigningData}; pub fn select_protocol( address: &str, @@ -124,7 +127,14 @@ impl MavConnection for UdpConnection { let mut reader = self.reader.lock().unwrap(); loop { + #[cfg(not(feature = "signing"))] let result = read_versioned_msg(reader.deref_mut(), self.protocol_version); + #[cfg(feature = "signing")] + let result = read_versioned_msg_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ); if self.server { if let addr @ Some(_) = reader.reader_ref().last_recv_address { self.writer.lock().unwrap().dest = addr; @@ -141,7 +151,6 @@ impl MavConnection for UdpConnection { let state = &mut *guard; let header = MavHeader { - incompat_flags: 0, sequence: state.sequence, system_id: header.system_id, component_id: header.component_id, @@ -151,7 +160,24 @@ impl MavConnection for UdpConnection { let len = if let Some(addr) = state.dest { let mut buf = Vec::new(); - write_versioned_msg(&mut buf, self.protocol_version, header, data)?; + #[cfg(not(feature = "signing"))] + write_versioned_msg( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; + #[cfg(feature = "signing")] + write_versioned_msg_signed( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; state.socket.send_to(&buf, addr)? } else { 0 diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 3b8402b858..fd9d9476fe 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -54,10 +54,12 @@ pub mod embedded; #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] use embedded::{Read, Write}; +#[cfg(not(feature = "signing"))] +type SigningData = (); #[cfg(feature = "signing")] -use self::connection::signing::SigningData; +mod signing; #[cfg(feature = "signing")] -pub use self::connection::SigningConfig; +pub use self::signing::{SigningConfig, SigningData}; #[cfg(feature = "signing")] use sha2::{Digest, Sha256}; @@ -100,7 +102,6 @@ pub trait MessageData: Sized { #[derive(Debug, Copy, Clone, PartialEq, Eq)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] pub struct MavHeader { - pub incompat_flags: u8, pub system_id: u8, pub component_id: u8, pub sequence: u8, @@ -126,7 +127,6 @@ pub const MAV_STX_V2: u8 = 0xFD; impl Default for MavHeader { fn default() -> Self { Self { - incompat_flags: 0, system_id: 255, component_id: 0, sequence: 0, @@ -220,7 +220,6 @@ impl MavFrame { let system_id = buf.get_u8(); let component_id = buf.get_u8(); let header = MavHeader { - incompat_flags: 0, system_id, component_id, sequence, @@ -262,6 +261,18 @@ pub fn read_versioned_msg( } } +#[cfg(feature = "signing")] +pub fn read_versioned_msg_signed( + r: &mut PeekReader, + version: MavlinkVersion, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_inner(r, signing_data), + MavlinkVersion::V1 => read_v1_msg(r), + } +} + #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: `` pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); @@ -721,6 +732,7 @@ impl MAVLinkV2MessageRaw { msgid: u32, payload_length: usize, extra_crc: u8, + incompat_flags: u8, ) { self.0[0] = MAV_STX_V2; let msgid_bytes = msgid.to_le_bytes(); @@ -728,7 +740,7 @@ impl MAVLinkV2MessageRaw { let header_buf = self.mut_header(); header_buf.copy_from_slice(&[ payload_length as u8, - header.incompat_flags, + incompat_flags, 0, //compat_flags header.sequence, header.system_id, @@ -757,6 +769,21 @@ impl MAVLinkV2MessageRaw { message_id, payload_length, M::extra_crc(message_id), + 0, + ); + } + + pub fn serialize_message_for_signing(&mut self, header: MavHeader, message: &M) { + let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; + let payload_length = message.ser(MavlinkVersion::V2, payload_buf); + + let message_id = message.message_id(); + self.serialize_stx_and_header_and_crc( + header, + message_id, + payload_length, + M::extra_crc(message_id), + 0x01, ); } @@ -764,14 +791,35 @@ impl MAVLinkV2MessageRaw { let payload_buf = &mut self.0[(1 + Self::HEADER_SIZE)..(1 + Self::HEADER_SIZE + 255)]; let payload_length = message_data.ser(MavlinkVersion::V2, payload_buf); - self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC); + self.serialize_stx_and_header_and_crc(header, D::ID, payload_length, D::EXTRA_CRC, 0); } } /// Return a raw buffer with the mavlink message +/// /// V2 maximum size is 280 bytes: `` +#[inline] pub fn read_v2_raw_message( reader: &mut PeekReader, +) -> Result { + read_v2_raw_message_inner::(reader, None) +} + +/// Return a raw buffer with the mavlink message with signing support +/// +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_raw_message_signed( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_inner::(reader, signing_data) +} + +fn read_v2_raw_message_inner( + reader: &mut PeekReader, + signing_data: Option<&SigningData>, ) -> Result { loop { loop { @@ -801,9 +849,20 @@ pub fn read_v2_raw_message( .copy_from_slice(payload_and_checksum_and_sign); if message.has_valid_crc::() { + // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes reader.consume(message.raw_bytes().len() - 1); - return Ok(message); + } else { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } } + + return Ok(message); } } @@ -891,10 +950,28 @@ pub async fn read_v2_raw_message_async( } /// Read a MAVLink v2 message from a Read stream. +#[inline] pub fn read_v2_msg( read: &mut PeekReader, ) -> Result<(MavHeader, M), error::MessageReadError> { - let message = read_v2_raw_message::(read)?; + read_v2_msg_inner(read, None) +} + +/// Read a MAVLink v2 message from a Read stream. +#[cfg(feature = "signing")] +#[inline] +pub fn read_v2_msg_signed( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_inner(read, signing_data) +} + +fn read_v2_msg_inner( + read: &mut PeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_inner::(read, signing_data)?; Ok(( MavHeader { @@ -960,6 +1037,21 @@ pub fn write_versioned_msg( } } +/// Write a message with signing support using the given mavlink version +#[cfg(feature = "signing")] +pub fn write_versioned_msg_signed( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_signed(w, header, data, signing_data), + MavlinkVersion::V1 => write_v1_msg(w, header, data), + } +} + /// Async write a message using the given mavlink version #[cfg(feature = "tokio-1")] pub async fn write_versioned_msg_async( @@ -1008,6 +1100,39 @@ pub fn write_v2_msg( Ok(len) } +/// Write a MAVLink v2 message to a Write stream with signing support. +#[cfg(feature = "signing")] +pub fn write_v2_msg_signed( + w: &mut W, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + + let signature_len = if let Some(signing_data) = signing_data { + if signing_data.config.sign_outgoing { + //header.incompat_flags |= MAVLINK_IFLAG_SIGNED; + message_raw.serialize_message_for_signing(header, data); + signing_data.sign_message(&mut message_raw); + MAVLinkV2MessageRaw::SIGNATURE_SIZE + } else { + message_raw.serialize_message(header, data); + 0 + } + } else { + message_raw.serialize_message(header, data); + 0 + }; + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len; + + w.write_all(&message_raw.0[..len])?; + + Ok(len) +} + /// Async write a MAVLink v2 message to a Write stream. #[cfg(feature = "tokio-1")] pub async fn write_v2_msg_async( diff --git a/mavlink-core/src/connection/signing.rs b/mavlink-core/src/signing.rs similarity index 55% rename from mavlink-core/src/connection/signing.rs rename to mavlink-core/src/signing.rs index fe43120e64..6007901806 100644 --- a/mavlink-core/src/connection/signing.rs +++ b/mavlink-core/src/signing.rs @@ -1,8 +1,12 @@ use crate::MAVLinkV2MessageRaw; +use std::time::SystemTime; use std::{collections::HashMap, sync::Mutex}; -// signing configuration +use crate::MAVLINK_IFLAG_SIGNED; + +/// Configuration used for MAVLink 2 messages signing as defined in . +#[derive(Debug, Clone)] pub struct SigningConfig { secret_key: [u8; 32], pub(crate) sign_outgoing: bool, @@ -12,11 +16,12 @@ pub struct SigningConfig { // mutable state of signing per connection pub(crate) struct SigningState { timestamp: u64, - // does never really change but is definitely not part of the setup + // currently link id is constant 0 link_id: u8, - stream_timestamps: HashMap<(u8, u8, u8), u64>, // TODO unsigned callback + stream_timestamps: HashMap<(u8, u8, u8), u64>, } +/// MAVLink 2 message signing data. pub struct SigningData { pub(crate) config: SigningConfig, pub(crate) state: Mutex, @@ -44,10 +49,15 @@ impl SigningData { } } - pub(crate) fn verify_signature(&self, message: MAVLinkV2MessageRaw) -> bool { - use crate::MAVLINK_IFLAG_SIGNED; - // TODO: fix that unwrap poison - let mut state = self.state.lock().unwrap(); + /// Verify the signature of a MAVLink 2 message. + pub fn verify_signature(&self, message: &MAVLinkV2MessageRaw) -> bool { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); let timestamp = message.signature_timestamp(); @@ -73,6 +83,7 @@ impl SigningData { message.calculate_signature(&self.config.secret_key, &mut signature_buffer); let result = signature_buffer == message.signature_value(); if result { + // if signature is valid update timestamps state.stream_timestamps.insert(stream_key, timestamp); state.timestamp = u64::max(state.timestamp, timestamp) } @@ -82,34 +93,40 @@ impl SigningData { } } - pub(crate) fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { - // TODO: fix that unwrap poison - let mut state = self.state.lock().unwrap(); - state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); - let ts_bytes = u64::to_le_bytes(state.timestamp); - message - .signature_timestamp_bytes_mut() - .copy_from_slice(&ts_bytes[0..6]); - // TODO link id set - *message.signature_link_id_mut() = state.link_id; + /// Sign a MAVLink 2 message if its incompatibility flag is set accordingly. + pub fn sign_message(&self, message: &mut MAVLinkV2MessageRaw) { + if message.incompatibility_flags() & MAVLINK_IFLAG_SIGNED > 0 { + // The code that holds the mutex lock is not expected to panic, therefore the expect is justified. + // The only issue that might cause a panic, presuming the opertions on the message buffer are sound, + // is the `SystemTime::now()` call in `get_current_timestamp()`. + let mut state = self + .state + .lock() + .expect("Code holding MutexGuard should not panic."); + state.timestamp = u64::max(state.timestamp, Self::get_current_timestamp()); + let ts_bytes = u64::to_le_bytes(state.timestamp); + message + .signature_timestamp_bytes_mut() + .copy_from_slice(&ts_bytes[0..6]); + *message.signature_link_id_mut() = state.link_id; - let mut signature_buffer = [0u8; 6]; - message.calculate_signature(&self.config.secret_key, &mut signature_buffer); + let mut signature_buffer = [0u8; 6]; + message.calculate_signature(&self.config.secret_key, &mut signature_buffer); - message - .signature_value_mut() - .copy_from_slice(&signature_buffer); - state.timestamp += 1; + message + .signature_value_mut() + .copy_from_slice(&signature_buffer); + state.timestamp += 1; + } } fn get_current_timestamp() -> u64 { - use std::time::SystemTime; // fallback to 0 if the system time appears to be before epoch let now = SystemTime::now() .duration_since(SystemTime::UNIX_EPOCH) .map(|n| n.as_micros()) .unwrap_or(0); - // use 1st January 2015 GMT as offset, fallback to 0 if before that date, this will overflow in April 2104 + // use 1st January 2015 GMT as offset, fallback to 0 if before that date, the used 48bit of this will overflow in 2104 ((now .checked_sub(1420070400u128 * 1000000u128) .unwrap_or_default()) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 46896b017c..0573047572 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -92,6 +92,7 @@ serde_arrays = { version = "0.1.0", optional = true } "std" = ["mavlink-core/std"] "udp" = ["mavlink-core/udp"] "tcp" = ["mavlink-core/tcp"] +"signing" = ["mavlink-core/signing"] "direct-serial" = ["mavlink-core/direct-serial"] # NOTE: Only one of 'embedded' and 'embedded-hal-02' features can be enabled. # Use "embedded' feature to enable embedded-hal=1.0 (embedded-io and embedded-io-async is part of embedded-hal). diff --git a/mavlink/examples/embedded-async-read/src/main.rs b/mavlink/examples/embedded-async-read/src/main.rs index 6b4e5c1dc1..7f53335de3 100644 --- a/mavlink/examples/embedded-async-read/src/main.rs +++ b/mavlink/examples/embedded-async-read/src/main.rs @@ -39,7 +39,6 @@ async fn main(spawner: Spawner) { // Create our mavlink header and heartbeat message let header = mavlink::MavHeader { - incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/examples/embedded/src/main.rs b/mavlink/examples/embedded/src/main.rs index 21b70396b1..4a71ca8fcf 100644 --- a/mavlink/examples/embedded/src/main.rs +++ b/mavlink/examples/embedded/src/main.rs @@ -82,7 +82,6 @@ fn main() -> ! { fn mavlink_header() -> mavlink::MavHeader { mavlink::MavHeader { - incompat_flags: 0, system_id: 1, component_id: 1, sequence: 42, diff --git a/mavlink/tests/mav_frame_tests.rs b/mavlink/tests/mav_frame_tests.rs index 5472dbb8c1..1c225efd09 100644 --- a/mavlink/tests/mav_frame_tests.rs +++ b/mavlink/tests/mav_frame_tests.rs @@ -96,7 +96,6 @@ mod mav_frame_tests { fn new(msg: MavMessage) -> MavFrame { MavFrame { header: MavHeader { - incompat_flags: 0, system_id: 1, component_id: 2, sequence: 84, diff --git a/mavlink/tests/signing.rs b/mavlink/tests/signing.rs new file mode 100644 index 0000000000..5ac3837f9d --- /dev/null +++ b/mavlink/tests/signing.rs @@ -0,0 +1,100 @@ +mod test_shared; + +#[cfg(feature = "signing")] +mod signing { + use mavlink::{ + common::HEARTBEAT_DATA, peek_reader::PeekReader, read_v2_raw_message, MAVLinkV2MessageRaw, + MavHeader, SigningConfig, SigningData, MAV_STX_V2, + }; + + use crate::test_shared::SECRET_KEY; + + const HEARTBEAT_SIGNED: &[u8] = &[ + MAV_STX_V2, + 0x09, + 0x01, // MAVLINK_IFLAG_SIGNED + 0x00, + crate::test_shared::COMMON_MSG_HEADER.sequence, + crate::test_shared::COMMON_MSG_HEADER.system_id, + crate::test_shared::COMMON_MSG_HEADER.component_id, + 0x00, // msg ID + 0x00, + 0x00, + 0x05, // payload + 0x00, + 0x00, + 0x00, + 0x02, + 0x03, + 0x59, + 0x03, + 0x03, + 0xc9, // checksum + 0x8b, + 0x00, // link_id + 0xff, // use max timestamp to ensure test will never fail against current time + 0xff, + 0xff, + 0xff, + 0xff, + 0xff, + 0x27, // signature + 0x18, + 0xb1, + 0x68, + 0xcc, + 0xf5, + ]; + + #[test] + pub fn test_verify() { + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let msg = read_v2_raw_message::(&mut r).unwrap(); + assert!( + signing_data.verify_signature(&msg), + "Message verification failed" + ); + } + + #[test] + pub fn test_invalid_ts() { + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + let mut r = PeekReader::new(HEARTBEAT_SIGNED); + let mut msg = read_v2_raw_message::(&mut r).unwrap(); + msg.signature_timestamp_bytes_mut() + .copy_from_slice(&[0, 0, 0, 0, 0, 0]); // set timestamp to min causing the timestamp test to fail + assert!( + !signing_data.verify_signature(&msg), + "Invalid message verified" + ); + } + + #[test] + pub fn test_sign_verify() { + use mavlink::common::MavMessage; + let heartbeat_message = MavMessage::HEARTBEAT(HEARTBEAT_DATA::default()); + let mut message = MAVLinkV2MessageRaw::new(); + let header = MavHeader { + system_id: 4, + component_id: 3, + sequence: 42, + }; + message.serialize_message_for_signing(header, &heartbeat_message); + + let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_data = SigningData::from_config(signing_cfg); + signing_data.sign_message(&mut message); + assert!( + signing_data.verify_signature(&message), + "Message verification failed" + ); + // the same message must not be allowed to be verified again + assert!( + !signing_data.verify_signature(&message), + "Invalid message verified" + ); + } +} diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index abea99598b..e097febe42 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -4,14 +4,27 @@ mod test_shared; mod test_tcp_connections { use std::thread; - /// Test whether we can send a message via TCP and receive it OK + #[cfg(feature = "signing")] + use mavlink::SigningConfig; + + use crate::test_shared; + + /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] pub fn test_tcp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 5; + #[cfg(feature = "signing")] + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, true, false); + #[cfg(feature = "signing")] + let singing_cfg_client = singing_cfg_server.clone(); + let server_thread = thread::spawn(move || { //TODO consider using get_available_port to use a random port - let server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + let mut server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + + #[cfg(feature = "signing")] + server.setup_signing(Some(singing_cfg_server)); let mut recv_count = 0; for _i in 0..RECEIVE_CHECK_COUNT { @@ -40,8 +53,12 @@ mod test_tcp_connections { thread::spawn(move || { let msg = mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); - let client = + let mut client = mavlink::connect("tcpout:127.0.0.1:14550").expect("Couldn't create client"); + + #[cfg(feature = "signing")] + client.setup_signing(Some(singing_cfg_client)); + for _i in 0..RECEIVE_CHECK_COUNT { client.send_default(&msg).ok(); } diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 332e9a1f02..45a72bf054 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -1,12 +1,18 @@ #![allow(unused)] pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { - incompat_flags: 0, sequence: 239, system_id: 1, component_id: 2, }; +#[cfg(feature = "signing")] +pub const SECRET_KEY: [u8; 32] = [ + 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, + 0x16, 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, + 0x56, 0xb3, +]; + #[cfg(feature = "common")] pub fn get_heartbeat_msg() -> mavlink::common::HEARTBEAT_DATA { mavlink::common::HEARTBEAT_DATA { From 312968c18e49237dbb9de56fcfa4f332109eed1e Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 11 Aug 2024 19:32:14 +0200 Subject: [PATCH 128/159] fix: remove std requirement for signing, fmt --- mavlink-core/Cargo.toml | 2 +- mavlink/tests/tcp_loopback_tests.rs | 5 +++-- mavlink/tests/test_shared/mod.rs | 5 ++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index d131be3c11..d276b2c44e 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -41,5 +41,5 @@ sha2 = { version = "0.10", optional = true } "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] "tokio-1" = ["dep:tokio"] -"signing" = ["dep:sha2", "std"] +"signing" = ["dep:sha2"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index e097febe42..7977c67256 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -9,7 +9,7 @@ mod test_tcp_connections { use crate::test_shared; - /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. + /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] pub fn test_tcp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 5; @@ -21,7 +21,8 @@ mod test_tcp_connections { let server_thread = thread::spawn(move || { //TODO consider using get_available_port to use a random port - let mut server = mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); + let mut server = + mavlink::connect("tcpin:0.0.0.0:14550").expect("Couldn't create server"); #[cfg(feature = "signing")] server.setup_signing(Some(singing_cfg_server)); diff --git a/mavlink/tests/test_shared/mod.rs b/mavlink/tests/test_shared/mod.rs index 45a72bf054..90f019cebd 100644 --- a/mavlink/tests/test_shared/mod.rs +++ b/mavlink/tests/test_shared/mod.rs @@ -8,9 +8,8 @@ pub const COMMON_MSG_HEADER: mavlink::MavHeader = mavlink::MavHeader { #[cfg(feature = "signing")] pub const SECRET_KEY: [u8; 32] = [ - 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, - 0x16, 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, - 0x56, 0xb3, + 0x00, 0x01, 0xf2, 0xe3, 0xd4, 0xc5, 0xb6, 0xa7, 0x98, 0x00, 0x70, 0x76, 0x34, 0x32, 0x00, 0x16, + 0x22, 0x42, 0x00, 0xcc, 0xff, 0x7a, 0x00, 0x52, 0x75, 0x73, 0x74, 0x00, 0x4d, 0x41, 0x56, 0xb3, ]; #[cfg(feature = "common")] From f2641535f52ce2dfb77985d370e270dbdd55a3de Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:02:29 +0200 Subject: [PATCH 129/159] test: add features signing, tokio-1 to clippy test: add signing to msrv check --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 45d144f3c0..08992a9375 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -23,7 +23,7 @@ jobs: components: clippy - uses: actions-rs-plus/clippy-check@v2 with: - args: --all --all-targets --features format-generated-code + args: --all --all-targets --features format-generated-code --features signing --features tokio-1 internal-tests: runs-on: ubuntu-latest @@ -59,7 +59,7 @@ jobs: with: use-cross: true command: check - args: --all --all-targets + args: --all --all-targets --features signing build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] From 6592af04a775da92832d986b47ca8bf76b2345d6 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:03:49 +0200 Subject: [PATCH 130/159] feat: add link_id to signing config --- mavlink-core/src/signing.rs | 16 ++++++++++------ mavlink/tests/signing.rs | 6 +++--- mavlink/tests/tcp_loopback_tests.rs | 6 +++--- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/mavlink-core/src/signing.rs b/mavlink-core/src/signing.rs index 6007901806..f1b129037c 100644 --- a/mavlink-core/src/signing.rs +++ b/mavlink-core/src/signing.rs @@ -9,6 +9,7 @@ use crate::MAVLINK_IFLAG_SIGNED; #[derive(Debug, Clone)] pub struct SigningConfig { secret_key: [u8; 32], + link_id: u8, pub(crate) sign_outgoing: bool, allow_unsigned: bool, } @@ -16,8 +17,6 @@ pub struct SigningConfig { // mutable state of signing per connection pub(crate) struct SigningState { timestamp: u64, - // currently link id is constant 0 - link_id: u8, stream_timestamps: HashMap<(u8, u8, u8), u64>, } @@ -28,9 +27,15 @@ pub struct SigningData { } impl SigningConfig { - pub fn new(secret_key: [u8; 32], sign_outgoing: bool, allow_unsigned: bool) -> Self { - SigningConfig { + pub fn new( + secret_key: [u8; 32], + link_id: u8, + sign_outgoing: bool, + allow_unsigned: bool, + ) -> Self { + Self { secret_key, + link_id, sign_outgoing, allow_unsigned, } @@ -43,7 +48,6 @@ impl SigningData { config, state: Mutex::new(SigningState { timestamp: 0, - link_id: 0, stream_timestamps: HashMap::new(), }), } @@ -108,7 +112,7 @@ impl SigningData { message .signature_timestamp_bytes_mut() .copy_from_slice(&ts_bytes[0..6]); - *message.signature_link_id_mut() = state.link_id; + *message.signature_link_id_mut() = self.config.link_id; let mut signature_buffer = [0u8; 6]; message.calculate_signature(&self.config.secret_key, &mut signature_buffer); diff --git a/mavlink/tests/signing.rs b/mavlink/tests/signing.rs index 5ac3837f9d..52ca63cd46 100644 --- a/mavlink/tests/signing.rs +++ b/mavlink/tests/signing.rs @@ -48,7 +48,7 @@ mod signing { #[test] pub fn test_verify() { - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); let mut r = PeekReader::new(HEARTBEAT_SIGNED); let msg = read_v2_raw_message::(&mut r).unwrap(); @@ -60,7 +60,7 @@ mod signing { #[test] pub fn test_invalid_ts() { - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); let mut r = PeekReader::new(HEARTBEAT_SIGNED); let mut msg = read_v2_raw_message::(&mut r).unwrap(); @@ -84,7 +84,7 @@ mod signing { }; message.serialize_message_for_signing(header, &heartbeat_message); - let signing_cfg = SigningConfig::new(SECRET_KEY, true, false); + let signing_cfg = SigningConfig::new(SECRET_KEY, 0, true, false); let signing_data = SigningData::from_config(signing_cfg); signing_data.sign_message(&mut message); assert!( diff --git a/mavlink/tests/tcp_loopback_tests.rs b/mavlink/tests/tcp_loopback_tests.rs index 7977c67256..170189cb97 100644 --- a/mavlink/tests/tcp_loopback_tests.rs +++ b/mavlink/tests/tcp_loopback_tests.rs @@ -5,9 +5,9 @@ mod test_tcp_connections { use std::thread; #[cfg(feature = "signing")] - use mavlink::SigningConfig; - use crate::test_shared; + #[cfg(feature = "signing")] + use mavlink::SigningConfig; /// Test whether we can send a message via TCP and receive it OK. This also test signing as a property of a MavConnection if the signing feature is enabled. #[test] @@ -15,7 +15,7 @@ mod test_tcp_connections { const RECEIVE_CHECK_COUNT: i32 = 5; #[cfg(feature = "signing")] - let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, true, false); + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, 0, true, false); #[cfg(feature = "signing")] let singing_cfg_client = singing_cfg_server.clone(); From 42b84d199a1f769fb779dfe821fadacf00083938 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:04:47 +0200 Subject: [PATCH 131/159] feat: add signing to async read/write functions --- mavlink-core/src/lib.rs | 32 +++++++++++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index fd9d9476fe..1ac75b915a 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -867,10 +867,21 @@ fn read_v2_raw_message_inner( } /// Async read a raw buffer with the mavlink message +/// /// V2 maximum size is 280 bytes: `` #[cfg(feature = "tokio-1")] pub async fn read_v2_raw_message_async( reader: &mut R, +) -> Result { + read_v2_raw_message_async_inner::(reader, None).await +} + +/// Async read a raw buffer with the mavlink message +/// V2 maximum size is 280 bytes: `` +#[cfg(feature = "tokio-1")] +async fn read_v2_raw_message_async_inner( + reader: &mut R, + signing_data: Option<&SigningData>, ) -> Result { loop { loop { @@ -895,10 +906,29 @@ pub async fn read_v2_raw_message_async() { + if !message.has_valid_crc::() { + continue; + } + + #[cfg(feature = "signing")] + if let Some(signing_data) = signing_data { + if !signing_data.verify_signature(&message) { + continue; + } + } + return Ok(message); } } + +/// Async read a raw buffer with the mavlink message with signing support +/// V2 maximum size is 280 bytes: `` +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_v2_raw_message_async_signed( + reader: &mut R, + signing_data: Option<&SigningData>, +) -> Result { + read_v2_raw_message_async_inner::(reader, signing_data).await } /// Async read a raw buffer with the mavlink message From 04a4732ec5127304e608a83ebc2e72ef9a15983d Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 23 Aug 2024 20:05:16 +0200 Subject: [PATCH 132/159] fix: clippy warnings --- mavlink-core/src/connection/direct_serial.rs | 2 +- mavlink-core/src/connection/file.rs | 2 +- mavlink-core/src/connection/tcp.rs | 2 +- mavlink-core/src/connection/udp.rs | 2 +- mavlink-core/src/lib.rs | 8 ++++---- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/mavlink-core/src/connection/direct_serial.rs b/mavlink-core/src/connection/direct_serial.rs index d2f376aa14..2954b98107 100644 --- a/mavlink-core/src/connection/direct_serial.rs +++ b/mavlink-core/src/connection/direct_serial.rs @@ -124,6 +124,6 @@ impl MavConnection for SerialConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/file.rs b/mavlink-core/src/connection/file.rs index c635d2be2d..7258259347 100644 --- a/mavlink-core/src/connection/file.rs +++ b/mavlink-core/src/connection/file.rs @@ -75,6 +75,6 @@ impl MavConnection for FileConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/tcp.rs b/mavlink-core/src/connection/tcp.rs index 04761ff6af..d362a5664a 100644 --- a/mavlink-core/src/connection/tcp.rs +++ b/mavlink-core/src/connection/tcp.rs @@ -144,6 +144,6 @@ impl MavConnection for TcpConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 9481684167..d2058fc896 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -196,7 +196,7 @@ impl MavConnection for UdpConnection { #[cfg(feature = "signing")] fn setup_signing(&mut self, signing_data: Option) { - self.signing_data = signing_data.map(|cfg| SigningData::from_config(cfg)) + self.signing_data = signing_data.map(SigningData::from_config) } } diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 1ac75b915a..12d470b614 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -705,11 +705,11 @@ impl MAVLinkV2MessageRaw { pub fn calculate_signature(&self, secret_key: &[u8], target_buffer: &mut [u8; 6]) { let mut hasher = Sha256::new(); hasher.update(secret_key); - hasher.update(&[MAV_STX_V2]); + hasher.update([MAV_STX_V2]); hasher.update(self.header()); hasher.update(self.payload()); hasher.update(self.checksum_bytes()); - hasher.update(&[self.signature_link_id()]); + hasher.update([self.signature_link_id()]); hasher.update(self.signature_timestamp_bytes()); target_buffer.copy_from_slice(&hasher.finalize()[0..6]); } @@ -917,9 +917,9 @@ async fn read_v2_raw_message_async_inner` From e481f5fd99d2604880f82b076243d0f99ffbb51c Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 00:03:23 +0200 Subject: [PATCH 133/159] fix: remove redundant feature gate test: add signing to internal-tests matrix test: add signing as msrv matrix option --- .github/workflows/test.yml | 8 ++++++-- mavlink-core/src/connection/udp.rs | 3 --- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 08992a9375..071df1ef7b 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -30,11 +30,12 @@ jobs: strategy: matrix: dialect: ["ardupilotmega", "asluav", "matrixpilot", "minimal", "paparazzi", "python_array_test", "slugs", "standard", "test", "ualberta", "uavionix", "icarous", "common"] + signing: ["", "--features signing"] steps: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@stable - name: Run internal tests - run: cargo test --verbose --features ${{ matrix.dialect }} --features signing -- --nocapture + run: cargo test --verbose --features ${{ matrix.dialect }} ${{ matrix.signing }} -- --nocapture mavlink-dump: runs-on: ubuntu-latest @@ -46,6 +47,9 @@ jobs: msrv: runs-on: ubuntu-latest + strategy: + matrix: + signing: ["", "--features signing"] steps: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master @@ -59,7 +63,7 @@ jobs: with: use-cross: true command: check - args: --all --all-targets --features signing + args: --all --all-targets ${{ matrix.signing }} build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index d2058fc896..6a4c75defe 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -166,8 +166,6 @@ impl MavConnection for UdpConnection { self.protocol_version, header, data, - #[cfg(feature = "signing")] - self.signing_data.as_ref(), )?; #[cfg(feature = "signing")] write_versioned_msg_signed( @@ -175,7 +173,6 @@ impl MavConnection for UdpConnection { self.protocol_version, header, data, - #[cfg(feature = "signing")] self.signing_data.as_ref(), )?; state.socket.send_to(&buf, addr)? From e34585702d98ddb097dedb3bc0a0992a1b49b418 Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 00:06:04 +0200 Subject: [PATCH 134/159] fix: fmt --- mavlink-core/src/connection/udp.rs | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/mavlink-core/src/connection/udp.rs b/mavlink-core/src/connection/udp.rs index 6a4c75defe..75e5be3c48 100644 --- a/mavlink-core/src/connection/udp.rs +++ b/mavlink-core/src/connection/udp.rs @@ -161,12 +161,7 @@ impl MavConnection for UdpConnection { let len = if let Some(addr) = state.dest { let mut buf = Vec::new(); #[cfg(not(feature = "signing"))] - write_versioned_msg( - &mut buf, - self.protocol_version, - header, - data, - )?; + write_versioned_msg(&mut buf, self.protocol_version, header, data)?; #[cfg(feature = "signing")] write_versioned_msg_signed( &mut buf, From f83fcbd9f06982c7dc2df25e519a24670461fc56 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 25 Aug 2024 19:12:44 +0100 Subject: [PATCH 135/159] chore(deps): update 'quickxml' --- mavlink-bindgen/Cargo.toml | 2 +- mavlink-bindgen/src/parser.rs | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index f34d44ab1d..d885ef9990 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -11,7 +11,7 @@ repository = "https://github.com/mavlink/rust-mavlink" [dependencies] crc-any = { workspace = true, default-features = false } -quick-xml = "0.26" +quick-xml = "0.36" quote = "1" proc-macro2 = "1.0.43" lazy_static = "1.2.0" diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index dd8e389ea1..eb4c2d6957 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1069,8 +1069,7 @@ pub fn parse_profile( path: in_path.to_path_buf(), })?; let mut reader = Reader::from_reader(BufReader::new(file)); - reader.trim_text(true); - reader.trim_text_end(true); + reader.config_mut().trim_text(true); let mut buf = Vec::new(); loop { From b41d5576d837dcf28b6e75c47cb4c95676ae4c10 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 25 Aug 2024 19:35:41 +0100 Subject: [PATCH 136/159] chore(deps): update 'num_derive' --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 064abc84b4..509312c49c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,7 +5,7 @@ resolver = "1" [workspace.dependencies] crc-any = { version = "2.3.5", default-features = false } num-traits = { version = "0.2", default-features = false } -num-derive = "0.3.2" +num-derive = "0.4" bitflags = "1.2.1" byteorder = { version = "1.3.4", default-features = false } From 1b8fbf1e63e727749f0b6a9951b66b625d1cd75d Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 11:51:10 +0200 Subject: [PATCH 137/159] doc: add tokio-1 and signing to docs.rs features --- mavlink/Cargo.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 0573047572..14ffd19199 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -112,4 +112,6 @@ features = [ "emit-description", "emit-extensions", "format-generated-code", + "tokio-1", + "signing" ] From 3285c6db7a6a37af26a71faa275de234ffe6f002 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Mon, 26 Aug 2024 05:50:38 +0100 Subject: [PATCH 138/159] exclude embedded-hal v0.2 from dependabot updates --- .github/dependabot.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 8ef5278d08..83fc85e850 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -4,6 +4,9 @@ updates: directory: "/" schedule: interval: daily + ignore: + - dependency-name: "embedded-hal" + versions: ["^0.2"] - package-ecosystem: github-actions directory: "/" schedule: From a729400831e57d991005f5443a47400734bb4bf7 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 25 Aug 2024 06:58:41 +0100 Subject: [PATCH 139/159] update MSRV checks to account for features --- .github/workflows/test.yml | 21 +++++++++------------ Cargo.toml | 1 + mavlink-bindgen/Cargo.toml | 1 + mavlink-core/Cargo.toml | 2 +- mavlink/Cargo.toml | 2 +- 5 files changed, 13 insertions(+), 14 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 071df1ef7b..c4c02f44d3 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -48,22 +48,19 @@ jobs: msrv: runs-on: ubuntu-latest strategy: - matrix: - signing: ["", "--features signing"] + matrix: + features: ["", "--features serde,tokio-1", "--features signing"] steps: - uses: actions/checkout@master + - name: Get MSRV from Cargo.toml + run: | + MSRV=$(grep 'rust-version' Cargo.toml | sed 's/.*= *"\(.*\)".*/\1/') + echo "MSRV=$MSRV" >> $GITHUB_ENV - uses: dtolnay/rust-toolchain@master with: - toolchain: 1.65.0 - - uses: actions-rs/cargo@v1 - with: - command: install - args: cross --locked - - uses: actions-rs/cargo@v1 - with: - use-cross: true - command: check - args: --all --all-targets ${{ matrix.signing }} + toolchain: ${{ env.MSRV }} + - uses: taiki-e/install-action@cargo-no-dev-deps + - run: cargo no-dev-deps check --all --all-targets ${{ matrix.features }} build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] diff --git a/Cargo.toml b/Cargo.toml index 509312c49c..71ebeb23ac 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,3 +11,4 @@ byteorder = { version = "1.3.4", default-features = false } [workspace.package] edition = "2021" +rust-version = "1.70.0" diff --git a/mavlink-bindgen/Cargo.toml b/mavlink-bindgen/Cargo.toml index d885ef9990..2c23aeefaa 100644 --- a/mavlink-bindgen/Cargo.toml +++ b/mavlink-bindgen/Cargo.toml @@ -6,6 +6,7 @@ license = "MIT/Apache-2.0" description = "Library used by rust-mavlink." readme = "README.md" repository = "https://github.com/mavlink/rust-mavlink" +rust-version.workspace = true # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index d276b2c44e..64a20c938d 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -14,7 +14,7 @@ readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition.workspace = true -rust-version = "1.65.0" +rust-version.workspace = true [dependencies] crc-any = { workspace = true, default-features = false } diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index 14ffd19199..cafcd6d1d6 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -16,7 +16,7 @@ readme = "../README.md" license = "MIT/Apache-2.0" repository = "https://github.com/mavlink/rust-mavlink" edition.workspace = true -rust-version = "1.65.0" +rust-version.workspace = true [build-dependencies] mavlink-bindgen = { path = "../mavlink-bindgen", default-features = false } From d5c5bc3f894d357a29ee0ef8c422284affaee780 Mon Sep 17 00:00:00 2001 From: Daniel Eades Date: Sun, 25 Aug 2024 07:25:02 +0100 Subject: [PATCH 140/159] address some lint warnings --- mavlink-bindgen/src/parser.rs | 37 ++++++++++++++++------------------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index eb4c2d6957..447367e129 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -365,7 +365,7 @@ impl MavEnum { #[cfg(feature = "emit-description")] let description = if let Some(description) = self.description.as_ref() { - let desc = format!("{description}"); + let desc = description.to_string(); quote!(#[doc = #desc]) } else { quote!() @@ -1001,7 +1001,7 @@ pub enum MavXmlElement { Extensions, } -fn identify_element(s: &[u8]) -> Option { +const fn identify_element(s: &[u8]) -> Option { use self::MavXmlElement::*; match s { b"version" => Some(Version), @@ -1066,7 +1066,7 @@ pub fn parse_profile( let mut events: Vec> = Vec::new(); let file = File::open(&in_path).map_err(|e| BindGenError::CouldNotReadDefinitionFile { source: e, - path: in_path.to_path_buf(), + path: in_path.clone(), })?; let mut reader = Reader::from_reader(BufReader::new(file)); reader.config_mut().trim_text(true); @@ -1088,14 +1088,11 @@ pub fn parse_profile( for e in events { match e { Ok(Event::Start(bytes)) => { - let id = match identify_element(bytes.name().into_inner()) { - None => { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - } - Some(kind) => kind, + let Some(id) = identify_element(bytes.name().into_inner()) else { + panic!( + "unexpected element {:?}", + String::from_utf8_lossy(bytes.name().into_inner()) + ); }; assert!( @@ -1109,20 +1106,20 @@ pub fn parse_profile( is_in_extension = true; } MavXmlElement::Message => { - message = Default::default(); + message = MavMessage::default(); } MavXmlElement::Field => { - field = Default::default(); + field = MavField::default(); field.is_extension = is_in_extension; } MavXmlElement::Enum => { - mavenum = Default::default(); + mavenum = MavEnum::default(); } MavXmlElement::Entry => { - entry = Default::default(); + entry = MavEnumEntry::default(); } MavXmlElement::Include => { - include = Default::default(); + include = PathBuf::default(); } MavXmlElement::Param => { paramid = None; @@ -1237,7 +1234,7 @@ pub fn parse_profile( if entry.params.is_none() { entry.params = Some(vec![]); } - if let b"index" = attr.key.into_inner() { + if attr.key.into_inner() == b"index" { let s = std::str::from_utf8(&attr.value).unwrap(); paramid = Some(s.parse::().unwrap()); } @@ -1251,7 +1248,7 @@ pub fn parse_profile( is_in_extension = true; } b"entry" => { - entry = Default::default(); + entry = MavEnumEntry::default(); for attr in bytes.attributes() { let attr = attr.unwrap(); match attr.key.into_inner() { @@ -1311,7 +1308,7 @@ pub fn parse_profile( eprintln!("TODO: deprecated {s:?}"); } data => { - panic!("unexpected text data {:?} reading {:?}", data, s); + panic!("unexpected text data {data:?} reading {s:?}"); } } } @@ -1489,7 +1486,7 @@ impl MavXmlFilter { } !self.extension_filter.is_in } - Err(error) => panic!("Failed to filter XML: {}", error), + Err(error) => panic!("Failed to filter XML: {error}"), } } } From cd600a2b13aad86b719a9451cd6983d9c9d5ce5d Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 16:35:26 +0200 Subject: [PATCH 141/159] doc: add doc_auto_cfg feature to doc and docsrs build for mavlink and mavlink-core --- mavlink-core/src/lib.rs | 1 + mavlink/src/lib.rs | 1 + 2 files changed, 2 insertions(+) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 12d470b614..0d3273c62b 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -19,6 +19,7 @@ //! feature without also using the `uavionix` and `icarous` features. //! #![cfg_attr(not(feature = "std"), no_std)] +#![cfg_attr(any(docsrs, doc), feature(doc_auto_cfg))] #![deny(clippy::all)] #![warn(clippy::use_self)] diff --git a/mavlink/src/lib.rs b/mavlink/src/lib.rs index 3690dd954c..71790d96d9 100644 --- a/mavlink/src/lib.rs +++ b/mavlink/src/lib.rs @@ -1,4 +1,5 @@ #![cfg_attr(not(feature = "std"), no_std)] +#![cfg_attr(any(docsrs, doc), feature(doc_auto_cfg))] // include generate definitions include!(concat!(env!("OUT_DIR"), "/mod.rs")); From c8b10f0eb990225a079667c86570421b9c8d070a Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 16:37:11 +0200 Subject: [PATCH 142/159] test: switch docs test to nightly, add all features used for docs.rs --- .github/workflows/test.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index c4c02f44d3..60f4ca1f96 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -126,9 +126,9 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@master - - uses: dtolnay/rust-toolchain@stable + - uses: dtolnay/rust-toolchain@nightly - name: Build docs - run: cargo doc + run: cargo doc --features "default all-dialects emit-description emit-extensions format-generated-code tokio-1 signing" - name: Deploy uses: peaceiris/actions-gh-pages@v3 if: ${{ github.ref == 'refs/heads/master' }} From bec09cc88908f8a2611f946bccfe79a681b055b3 Mon Sep 17 00:00:00 2001 From: pv42 Date: Mon, 26 Aug 2024 16:57:23 +0200 Subject: [PATCH 143/159] test: disable doc_auto_cfg for doctest --- mavlink-core/src/lib.rs | 2 +- mavlink/src/lib.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 0d3273c62b..2ef674b104 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -19,7 +19,7 @@ //! feature without also using the `uavionix` and `icarous` features. //! #![cfg_attr(not(feature = "std"), no_std)] -#![cfg_attr(any(docsrs, doc), feature(doc_auto_cfg))] +#![cfg_attr(all(any(docsrs, doc), not(doctest)), feature(doc_auto_cfg))] #![deny(clippy::all)] #![warn(clippy::use_self)] diff --git a/mavlink/src/lib.rs b/mavlink/src/lib.rs index 71790d96d9..bfdcbeafb2 100644 --- a/mavlink/src/lib.rs +++ b/mavlink/src/lib.rs @@ -1,5 +1,5 @@ #![cfg_attr(not(feature = "std"), no_std)] -#![cfg_attr(any(docsrs, doc), feature(doc_auto_cfg))] +#![cfg_attr(all(any(docsrs, doc), not(doctest)), feature(doc_auto_cfg))] // include generate definitions include!(concat!(env!("OUT_DIR"), "/mod.rs")); From 49ecc035436a5537ec92d5e6b8d524fd2e3e7dd4 Mon Sep 17 00:00:00 2001 From: Cyril Plisko Date: Thu, 29 Aug 2024 22:45:45 +0300 Subject: [PATCH 144/159] mavlink/build.rs simplification --- mavlink/build/main.rs | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/mavlink/build/main.rs b/mavlink/build/main.rs index 4d1cfd4c73..64f3aea0ab 100644 --- a/mavlink/build/main.rs +++ b/mavlink/build/main.rs @@ -21,10 +21,8 @@ fn main() -> ExitCode { } // find & apply patches to XML definitions to avoid crashes - let mut patch_dir = src_dir.to_path_buf(); - patch_dir.push("build/patches"); - let mut mavlink_dir = src_dir.to_path_buf(); - mavlink_dir.push("mavlink"); + let patch_dir = src_dir.join("build/patches"); + let mavlink_dir = src_dir.join("mavlink"); if let Ok(dir) = read_dir(patch_dir) { for entry in dir.flatten() { @@ -40,8 +38,7 @@ fn main() -> ExitCode { } } - let mut definitions_dir = src_dir.to_path_buf(); - definitions_dir.push("mavlink/message_definitions/v1.0"); + let definitions_dir = src_dir.join("mavlink/message_definitions/v1.0"); let out_dir = env::var("OUT_DIR").unwrap(); From 3cc4cceb4350d27e3c83b406aa63ecd583ec7949 Mon Sep 17 00:00:00 2001 From: Cyril Plisko Date: Sun, 8 Sep 2024 10:35:12 +0300 Subject: [PATCH 145/159] mavlink-bindgen: build.rs simplification --- mavlink-bindgen/src/parser.rs | 42 +++++++++++++++-------------------- 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 447367e129..fc6cff305a 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1150,23 +1150,16 @@ pub fn parse_profile( Some(&MavXmlElement::Entry) => { match attr.key.into_inner() { b"name" => { - let name = String::from_utf8(attr.value.to_vec()).unwrap(); - entry.name = name; + entry.name = String::from_utf8_lossy(&attr.value).to_string(); } b"value" => { + let value = String::from_utf8_lossy(&attr.value); // Deal with hexadecimal numbers - if attr.value.starts_with(b"0x") { - entry.value = Some( - u32::from_str_radix( - std::str::from_utf8(&attr.value[2..]).unwrap(), - 16, - ) - .unwrap(), - ); - } else { - let s = std::str::from_utf8(&attr.value[..]).unwrap(); - entry.value = Some(s.parse::().unwrap()); - } + let (src, radix) = value + .strip_prefix("0x") + .map(|value| (value, 16)) + .unwrap_or((value.as_ref(), 10)); + entry.value = u32::from_str_radix(src, radix).ok(); } _ => (), } @@ -1187,11 +1180,11 @@ pub fn parse_profile( .collect::>() .join(""); */ - message.name = String::from_utf8(attr.value.to_vec()).unwrap(); + message.name = String::from_utf8_lossy(&attr.value).to_string(); } b"id" => { - let s = std::str::from_utf8(&attr.value).unwrap(); - message.id = s.parse::().unwrap(); + message.id = + String::from_utf8_lossy(&attr.value).parse().unwrap(); } _ => (), } @@ -1199,15 +1192,16 @@ pub fn parse_profile( Some(&MavXmlElement::Field) => { match attr.key.into_inner() { b"name" => { - let name = String::from_utf8(attr.value.to_vec()).unwrap(); - field.name = name; - if field.name == "type" { - field.name = "mavtype".to_string(); - } + let name = String::from_utf8_lossy(&attr.value); + field.name = if name == "type" { + "mavtype".to_string() + } else { + name.to_string() + }; } b"type" => { - let s = std::str::from_utf8(&attr.value).unwrap(); - field.mavtype = MavType::parse_type(s).unwrap(); + let r#type = String::from_utf8_lossy(&attr.value); + field.mavtype = MavType::parse_type(&r#type).unwrap(); } b"enum" => { field.enumtype = Some( From 4cc4513fdc8115c74b7fe0e0249a3421d36a0e6d Mon Sep 17 00:00:00 2001 From: Cyril Plisko Date: Mon, 9 Sep 2024 11:52:40 +0300 Subject: [PATCH 146/159] mavlink-bindgen: parser: Simple PascalCase implementation --- mavlink-bindgen/src/parser.rs | 39 ++++++++++++++++------------------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index fc6cff305a..05398e60ac 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1134,16 +1134,7 @@ pub fn parse_profile( match stack.last() { Some(&MavXmlElement::Enum) => { if attr.key.into_inner() == b"name" { - mavenum.name = attr - .value - .clone() - .split(|b| *b == b'_') - .map(|x| x.to_ascii_lowercase()) - .map(|mut v| { - v[0] = v[0].to_ascii_uppercase(); - String::from_utf8(v).unwrap() - }) - .collect(); + mavenum.name = to_pascal_case(attr.value); //mavenum.name = attr.value.clone(); } } @@ -1204,17 +1195,7 @@ pub fn parse_profile( field.mavtype = MavType::parse_type(&r#type).unwrap(); } b"enum" => { - field.enumtype = Some( - attr.value - .clone() - .split(|b| *b == b'_') - .map(|x| x.to_ascii_lowercase()) - .map(|mut v| { - v[0] = v[0].to_ascii_uppercase(); - String::from_utf8(v).unwrap() - }) - .collect(), - ); + field.enumtype = Some(to_pascal_case(attr.value)); //field.enumtype = Some(attr.value.clone()); } b"display" => { @@ -1484,3 +1465,19 @@ impl MavXmlFilter { } } } + +fn to_pascal_case(text: impl AsRef<[u8]>) -> String { + text.as_ref() + .split(|c| *c == b'_') + .map(String::from_utf8_lossy) + .map(capitalize_word) + .collect() +} + +fn capitalize_word(text: impl AsRef) -> String { + let mut chars = text.as_ref().chars(); + match chars.next() { + None => String::new(), + Some(char) => char.to_uppercase().to_string() + &chars.as_str().to_ascii_lowercase(), + } +} From 8f6fe9588ff60e3f5ad05da4eb25350deca61a09 Mon Sep 17 00:00:00 2001 From: Cyril Plisko Date: Mon, 9 Sep 2024 12:07:29 +0300 Subject: [PATCH 147/159] mavlink-bindgen: parser: less unwrap() --- mavlink-bindgen/src/parser.rs | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 05398e60ac..dce7263129 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1200,7 +1200,7 @@ pub fn parse_profile( } b"display" => { field.display = - Some(String::from_utf8(attr.value.to_vec()).unwrap()); + Some(String::from_utf8_lossy(&attr.value).to_string()); } _ => (), } @@ -1210,8 +1210,8 @@ pub fn parse_profile( entry.params = Some(vec![]); } if attr.key.into_inner() == b"index" { - let s = std::str::from_utf8(&attr.value).unwrap(); - paramid = Some(s.parse::().unwrap()); + paramid = + Some(String::from_utf8_lossy(&attr.value).parse().unwrap()); } } _ => (), @@ -1228,11 +1228,11 @@ pub fn parse_profile( let attr = attr.unwrap(); match attr.key.into_inner() { b"name" => { - entry.name = String::from_utf8(attr.value.to_vec()).unwrap(); + entry.name = String::from_utf8_lossy(&attr.value).to_string(); } b"value" => { - let s = std::str::from_utf8(&attr.value).unwrap(); - entry.value = Some(s.parse().unwrap()); + entry.value = + Some(String::from_utf8_lossy(&attr.value).parse().unwrap()); } _ => (), } @@ -1242,7 +1242,7 @@ pub fn parse_profile( _ => (), }, Ok(Event::Text(bytes)) => { - let s = String::from_utf8(bytes.to_vec()).unwrap(); + let s = String::from_utf8_lossy(&bytes).to_string(); use self::MavXmlElement::*; match (stack.last(), stack.get(stack.len() - 2)) { @@ -1259,15 +1259,16 @@ pub fn parse_profile( entry.description = Some(s.replace('\n', " ")); } (Some(&Param), Some(&Entry)) => { - if let Some(params) = &mut entry.params { + if let Some(params) = entry.params.as_mut() { // Some messages can jump between values, like: // 0, 1, 2, 7 - if params.len() < paramid.unwrap() { - for index in params.len()..paramid.unwrap() { + let paramid = paramid.unwrap(); + if params.len() < paramid { + for index in params.len()..paramid { params.insert(index, String::from("The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).")); } } - params[paramid.unwrap() - 1] = s; + params[paramid - 1] = s; } } (Some(&Include), Some(&Mavlink)) => { From 38d94ff81cb44b6c62eccb8ab5566554b37f1636 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?J=C3=B6rn=20Roth?= Date: Wed, 4 Sep 2024 14:00:38 +0200 Subject: [PATCH 148/159] test: remove all dev targets from msrv check (fixes #277) --- .github/workflows/test.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 60f4ca1f96..d3f2ef49c7 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -60,7 +60,7 @@ jobs: with: toolchain: ${{ env.MSRV }} - uses: taiki-e/install-action@cargo-no-dev-deps - - run: cargo no-dev-deps check --all --all-targets ${{ matrix.features }} + - run: cargo no-dev-deps check --all --lib --bins ${{ matrix.features }} build: needs: [formatting, linting, internal-tests, mavlink-dump, msrv] From 3e7191e1ae514b926a31b05dcbfbc4ebbd9497d6 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 30 Aug 2024 00:22:52 +0200 Subject: [PATCH 149/159] feat: add async connection mod with tcp, udp, file feat: add async peek reader feat: change async read fn to use AsyncPeekReader feat: add read_v1_msg_async, read_v2_msg_async_signed, write_versioned_msg_async_signed, write_v2_msg_async_signed --- mavlink-core/Cargo.toml | 9 +- mavlink-core/src/async_connection/file.rs | 83 +++++++ mavlink-core/src/async_connection/mod.rs | 137 +++++++++++ mavlink-core/src/async_connection/tcp.rs | 156 ++++++++++++ mavlink-core/src/async_connection/udp.rs | 277 ++++++++++++++++++++++ mavlink-core/src/async_peek_reader.rs | 147 ++++++++++++ mavlink-core/src/lib.rs | 185 ++++++++++++++- 7 files changed, 980 insertions(+), 14 deletions(-) create mode 100644 mavlink-core/src/async_connection/file.rs create mode 100644 mavlink-core/src/async_connection/mod.rs create mode 100644 mavlink-core/src/async_connection/tcp.rs create mode 100644 mavlink-core/src/async_connection/udp.rs create mode 100644 mavlink-core/src/async_peek_reader.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 64a20c938d..d51915905a 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -26,8 +26,9 @@ embedded-io-async = { version = "0.6.1", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } -tokio = { version = "1.0", default-features = false, features = ["io-util"], optional = true } +tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs", ], optional = true } sha2 = { version = "0.10", optional = true } +async-trait = { version = "0.1.18", optional = true } [features] "std" = ["byteorder/std"] @@ -40,6 +41,10 @@ sha2 = { version = "0.10", optional = true } "embedded" = ["dep:embedded-io", "dep:embedded-io-async"] "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] -"tokio-1" = ["dep:tokio"] +"tokio-1" = ["dep:tokio", "dep:async-trait"] "signing" = ["dep:sha2"] default = ["std", "tcp", "udp", "direct-serial", "serde"] + +[dev-dependencies] +# 1.39 tokio macros seems to be currently broken +tokio = { version = "1.0, <=1.38", default-features = false, features = ["io-util", "net", "sync", "fs","macros", "rt" ] } \ No newline at end of file diff --git a/mavlink-core/src/async_connection/file.rs b/mavlink-core/src/async_connection/file.rs new file mode 100644 index 0000000000..9b1c90af3a --- /dev/null +++ b/mavlink-core/src/async_connection/file.rs @@ -0,0 +1,83 @@ +use core::ops::DerefMut; + +use super::AsyncMavConnection; +use crate::error::{MessageReadError, MessageWriteError}; + +use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; + +use tokio::fs::File; +use tokio::io; +use tokio::sync::Mutex; + +#[cfg(not(feature = "signing"))] +use crate::read_versioned_msg_async; + +#[cfg(feature = "signing")] +use crate::{read_versioned_msg_async_signed, SigningConfig, SigningData}; + +/// File MAVLINK connection + +pub async fn open(file_path: &str) -> io::Result { + let file = File::open(file_path).await?; + Ok(AsyncFileConnection { + file: Mutex::new(AsyncPeekReader::new(file)), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub struct AsyncFileConnection { + file: Mutex>, + protocol_version: MavlinkVersion, + + #[cfg(feature = "signing")] + signing_data: Option, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncFileConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut file = self.file.lock().await; + + loop { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(file.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + file.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + match result { + ok @ Ok(..) => { + return ok; + } + Err(MessageReadError::Io(e)) => { + if e.kind() == io::ErrorKind::UnexpectedEof { + return Err(MessageReadError::Io(e)); + } + } + _ => {} + } + } + } + + async fn send(&self, _header: &MavHeader, _data: &M) -> Result { + Ok(0) + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn get_protocol_version(&self) -> MavlinkVersion { + self.protocol_version + } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(SigningData::from_config) + } +} diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs new file mode 100644 index 0000000000..6ef4cc156a --- /dev/null +++ b/mavlink-core/src/async_connection/mod.rs @@ -0,0 +1,137 @@ +use tokio::io; + +use crate::{MavFrame, MavHeader, MavlinkVersion, Message}; + +#[cfg(feature = "tcp")] +mod tcp; + +#[cfg(feature = "udp")] +mod udp; + +mod file; + +#[cfg(feature = "signing")] +use crate::SigningConfig; + +/// A MAVLink connection +#[async_trait::async_trait] +pub trait AsyncMavConnection { + /// Receive a mavlink message. + /// + /// Blocks until a valid frame is received, ignoring invalid messages. + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; + + /// Send a mavlink message + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result; + + fn set_protocol_version(&mut self, version: MavlinkVersion); + fn get_protocol_version(&self) -> MavlinkVersion; + + /// Write whole frame + async fn send_frame( + &self, + frame: &MavFrame, + ) -> Result { + self.send(&frame.header, &frame.msg).await + } + + /// Read whole frame + async fn recv_frame(&self) -> Result, crate::error::MessageReadError> { + let (header, msg) = self.recv().await?; + let protocol_version = self.get_protocol_version(); + Ok(MavFrame { + header, + msg, + protocol_version, + }) + } + + /// Send a message with default header + async fn send_default(&self, data: &M) -> Result { + let header = MavHeader::default(); + self.send(&header, data).await + } + + /// Setup secret key used for message signing, or disable message signing + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option); +} + +/// Connect to a MAVLink node by address string. +/// +/// The address must be in one of the following formats: +/// +/// * `tcpin::` to create a TCP server, listening for incoming connections +/// * `tcpout::` to create a TCP client +/// * `udpin::` to create a UDP server, listening for incoming packets +/// * `udpout::` to create a UDP client +/// * `udpbcast::` to create a UDP broadcast +/// * NOT `serial::` to create a serial connection +/// * `file:` to extract file data +/// +/// The type of the connection is determined at runtime based on the address type, so the +/// connection is returned as a trait object. +// TODO only reason this has to be send is udp serve +pub async fn connect_async( + address: &str, +) -> io::Result + Sync + Send>> { + let protocol_err = Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )); + + if cfg!(feature = "tcp") && address.starts_with("tcp") { + #[cfg(feature = "tcp")] + { + tcp::select_protocol(address).await + } + #[cfg(not(feature = "tcp"))] + { + protocol_err + } + } else if cfg!(feature = "udp") && address.starts_with("udp") { + #[cfg(feature = "udp")] + { + udp::select_protocol(address).await + } + #[cfg(not(feature = "udp"))] + { + protocol_err + } + } else if cfg!(feature = "direct-serial") && address.starts_with("serial:") { + #[cfg(feature = "direct-serial")] + { + todo!() + //Ok(Box::new(direct_serial::open(&address["serial:".len()..])?)) + } + #[cfg(not(feature = "direct-serial"))] + { + protocol_err + } + } else if address.starts_with("file") { + Ok(Box::new(file::open(&address["file:".len()..]).await?)) + } else { + protocol_err + } +} + +// TODO remove this ? +/// Returns the socket address for the given address. +pub(crate) fn get_socket_addr( + address: T, +) -> Result { + let addr = match address.to_socket_addrs()?.next() { + Some(addr) => addr, + None => { + return Err(io::Error::new( + io::ErrorKind::Other, + "Host address lookup failed", + )); + } + }; + Ok(addr) +} diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs new file mode 100644 index 0000000000..e05d70455b --- /dev/null +++ b/mavlink-core/src/async_connection/tcp.rs @@ -0,0 +1,156 @@ +use super::{get_socket_addr, AsyncMavConnection}; +use crate::async_peek_reader::AsyncPeekReader; +use crate::{MavHeader, MavlinkVersion, Message}; + +use core::ops::DerefMut; +use tokio::io; +use tokio::net::tcp::{OwnedReadHalf, OwnedWriteHalf}; +use tokio::net::{TcpListener, TcpStream}; +use tokio::sync::Mutex; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg_async, write_versioned_msg_async}; +#[cfg(feature = "signing")] +use crate::{ + read_versioned_msg_async_signed, write_versioned_msg_async_signed, SigningConfig, SigningData, +}; + +/// TCP MAVLink connection + +pub async fn select_protocol( + address: &str, +) -> io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("tcpout:") { + tcpout(address).await + } else if let Some(address) = address.strip_prefix("tcpin:") { + tcpin(address).await + } else { + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub async fn tcpout(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + + let socket = TcpStream::connect(addr).await?; + + let (reader, writer) = socket.into_split(); + + Ok(TcpConnection { + reader: Mutex::new(AsyncPeekReader::new(reader)), + writer: Mutex::new(TcpWrite { + socket: writer, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub async fn tcpin(address: T) -> io::Result { + let addr = get_socket_addr(address)?; + let listener = TcpListener::bind(addr).await?; + + //For now we only accept one incoming stream: this blocks until we get one + match listener.accept().await { + Ok((socket, _)) => { + let (reader, writer) = socket.into_split(); + return Ok(TcpConnection { + reader: Mutex::new(AsyncPeekReader::new(reader)), + writer: Mutex::new(TcpWrite { + socket: writer, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }); + } + Err(e) => { + //TODO don't println in lib + println!("listener err: {e}"); + } + } + Err(io::Error::new( + io::ErrorKind::NotConnected, + "No incoming connections!", + )) +} + +pub struct TcpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, +} + +struct TcpWrite { + socket: OwnedWriteHalf, + sequence: u8, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for TcpConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().await; + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(reader.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + result + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut lock = self.writer.lock().await; + + let header = MavHeader { + sequence: lock.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + lock.sequence = lock.sequence.wrapping_add(1); + #[cfg(not(feature = "signing"))] + let result = + write_versioned_msg_async(&mut lock.socket, self.protocol_version, header, data).await; + #[cfg(feature = "signing")] + let result = write_versioned_msg_async_signed( + &mut lock.socket, + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ) + .await; + result + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn get_protocol_version(&self) -> MavlinkVersion { + self.protocol_version + } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(SigningData::from_config) + } +} diff --git a/mavlink-core/src/async_connection/udp.rs b/mavlink-core/src/async_connection/udp.rs new file mode 100644 index 0000000000..cd945e8055 --- /dev/null +++ b/mavlink-core/src/async_connection/udp.rs @@ -0,0 +1,277 @@ +use core::{ops::DerefMut, task::Poll}; +use std::{collections::VecDeque, io::Read, sync::Arc}; + +use tokio::{ + io::{self, AsyncRead, ReadBuf}, + net::UdpSocket, + sync::Mutex, +}; + +use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; + +use super::{get_socket_addr, AsyncMavConnection}; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg_async, write_versioned_msg_async}; +#[cfg(feature = "signing")] +use crate::{ + read_versioned_msg_async_signed, write_versioned_msg_signed, SigningConfig, SigningData, +}; + +pub async fn select_protocol( + address: &str, +) -> tokio::io::Result + Sync + Send>> { + let connection = if let Some(address) = address.strip_prefix("udpin:") { + udpin(address).await + } else if let Some(address) = address.strip_prefix("udpout:") { + udpout(address).await + } else if let Some(address) = address.strip_prefix("udpbcast:") { + udpbcast(address).await + } else { + Err(tokio::io::Error::new( + tokio::io::ErrorKind::AddrNotAvailable, + "Protocol unsupported", + )) + }; + + Ok(Box::new(connection?)) +} + +pub async fn udpbcast(address: T) -> tokio::io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0").await?; + socket + .set_broadcast(true) + .expect("Couldn't bind to broadcast address."); + UdpConnection::new(socket, false, Some(addr)) +} + +pub async fn udpout(address: T) -> tokio::io::Result { + let addr = get_socket_addr(address)?; + let socket = UdpSocket::bind("0.0.0.0:0").await?; + UdpConnection::new(socket, false, Some(addr)) +} + +pub async fn udpin(address: T) -> tokio::io::Result { + let addr = address + .to_socket_addrs() + .unwrap() + .next() + .expect("Invalid address"); + let socket = UdpSocket::bind(addr).await?; + UdpConnection::new(socket, true, None) +} + +struct UdpRead { + socket: Arc, + buffer: VecDeque, + last_recv_address: Option, +} + +const MTU_SIZE: usize = 1500; +impl AsyncRead for UdpRead { + fn poll_read( + mut self: core::pin::Pin<&mut Self>, + cx: &mut core::task::Context<'_>, + buf: &mut tokio::io::ReadBuf<'_>, + ) -> Poll> { + if self.buffer.is_empty() { + let mut read_buffer = [0u8; MTU_SIZE]; + let mut read_buffer = ReadBuf::new(&mut read_buffer); + + match self.socket.poll_recv_from(cx, &mut read_buffer) { + Poll::Ready(Ok(address)) => { + let n_buffer = read_buffer.filled().len(); + + let n = (&read_buffer.filled()[0..n_buffer]).read(buf.initialize_unfilled())?; + buf.advance(n); + + self.buffer.extend(&read_buffer.filled()[n..n_buffer]); + self.last_recv_address = Some(address); + Poll::Ready(Ok(())) + } + Poll::Ready(Err(err)) => Poll::Ready(Err(err)), + Poll::Pending => Poll::Pending, + } + } else { + let read_result = self.buffer.read(buf.initialize_unfilled()); + let result = match read_result { + Ok(n) => { + buf.advance(n); + Ok(()) + } + Err(err) => Err(err), + }; + Poll::Ready(result) + } + } +} + +struct UdpWrite { + socket: Arc, + dest: Option, + sequence: u8, +} + +pub struct UdpConnection { + reader: Mutex>, + writer: Mutex, + protocol_version: MavlinkVersion, + server: bool, + #[cfg(feature = "signing")] + signing_data: Option, +} + +impl UdpConnection { + fn new( + socket: UdpSocket, + server: bool, + dest: Option, + ) -> tokio::io::Result { + let socket = Arc::new(socket); + Ok(Self { + server, + reader: Mutex::new(AsyncPeekReader::new(UdpRead { + socket: socket.clone(), + buffer: VecDeque::new(), + last_recv_address: None, + })), + writer: Mutex::new(UdpWrite { + socket, + dest, + sequence: 0, + }), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) + } +} + +#[async_trait::async_trait] +impl AsyncMavConnection for UdpConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut reader = self.reader.lock().await; + + loop { + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(reader.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + reader.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + if self.server { + if let addr @ Some(_) = reader.reader_ref().last_recv_address { + self.writer.lock().await.dest = addr; + } + } + if let ok @ Ok(..) = result { + return ok; + } + } + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut guard = self.writer.lock().await; + let state = &mut *guard; + + let header = MavHeader { + sequence: state.sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + state.sequence = state.sequence.wrapping_add(1); + + let len = if let Some(addr) = state.dest { + let mut buf = Vec::new(); + #[cfg(not(feature = "signing"))] + write_versioned_msg_async( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + ) + .await?; + #[cfg(feature = "signing")] + write_versioned_msg_signed( + &mut buf, + self.protocol_version, + header, + data, + #[cfg(feature = "signing")] + self.signing_data.as_ref(), + )?; + state.socket.send_to(&buf, addr).await? + } else { + 0 + }; + + Ok(len) + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn get_protocol_version(&self) -> MavlinkVersion { + self.protocol_version + } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(SigningData::from_config) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use tokio::io::AsyncReadExt; + + #[tokio::test] + async fn test_datagram_buffering() { + let receiver_socket = Arc::new(UdpSocket::bind("127.0.0.1:5001").await.unwrap()); + let mut udp_reader = UdpRead { + socket: receiver_socket.clone(), + buffer: VecDeque::new(), + last_recv_address: None, + }; + let sender_socket = UdpSocket::bind("0.0.0.0:0").await.unwrap(); + sender_socket.connect("127.0.0.1:5001").await.unwrap(); + + let datagram: Vec = (0..50).collect::>(); + + let mut n_sent = sender_socket.send(&datagram).await.unwrap(); + assert_eq!(n_sent, datagram.len()); + n_sent = sender_socket.send(&datagram).await.unwrap(); + assert_eq!(n_sent, datagram.len()); + + let mut buf = [0u8; 30]; + + let mut n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 30); + assert_eq!(&buf[0..n_read], (0..30).collect::>().as_slice()); + + n_read = udp_reader.read(&mut buf).await.unwrap(); + assert_eq!(n_read, 20); + assert_eq!(&buf[0..n_read], (30..50).collect::>().as_slice()); + } +} diff --git a/mavlink-core/src/async_peek_reader.rs b/mavlink-core/src/async_peek_reader.rs new file mode 100644 index 0000000000..1d6ada2c4b --- /dev/null +++ b/mavlink-core/src/async_peek_reader.rs @@ -0,0 +1,147 @@ +//! This module implements a buffered/peekable reader using async I/O. +//! +//! The purpose of the buffered/peekable reader is to allow for backtracking parsers. +//! +//! This is the async version of [`crate::peek_reader::PeekReader`]. +//! A reader implementing the tokio library's [`tokio::io::AsyncBufRead`]/[`tokio::io::AsyncBufReadExt`] traits 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 [`tokio::io::AsyncBufRead`]'s trait philosophy. +//! +//! The main type [`AsyncPeekReader`] does not implement [`tokio::io::AsyncBufReadExt`] itself, as there is no added benefit +//! in doing so. +//! +use tokio::io::AsyncReadExt; + +use crate::error::MessageReadError; + +/// A buffered/peekable reader +/// +/// This reader wraps a type implementing [`tokio::io::AsyncRead`] 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. +/// +/// NOTE: This reader is generic over the size of the buffer, defaulting to MAVLink's current largest +/// possible message size of 280 bytes +/// +pub struct AsyncPeekReader { + // Internal buffer + buffer: [u8; BUFFER_SIZE], + // The position of the next byte to read from the buffer. + cursor: usize, + // The position of the next byte to read into the buffer. + top: usize, + // The wrapped reader. + reader: R, +} + +impl AsyncPeekReader { + /// Instantiates a new [`AsyncPeekReader`], wrapping the provided [`tokio::io::AsyncReadExt`] and using the default chunk size + pub fn new(reader: R) -> Self { + Self { + buffer: [0; BUFFER_SIZE], + cursor: 0, + top: 0, + reader, + } + } + + /// 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 [`tokio::io::AsyncReadExt`] 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 async fn peek_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, false).await + } + + /// 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 [`tokio::io::AsyncReadExt`] 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 async fn read_exact(&mut self, amount: usize) -> Result<&[u8], MessageReadError> { + self.fetch(amount, true).await + } + + /// Reads a byte from the internal buffer + /// + /// If the internal buffer does not contain enough data, this function will read + /// from the underlying [`tokio::io::AsyncReadExt`] 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 async fn read_u8(&mut self) -> Result { + let buf = self.read_exact(1).await?; + Ok(buf[0]) + } + + /// 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.top - self.cursor); + self.cursor += amount; + amount + } + + /// Returns an immutable reference to the underlying [`tokio::io::AsyncRead`] + /// + /// 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 [`tokio::io::AsyncRead`] + /// + /// 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 + async fn fetch(&mut self, amount: usize, consume: bool) -> Result<&[u8], MessageReadError> { + let buffered = self.top - self.cursor; + + // the caller requested more bytes than we have buffered, fetch them from the reader + if buffered < amount { + let bytes_read = amount - buffered; + assert!(bytes_read < BUFFER_SIZE); + let mut buf = [0u8; BUFFER_SIZE]; + + // read needed bytes from reader + self.reader.read_exact(&mut buf[..bytes_read]).await?; + + // if some bytes were read, add them to the buffer + + if self.buffer.len() - self.top < bytes_read { + // reallocate + self.buffer.copy_within(self.cursor..self.top, 0); + self.cursor = 0; + self.top = buffered; + } + self.buffer[self.top..self.top + bytes_read].copy_from_slice(&buf[..bytes_read]); + self.top += bytes_read; + } + + let result = &self.buffer[self.cursor..self.cursor + amount]; + if consume { + self.cursor += amount; + } + Ok(result) + } +} diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 2ef674b104..d68fe1d214 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -50,6 +50,16 @@ pub mod error; #[cfg(feature = "std")] pub use self::connection::{connect, MavConnection}; +#[cfg(feature = "tokio-1")] +mod async_connection; +#[cfg(feature = "tokio-1")] +pub use self::async_connection::{connect_async, AsyncMavConnection}; + +#[cfg(feature = "tokio-1")] +pub mod async_peek_reader; +#[cfg(feature = "tokio-1")] +use async_peek_reader::AsyncPeekReader; + #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] pub mod embedded; #[cfg(any(feature = "embedded", feature = "embedded-hal-02"))] @@ -262,6 +272,17 @@ pub fn read_versioned_msg( } } +#[cfg(feature = "tokio-1")] +pub async fn read_versioned_msg_async( + r: &mut AsyncPeekReader, + version: MavlinkVersion, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_async(r).await, + MavlinkVersion::V1 => read_v1_msg_async(r).await, + } +} + #[cfg(feature = "signing")] pub fn read_versioned_msg_signed( r: &mut PeekReader, @@ -274,6 +295,18 @@ pub fn read_versioned_msg_signed( } } +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_versioned_msg_async_signed( + r: &mut AsyncPeekReader, + version: MavlinkVersion, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + match version { + MavlinkVersion::V2 => read_v2_msg_async_inner(r, signing_data).await, + MavlinkVersion::V1 => read_v1_msg_async(r).await, + } +} + #[derive(Debug, Copy, Clone, PartialEq, Eq)] // Follow protocol definition: `` pub struct MAVLinkV1MessageRaw([u8; 1 + Self::HEADER_SIZE + 255 + 2]); @@ -445,6 +478,42 @@ pub fn read_v1_raw_message( } } +/// Return a raw buffer with the mavlink message +/// V1 maximum size is 263 bytes: `` +#[cfg(feature = "tokio-1")] +pub async fn read_v1_raw_message_async( + reader: &mut AsyncPeekReader, +) -> Result { + loop { + loop { + // search for the magic framing value indicating start of mavlink message + if reader.read_u8().await? == MAV_STX { + break; + } + } + + let mut message = MAVLinkV1MessageRaw::new(); + + message.0[0] = MAV_STX; + let header = &reader.peek_exact(MAVLinkV1MessageRaw::HEADER_SIZE).await? + [..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).await? + [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); + } + } +} + /// Async read a raw buffer with the mavlink message /// V1 maximum size is 263 bytes: `` /// @@ -507,6 +576,27 @@ pub fn read_v1_msg( )) } +/// Read a MAVLink v1 message from a Read stream. +#[cfg(feature = "tokio-1")] +pub async fn read_v1_msg_async( + r: &mut AsyncPeekReader, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v1_raw_message_async::(r).await?; + + Ok(( + MavHeader { + sequence: message.sequence(), + system_id: message.system_id(), + component_id: message.component_id(), + }, + M::parse( + MavlinkVersion::V1, + u32::from(message.message_id()), + message.payload(), + )?, + )) +} + /// Async read a MAVLink v1 message from a Read stream. /// /// NOTE: it will be add ~80KB to firmware flash size because all *_DATA::deser methods will be add to firmware. @@ -872,7 +962,7 @@ fn read_v2_raw_message_inner( /// V2 maximum size is 280 bytes: `` #[cfg(feature = "tokio-1")] pub async fn read_v2_raw_message_async( - reader: &mut R, + reader: &mut AsyncPeekReader, ) -> Result { read_v2_raw_message_async_inner::(reader, None).await } @@ -881,7 +971,7 @@ pub async fn read_v2_raw_message_async` #[cfg(feature = "tokio-1")] async fn read_v2_raw_message_async_inner( - reader: &mut R, + reader: &mut AsyncPeekReader, signing_data: Option<&SigningData>, ) -> Result { loop { @@ -895,19 +985,26 @@ async fn read_v2_raw_message_async_inner 0 { // if there are incompatibility flags set that we do not know discard the message continue; } - reader - .read_exact(message.mut_payload_and_checksum_and_sign()) - .await?; + let packet_length = message.raw_bytes().len() - 1; + let payload_and_checksum_and_sign = &reader.peek_exact(packet_length).await? + [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::() { + if message.has_valid_crc::() { + // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes + reader.consume(message.raw_bytes().len() - 1); + } else { continue; } @@ -926,7 +1023,7 @@ async fn read_v2_raw_message_async_inner` #[cfg(all(feature = "tokio-1", feature = "signing"))] pub async fn read_v2_raw_message_async_signed( - reader: &mut R, + reader: &mut AsyncPeekReader, signing_data: Option<&SigningData>, ) -> Result { read_v2_raw_message_async_inner::(reader, signing_data).await @@ -1017,9 +1114,26 @@ fn read_v2_msg_inner( /// Async read a MAVLink v2 message from a Read stream. #[cfg(feature = "tokio-1")] pub async fn read_v2_msg_async( - read: &mut R, + read: &mut AsyncPeekReader, +) -> Result<(MavHeader, M), error::MessageReadError> { + read_v2_msg_async_inner(read, None).await +} + +/// Async read a MAVLink v2 message from a Read stream. +#[cfg(all(feature = "tokio-1", feature = "signing"))] +pub async fn read_v2_msg_async_signed( + read: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, ) -> Result<(MavHeader, M), error::MessageReadError> { - let message = read_v2_raw_message_async::(read).await?; + read_v2_msg_async_inner(read, signing_data).await +} + +#[cfg(feature = "tokio-1")] +async fn read_v2_msg_async_inner( + read: &mut AsyncPeekReader, + signing_data: Option<&SigningData>, +) -> Result<(MavHeader, M), error::MessageReadError> { + let message = read_v2_raw_message_async_signed::(read, signing_data).await?; Ok(( MavHeader { @@ -1097,6 +1211,21 @@ pub async fn write_versioned_msg_async( + w: &mut W, + version: MavlinkVersion, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + match version { + MavlinkVersion::V2 => write_v2_msg_async_signed(w, header, data, signing_data).await, + MavlinkVersion::V1 => write_v1_msg_async(w, header, data).await, + } +} + /// Async write a message using the given mavlink version /// /// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. @@ -1143,7 +1272,6 @@ pub fn write_v2_msg_signed( let signature_len = if let Some(signing_data) = signing_data { if signing_data.config.sign_outgoing { - //header.incompat_flags |= MAVLINK_IFLAG_SIGNED; message_raw.serialize_message_for_signing(header, data); signing_data.sign_message(&mut message_raw); MAVLinkV2MessageRaw::SIGNATURE_SIZE @@ -1182,6 +1310,39 @@ pub async fn write_v2_msg_async Ok(len) } +/// Write a MAVLink v2 message to a Write stream with signing support. +#[cfg(feature = "signing")] +#[cfg(feature = "tokio-1")] +pub async fn write_v2_msg_async_signed( + w: &mut W, + header: MavHeader, + data: &M, + signing_data: Option<&SigningData>, +) -> Result { + let mut message_raw = MAVLinkV2MessageRaw::new(); + + let signature_len = if let Some(signing_data) = signing_data { + if signing_data.config.sign_outgoing { + message_raw.serialize_message_for_signing(header, data); + signing_data.sign_message(&mut message_raw); + MAVLinkV2MessageRaw::SIGNATURE_SIZE + } else { + message_raw.serialize_message(header, data); + 0 + } + } else { + message_raw.serialize_message(header, data); + 0 + }; + + let payload_length: usize = message_raw.payload_length().into(); + let len = 1 + MAVLinkV2MessageRaw::HEADER_SIZE + payload_length + 2 + signature_len; + + w.write_all(&message_raw.0[..len]).await?; + + Ok(len) +} + /// Async write a MAVLink v2 message to a Write stream. /// /// NOTE: it will be add ~70KB to firmware flash size because all *_DATA::ser methods will be add to firmware. From 7dcc48f2143fcd0efd84625c26c4886a4cce9777 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 1 Sep 2024 16:13:49 +0200 Subject: [PATCH 150/159] test: add async versiosn of network loopback tests fix: remove tokio version cap for dev-dep --- mavlink-core/Cargo.toml | 3 +- mavlink/Cargo.toml | 3 + mavlink/tests/tcp_loopback_async_tests.rs | 69 +++++++++++++++++++++++ mavlink/tests/udp_loopback_async_tests.rs | 46 +++++++++++++++ 4 files changed, 119 insertions(+), 2 deletions(-) create mode 100644 mavlink/tests/tcp_loopback_async_tests.rs create mode 100644 mavlink/tests/udp_loopback_async_tests.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index d51915905a..eb749b07c4 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -46,5 +46,4 @@ async-trait = { version = "0.1.18", optional = true } default = ["std", "tcp", "udp", "direct-serial", "serde"] [dev-dependencies] -# 1.39 tokio macros seems to be currently broken -tokio = { version = "1.0, <=1.38", default-features = false, features = ["io-util", "net", "sync", "fs","macros", "rt" ] } \ No newline at end of file +tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs","macros", "rt" ] } \ No newline at end of file diff --git a/mavlink/Cargo.toml b/mavlink/Cargo.toml index cafcd6d1d6..669d9b0752 100644 --- a/mavlink/Cargo.toml +++ b/mavlink/Cargo.toml @@ -115,3 +115,6 @@ features = [ "tokio-1", "signing" ] + +[dev-dependencies] +tokio = { version = "1.0", default-features = false, features = ["macros", "rt", "time" ] } diff --git a/mavlink/tests/tcp_loopback_async_tests.rs b/mavlink/tests/tcp_loopback_async_tests.rs new file mode 100644 index 0000000000..edd56ba595 --- /dev/null +++ b/mavlink/tests/tcp_loopback_async_tests.rs @@ -0,0 +1,69 @@ +mod test_shared; + +#[cfg(all(feature = "tokio-1", feature = "tcp", feature = "common"))] +mod test_tcp_connections { + #[cfg(feature = "signing")] + use crate::test_shared; + #[cfg(feature = "signing")] + use mavlink::SigningConfig; + + /// Test whether we can send a message via TCP and receive it OK using async_connect. + /// This also test signing as a property of a MavConnection if the signing feature is enabled. + #[tokio::test] + pub async fn test_tcp_loopback() { + const RECEIVE_CHECK_COUNT: i32 = 5; + + #[cfg(feature = "signing")] + let singing_cfg_server = SigningConfig::new(test_shared::SECRET_KEY, 0, true, false); + #[cfg(feature = "signing")] + let singing_cfg_client = singing_cfg_server.clone(); + + let server_thread = tokio::spawn(async move { + //TODO consider using get_available_port to use a random port + let mut server = + mavlink::connect_async("tcpin:0.0.0.0:14551").await.expect("Couldn't create server"); + + #[cfg(feature = "signing")] + server.setup_signing(Some(singing_cfg_server)); + + let mut recv_count = 0; + for _i in 0..RECEIVE_CHECK_COUNT { + match server.recv().await { + Ok((_header, msg)) => { + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; + } + } + Err(..) => { + // one message read failure fails the test + break; + } + } + } + assert_eq!(recv_count, RECEIVE_CHECK_COUNT); + }); + + // Give some time for the server to connect + tokio::time::sleep(std::time::Duration::from_millis(100)).await; + + // have the client send a few hearbeats + tokio::spawn(async move { + let msg = + mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); + let mut client = + mavlink::connect_async("tcpout:127.0.0.1:14551").await.expect("Couldn't create client"); + + #[cfg(feature = "signing")] + client.setup_signing(Some(singing_cfg_client)); + + for _i in 0..RECEIVE_CHECK_COUNT { + client.send_default(&msg).await.ok(); + } + }); + + server_thread.await.unwrap(); + } +} diff --git a/mavlink/tests/udp_loopback_async_tests.rs b/mavlink/tests/udp_loopback_async_tests.rs new file mode 100644 index 0000000000..3d6b46d236 --- /dev/null +++ b/mavlink/tests/udp_loopback_async_tests.rs @@ -0,0 +1,46 @@ +mod test_shared; + +#[cfg(all(feature = "tokio-1", feature = "udp", feature = "common"))] +mod test_udp_connections { + + /// Test whether we can send a message via UDP and receive it OK using async_connect + #[tokio::test] + pub async fn test_udp_loopback() { + const RECEIVE_CHECK_COUNT: i32 = 3; + + let server = mavlink::connect_async("udpin:0.0.0.0:14552").await.expect("Couldn't create server"); + + // have the client send one heartbeat per second + tokio::spawn({ + async move { + let msg = + mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); + let client = + mavlink::connect_async("udpout:127.0.0.1:14552").await.expect("Couldn't create client"); + loop { + client.send_default(&msg).await.ok(); + } + } + }); + + //TODO use std::sync::WaitTimeoutResult to timeout ourselves if recv fails? + let mut recv_count = 0; + for _i in 0..RECEIVE_CHECK_COUNT { + match server.recv().await { + Ok((_header, msg)) => { + if let mavlink::common::MavMessage::HEARTBEAT(_heartbeat_msg) = msg { + recv_count += 1; + } else { + // one message parse failure fails the test + break; + } + } + Err(..) => { + // one message read failure fails the test + break; + } + } + } + assert_eq!(recv_count, RECEIVE_CHECK_COUNT); + } +} From 00897019ff77f38c96c83999510bfac5c0d13166 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 1 Sep 2024 16:16:32 +0200 Subject: [PATCH 151/159] fix: read_v2_msg_async_inner, write_versioned_msg_async_signed doc: update for async fix: remove unused serial code from async connection --- mavlink-core/src/async_connection/mod.rs | 20 ++++---------------- mavlink-core/src/lib.rs | 4 ++-- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index 6ef4cc156a..ed727ec93b 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -13,12 +13,12 @@ mod file; #[cfg(feature = "signing")] use crate::SigningConfig; -/// A MAVLink connection +/// An async MAVLink connection #[async_trait::async_trait] pub trait AsyncMavConnection { /// Receive a mavlink message. /// - /// Blocks until a valid frame is received, ignoring invalid messages. + /// Wait until a valid frame is received, ignoring invalid messages. async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; /// Send a mavlink message @@ -61,7 +61,7 @@ pub trait AsyncMavConnection { fn setup_signing(&mut self, signing_data: Option); } -/// Connect to a MAVLink node by address string. +/// Connect asynchronously to a MAVLink node by address string. /// /// The address must be in one of the following formats: /// @@ -70,12 +70,11 @@ pub trait AsyncMavConnection { /// * `udpin::` to create a UDP server, listening for incoming packets /// * `udpout::` to create a UDP client /// * `udpbcast::` to create a UDP broadcast -/// * NOT `serial::` to create a serial connection /// * `file:` to extract file data /// +/// Serial is currently not supported for async connections, use [`crate::connect`] instead. /// The type of the connection is determined at runtime based on the address type, so the /// connection is returned as a trait object. -// TODO only reason this has to be send is udp serve pub async fn connect_async( address: &str, ) -> io::Result + Sync + Send>> { @@ -102,16 +101,6 @@ pub async fn connect_async( { protocol_err } - } else if cfg!(feature = "direct-serial") && address.starts_with("serial:") { - #[cfg(feature = "direct-serial")] - { - todo!() - //Ok(Box::new(direct_serial::open(&address["serial:".len()..])?)) - } - #[cfg(not(feature = "direct-serial"))] - { - protocol_err - } } else if address.starts_with("file") { Ok(Box::new(file::open(&address["file:".len()..]).await?)) } else { @@ -119,7 +108,6 @@ pub async fn connect_async( } } -// TODO remove this ? /// Returns the socket address for the given address. pub(crate) fn get_socket_addr( address: T, diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index d68fe1d214..7a3341950d 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -1133,7 +1133,7 @@ async fn read_v2_msg_async_inner read: &mut AsyncPeekReader, signing_data: Option<&SigningData>, ) -> Result<(MavHeader, M), error::MessageReadError> { - let message = read_v2_raw_message_async_signed::(read, signing_data).await?; + let message = read_v2_raw_message_async_inner::(read, signing_data).await?; Ok(( MavHeader { @@ -1212,7 +1212,7 @@ pub async fn write_versioned_msg_async( w: &mut W, version: MavlinkVersion, From f81f0b4118f073ced6270551ef37dfd7b75921b3 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 1 Sep 2024 16:17:44 +0200 Subject: [PATCH 152/159] fix: fmt --- mavlink/tests/tcp_loopback_async_tests.rs | 10 ++++++---- mavlink/tests/udp_loopback_async_tests.rs | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/mavlink/tests/tcp_loopback_async_tests.rs b/mavlink/tests/tcp_loopback_async_tests.rs index edd56ba595..860ce0b10e 100644 --- a/mavlink/tests/tcp_loopback_async_tests.rs +++ b/mavlink/tests/tcp_loopback_async_tests.rs @@ -20,8 +20,9 @@ mod test_tcp_connections { let server_thread = tokio::spawn(async move { //TODO consider using get_available_port to use a random port - let mut server = - mavlink::connect_async("tcpin:0.0.0.0:14551").await.expect("Couldn't create server"); + let mut server = mavlink::connect_async("tcpin:0.0.0.0:14551") + .await + .expect("Couldn't create server"); #[cfg(feature = "signing")] server.setup_signing(Some(singing_cfg_server)); @@ -53,8 +54,9 @@ mod test_tcp_connections { tokio::spawn(async move { let msg = mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); - let mut client = - mavlink::connect_async("tcpout:127.0.0.1:14551").await.expect("Couldn't create client"); + let mut client = mavlink::connect_async("tcpout:127.0.0.1:14551") + .await + .expect("Couldn't create client"); #[cfg(feature = "signing")] client.setup_signing(Some(singing_cfg_client)); diff --git a/mavlink/tests/udp_loopback_async_tests.rs b/mavlink/tests/udp_loopback_async_tests.rs index 3d6b46d236..4b3934523e 100644 --- a/mavlink/tests/udp_loopback_async_tests.rs +++ b/mavlink/tests/udp_loopback_async_tests.rs @@ -8,15 +8,18 @@ mod test_udp_connections { pub async fn test_udp_loopback() { const RECEIVE_CHECK_COUNT: i32 = 3; - let server = mavlink::connect_async("udpin:0.0.0.0:14552").await.expect("Couldn't create server"); + let server = mavlink::connect_async("udpin:0.0.0.0:14552") + .await + .expect("Couldn't create server"); // have the client send one heartbeat per second tokio::spawn({ async move { let msg = mavlink::common::MavMessage::HEARTBEAT(crate::test_shared::get_heartbeat_msg()); - let client = - mavlink::connect_async("udpout:127.0.0.1:14552").await.expect("Couldn't create client"); + let client = mavlink::connect_async("udpout:127.0.0.1:14552") + .await + .expect("Couldn't create client"); loop { client.send_default(&msg).await.ok(); } From 68536ef405005dacaf51ef59fbe67d87fb33535e Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 4 Sep 2024 11:57:36 +0200 Subject: [PATCH 153/159] fix: add Send marker to AsyncMavConnection for rust 1.70 --- mavlink-core/src/async_connection/file.rs | 2 +- mavlink-core/src/async_connection/mod.rs | 2 +- mavlink-core/src/async_connection/tcp.rs | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mavlink-core/src/async_connection/file.rs b/mavlink-core/src/async_connection/file.rs index 9b1c90af3a..852273649f 100644 --- a/mavlink-core/src/async_connection/file.rs +++ b/mavlink-core/src/async_connection/file.rs @@ -36,7 +36,7 @@ pub struct AsyncFileConnection { } #[async_trait::async_trait] -impl AsyncMavConnection for AsyncFileConnection { +impl AsyncMavConnection for AsyncFileConnection { async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut file = self.file.lock().await; diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index ed727ec93b..a708a67328 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -15,7 +15,7 @@ use crate::SigningConfig; /// An async MAVLink connection #[async_trait::async_trait] -pub trait AsyncMavConnection { +pub trait AsyncMavConnection { /// Receive a mavlink message. /// /// Wait until a valid frame is received, ignoring invalid messages. diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs index e05d70455b..e8171c7961 100644 --- a/mavlink-core/src/async_connection/tcp.rs +++ b/mavlink-core/src/async_connection/tcp.rs @@ -17,7 +17,7 @@ use crate::{ /// TCP MAVLink connection -pub async fn select_protocol( +pub async fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { let connection = if let Some(address) = address.strip_prefix("tcpout:") { @@ -97,7 +97,7 @@ struct TcpWrite { } #[async_trait::async_trait] -impl AsyncMavConnection for TcpConnection { +impl AsyncMavConnection for TcpConnection { async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut reader = self.reader.lock().await; #[cfg(not(feature = "signing"))] From 38e85b6ababe95ba209df33aa3f5977a18e3e371 Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 8 Sep 2024 14:18:24 +0200 Subject: [PATCH 154/159] doc: replace block with yield, move file comment, fmt cargo --- mavlink-core/Cargo.toml | 4 ++-- mavlink-core/src/async_connection/file.rs | 4 ++-- mavlink-core/src/async_connection/mod.rs | 2 +- mavlink-core/src/async_connection/tcp.rs | 6 +++--- mavlink-core/src/async_connection/udp.rs | 2 ++ mavlink-core/src/lib.rs | 1 - 6 files changed, 10 insertions(+), 9 deletions(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index eb749b07c4..5bf97b903c 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -26,7 +26,7 @@ embedded-io-async = { version = "0.6.1", optional = true } serde = { version = "1.0.115", optional = true, features = ["derive"] } serde_arrays = { version = "0.1.0", optional = true } serial = { version = "0.4", optional = true } -tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs", ], optional = true } +tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs"], optional = true } sha2 = { version = "0.10", optional = true } async-trait = { version = "0.1.18", optional = true } @@ -46,4 +46,4 @@ async-trait = { version = "0.1.18", optional = true } default = ["std", "tcp", "udp", "direct-serial", "serde"] [dev-dependencies] -tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs","macros", "rt" ] } \ No newline at end of file +tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs", "macros", "rt"] } \ No newline at end of file diff --git a/mavlink-core/src/async_connection/file.rs b/mavlink-core/src/async_connection/file.rs index 852273649f..5dae56d99e 100644 --- a/mavlink-core/src/async_connection/file.rs +++ b/mavlink-core/src/async_connection/file.rs @@ -1,3 +1,5 @@ +//! Async File MAVLINK connection + use core::ops::DerefMut; use super::AsyncMavConnection; @@ -15,8 +17,6 @@ use crate::read_versioned_msg_async; #[cfg(feature = "signing")] use crate::{read_versioned_msg_async_signed, SigningConfig, SigningData}; -/// File MAVLINK connection - pub async fn open(file_path: &str) -> io::Result { let file = File::open(file_path).await?; Ok(AsyncFileConnection { diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index a708a67328..35e2fa0e78 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -18,7 +18,7 @@ use crate::SigningConfig; pub trait AsyncMavConnection { /// Receive a mavlink message. /// - /// Wait until a valid frame is received, ignoring invalid messages. + /// Yield until a valid frame is received, ignoring invalid messages. async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError>; /// Send a mavlink message diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs index e8171c7961..b0307d377d 100644 --- a/mavlink-core/src/async_connection/tcp.rs +++ b/mavlink-core/src/async_connection/tcp.rs @@ -1,3 +1,5 @@ +//! Async TCP MAVLink connection + use super::{get_socket_addr, AsyncMavConnection}; use crate::async_peek_reader::AsyncPeekReader; use crate::{MavHeader, MavlinkVersion, Message}; @@ -15,8 +17,6 @@ use crate::{ read_versioned_msg_async_signed, write_versioned_msg_async_signed, SigningConfig, SigningData, }; -/// TCP MAVLink connection - pub async fn select_protocol( address: &str, ) -> io::Result + Sync + Send>> { @@ -57,7 +57,7 @@ pub async fn tcpin(address: T) -> io::Result { let (reader, writer) = socket.into_split(); diff --git a/mavlink-core/src/async_connection/udp.rs b/mavlink-core/src/async_connection/udp.rs index cd945e8055..c3082d0edb 100644 --- a/mavlink-core/src/async_connection/udp.rs +++ b/mavlink-core/src/async_connection/udp.rs @@ -1,3 +1,5 @@ +//! Async UDP MAVLink connection + use core::{ops::DerefMut, task::Poll}; use std::{collections::VecDeque, io::Read, sync::Arc}; diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 7a3341950d..8af70ad89b 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -958,7 +958,6 @@ fn read_v2_raw_message_inner( } /// Async read a raw buffer with the mavlink message -/// /// V2 maximum size is 280 bytes: `` #[cfg(feature = "tokio-1")] pub async fn read_v2_raw_message_async( From fbd22390066f008218ebcc618e112e5167d29a1c Mon Sep 17 00:00:00 2001 From: pv42 Date: Sun, 8 Sep 2024 14:30:23 +0200 Subject: [PATCH 155/159] fix: rename tcp/udp connection to AsyncTcpConnection/AsyncUdpConnection, remove unneeded tokio:: prefixes --- mavlink-core/src/async_connection/tcp.rs | 12 +++++----- mavlink-core/src/async_connection/udp.rs | 30 ++++++++++++------------ 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/mavlink-core/src/async_connection/tcp.rs b/mavlink-core/src/async_connection/tcp.rs index b0307d377d..4cd1f9ac9a 100644 --- a/mavlink-core/src/async_connection/tcp.rs +++ b/mavlink-core/src/async_connection/tcp.rs @@ -34,14 +34,14 @@ pub async fn select_protocol( Ok(Box::new(connection?)) } -pub async fn tcpout(address: T) -> io::Result { +pub async fn tcpout(address: T) -> io::Result { let addr = get_socket_addr(address)?; let socket = TcpStream::connect(addr).await?; let (reader, writer) = socket.into_split(); - Ok(TcpConnection { + Ok(AsyncTcpConnection { reader: Mutex::new(AsyncPeekReader::new(reader)), writer: Mutex::new(TcpWrite { socket: writer, @@ -53,7 +53,7 @@ pub async fn tcpout(address: T) -> io::Result(address: T) -> io::Result { +pub async fn tcpin(address: T) -> io::Result { let addr = get_socket_addr(address)?; let listener = TcpListener::bind(addr).await?; @@ -61,7 +61,7 @@ pub async fn tcpin(address: T) -> io::Result { let (reader, writer) = socket.into_split(); - return Ok(TcpConnection { + return Ok(AsyncTcpConnection { reader: Mutex::new(AsyncPeekReader::new(reader)), writer: Mutex::new(TcpWrite { socket: writer, @@ -83,7 +83,7 @@ pub async fn tcpin(address: T) -> io::Result>, writer: Mutex, protocol_version: MavlinkVersion, @@ -97,7 +97,7 @@ struct TcpWrite { } #[async_trait::async_trait] -impl AsyncMavConnection for TcpConnection { +impl AsyncMavConnection for AsyncTcpConnection { async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut reader = self.reader.lock().await; #[cfg(not(feature = "signing"))] diff --git a/mavlink-core/src/async_connection/udp.rs b/mavlink-core/src/async_connection/udp.rs index c3082d0edb..3f06b83e1d 100644 --- a/mavlink-core/src/async_connection/udp.rs +++ b/mavlink-core/src/async_connection/udp.rs @@ -22,7 +22,7 @@ use crate::{ pub async fn select_protocol( address: &str, -) -> tokio::io::Result + Sync + Send>> { +) -> io::Result + Sync + Send>> { let connection = if let Some(address) = address.strip_prefix("udpin:") { udpin(address).await } else if let Some(address) = address.strip_prefix("udpout:") { @@ -30,8 +30,8 @@ pub async fn select_protocol( } else if let Some(address) = address.strip_prefix("udpbcast:") { udpbcast(address).await } else { - Err(tokio::io::Error::new( - tokio::io::ErrorKind::AddrNotAvailable, + Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, "Protocol unsupported", )) }; @@ -39,29 +39,29 @@ pub async fn select_protocol( Ok(Box::new(connection?)) } -pub async fn udpbcast(address: T) -> tokio::io::Result { +pub async fn udpbcast(address: T) -> io::Result { let addr = get_socket_addr(address)?; let socket = UdpSocket::bind("0.0.0.0:0").await?; socket .set_broadcast(true) .expect("Couldn't bind to broadcast address."); - UdpConnection::new(socket, false, Some(addr)) + AsyncUdpConnection::new(socket, false, Some(addr)) } -pub async fn udpout(address: T) -> tokio::io::Result { +pub async fn udpout(address: T) -> io::Result { let addr = get_socket_addr(address)?; let socket = UdpSocket::bind("0.0.0.0:0").await?; - UdpConnection::new(socket, false, Some(addr)) + AsyncUdpConnection::new(socket, false, Some(addr)) } -pub async fn udpin(address: T) -> tokio::io::Result { +pub async fn udpin(address: T) -> io::Result { let addr = address .to_socket_addrs() .unwrap() .next() .expect("Invalid address"); let socket = UdpSocket::bind(addr).await?; - UdpConnection::new(socket, true, None) + AsyncUdpConnection::new(socket, true, None) } struct UdpRead { @@ -75,7 +75,7 @@ impl AsyncRead for UdpRead { fn poll_read( mut self: core::pin::Pin<&mut Self>, cx: &mut core::task::Context<'_>, - buf: &mut tokio::io::ReadBuf<'_>, + buf: &mut io::ReadBuf<'_>, ) -> Poll> { if self.buffer.is_empty() { let mut read_buffer = [0u8; MTU_SIZE]; @@ -115,7 +115,7 @@ struct UdpWrite { sequence: u8, } -pub struct UdpConnection { +pub struct AsyncUdpConnection { reader: Mutex>, writer: Mutex, protocol_version: MavlinkVersion, @@ -124,12 +124,12 @@ pub struct UdpConnection { signing_data: Option, } -impl UdpConnection { +impl AsyncUdpConnection { fn new( socket: UdpSocket, server: bool, dest: Option, - ) -> tokio::io::Result { + ) -> io::Result { let socket = Arc::new(socket); Ok(Self { server, @@ -151,7 +151,7 @@ impl UdpConnection { } #[async_trait::async_trait] -impl AsyncMavConnection for UdpConnection { +impl AsyncMavConnection for AsyncUdpConnection { async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { let mut reader = self.reader.lock().await; @@ -238,7 +238,7 @@ impl AsyncMavConnection for UdpConnection { #[cfg(test)] mod tests { use super::*; - use tokio::io::AsyncReadExt; + use io::AsyncReadExt; #[tokio::test] async fn test_datagram_buffering() { From 5f2ecbe856a02e7bd17f43de63c8136db7947715 Mon Sep 17 00:00:00 2001 From: danieleades <33452915+danieleades@users.noreply.github.com> Date: Fri, 20 Sep 2024 14:53:46 +0100 Subject: [PATCH 156/159] bump pinned clippy version --- .github/workflows/test.yml | 2 +- mavlink-core/src/utils.rs | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index d3f2ef49c7..1eb228de28 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -19,7 +19,7 @@ jobs: - uses: actions/checkout@master - uses: dtolnay/rust-toolchain@master with: - toolchain: nightly-2023-10-21 + toolchain: nightly-2024-09-10 components: clippy - uses: actions-rs-plus/clippy-check@v2 with: diff --git a/mavlink-core/src/utils.rs b/mavlink-core/src/utils.rs index efefcbadb8..b8126360ec 100644 --- a/mavlink-core/src/utils.rs +++ b/mavlink-core/src/utils.rs @@ -19,7 +19,9 @@ pub fn remove_trailing_zeroes(data: &[u8]) -> usize { } /// A trait very similar to `Default` but is only implemented for the equivalent Rust types to -/// `MavType`s. This is only needed because rust doesn't currently implement `Default` for arrays +/// `MavType`s. +/// +/// This is only needed because rust doesn't currently implement `Default` for arrays /// of all sizes. In particular this trait is only ever used when the "serde" feature is enabled. /// For more information, check out [this issue](https://users.rust-lang.org/t/issue-for-derives-for-arrays-greater-than-size-32/59055/3). pub trait RustDefault: Copy { From 0e936c5d7b0a8f99a6fff5971e8575c6463e8181 Mon Sep 17 00:00:00 2001 From: roby2014 Date: Sat, 21 Sep 2024 13:37:02 +0100 Subject: [PATCH 157/159] feat: async serial --- mavlink-core/Cargo.toml | 3 +- .../src/async_connection/direct_serial.rs | 120 ++++++++++++++++++ mavlink-core/src/async_connection/mod.rs | 14 +- 3 files changed, 135 insertions(+), 2 deletions(-) create mode 100644 mavlink-core/src/async_connection/direct_serial.rs diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 5bf97b903c..75f4f449f5 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -29,6 +29,7 @@ serial = { version = "0.4", optional = true } tokio = { version = "1.0", default-features = false, features = ["io-util", "net", "sync", "fs"], optional = true } sha2 = { version = "0.10", optional = true } async-trait = { version = "0.1.18", optional = true } +tokio-serial = { version = "5.4.4", default-features = false, optional = true } [features] "std" = ["byteorder/std"] @@ -41,7 +42,7 @@ async-trait = { version = "0.1.18", optional = true } "embedded" = ["dep:embedded-io", "dep:embedded-io-async"] "embedded-hal-02" = ["dep:nb", "dep:embedded-hal-02"] "serde" = ["dep:serde", "dep:serde_arrays"] -"tokio-1" = ["dep:tokio", "dep:async-trait"] +"tokio-1" = ["dep:tokio", "dep:async-trait", "dep:tokio-serial"] "signing" = ["dep:sha2"] default = ["std", "tcp", "udp", "direct-serial", "serde"] diff --git a/mavlink-core/src/async_connection/direct_serial.rs b/mavlink-core/src/async_connection/direct_serial.rs new file mode 100644 index 0000000000..d9b2fe1de2 --- /dev/null +++ b/mavlink-core/src/async_connection/direct_serial.rs @@ -0,0 +1,120 @@ +//! Async Serial MAVLINK connection + +use core::ops::DerefMut; +use std::io; + +use tokio::sync::Mutex; +use tokio_serial::{SerialPort, SerialPortBuilderExt, SerialStream}; + +use crate::{async_peek_reader::AsyncPeekReader, MavHeader, MavlinkVersion, Message}; + +#[cfg(not(feature = "signing"))] +use crate::{read_versioned_msg_async, write_versioned_msg_async}; +#[cfg(feature = "signing")] +use crate::{ + read_versioned_msg_async_signed, write_versioned_msg_async_signed, SigningConfig, SigningData, +}; + +use super::AsyncMavConnection; + +pub fn open(settings: &str) -> io::Result { + let settings_toks: Vec<&str> = settings.split(':').collect(); + if settings_toks.len() < 2 { + return Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Incomplete port settings", + )); + } + + let Ok(baud) = settings_toks[1].parse::() else { + return Err(io::Error::new( + io::ErrorKind::AddrNotAvailable, + "Invalid baud rate", + )); + }; + + let port_name = settings_toks[0]; + let mut port = tokio_serial::new(port_name, baud).open_native_async()?; + port.set_data_bits(tokio_serial::DataBits::Eight)?; + port.set_parity(tokio_serial::Parity::None)?; + port.set_stop_bits(tokio_serial::StopBits::One)?; + port.set_flow_control(tokio_serial::FlowControl::None)?; + + Ok(AsyncSerialConnection { + port: Mutex::new(AsyncPeekReader::new(port)), + sequence: Mutex::new(0), + protocol_version: MavlinkVersion::V2, + #[cfg(feature = "signing")] + signing_data: None, + }) +} + +pub struct AsyncSerialConnection { + port: Mutex>, + sequence: Mutex, + protocol_version: MavlinkVersion, + #[cfg(feature = "signing")] + signing_data: Option, +} + +#[async_trait::async_trait] +impl AsyncMavConnection for AsyncSerialConnection { + async fn recv(&self) -> Result<(MavHeader, M), crate::error::MessageReadError> { + let mut port = self.port.lock().await; + + #[cfg(not(feature = "signing"))] + let result = read_versioned_msg_async(port.deref_mut(), self.protocol_version).await; + #[cfg(feature = "signing")] + let result = read_versioned_msg_async_signed( + port.deref_mut(), + self.protocol_version, + self.signing_data.as_ref(), + ) + .await; + result + } + + async fn send( + &self, + header: &MavHeader, + data: &M, + ) -> Result { + let mut port = self.port.lock().await; + let mut sequence = self.sequence.lock().await; + + let header = MavHeader { + sequence: *sequence, + system_id: header.system_id, + component_id: header.component_id, + }; + + *sequence = sequence.wrapping_add(1); + + #[cfg(not(feature = "signing"))] + let result = + write_versioned_msg_async(port.reader_mut(), self.protocol_version, header, data).await; + #[cfg(feature = "signing")] + let result = write_versioned_msg_async_signed( + port.reader_mut(), + self.protocol_version, + header, + data, + self.signing_data.as_ref(), + ) + .await; + result + } + + fn set_protocol_version(&mut self, version: MavlinkVersion) { + self.protocol_version = version; + } + + fn get_protocol_version(&self) -> MavlinkVersion { + self.protocol_version + } + + #[cfg(feature = "signing")] + fn setup_signing(&mut self, signing_data: Option) { + self.signing_data = signing_data.map(SigningData::from_config) + } +} diff --git a/mavlink-core/src/async_connection/mod.rs b/mavlink-core/src/async_connection/mod.rs index 35e2fa0e78..b2eb5c6757 100644 --- a/mavlink-core/src/async_connection/mod.rs +++ b/mavlink-core/src/async_connection/mod.rs @@ -8,6 +8,9 @@ mod tcp; #[cfg(feature = "udp")] mod udp; +#[cfg(feature = "direct-serial")] +mod direct_serial; + mod file; #[cfg(feature = "signing")] @@ -70,9 +73,9 @@ pub trait AsyncMavConnection { /// * `udpin::` to create a UDP server, listening for incoming packets /// * `udpout::` to create a UDP client /// * `udpbcast::` to create a UDP broadcast +/// * `serial::` to create a serial connection /// * `file:` to extract file data /// -/// Serial is currently not supported for async connections, use [`crate::connect`] instead. /// The type of the connection is determined at runtime based on the address type, so the /// connection is returned as a trait object. pub async fn connect_async( @@ -101,6 +104,15 @@ pub async fn connect_async( { protocol_err } + } else if cfg!(feature = "direct-serial") && address.starts_with("serial") { + #[cfg(feature = "direct-serial")] + { + Ok(Box::new(direct_serial::open(&address["serial:".len()..])?)) + } + #[cfg(not(feature = "direct-serial"))] + { + protocol_err + } } else if address.starts_with("file") { Ok(Box::new(file::open(&address["file:".len()..]).await?)) } else { From 90d2ad40914e9b4a4ab2b5e88303b84d39b1649c Mon Sep 17 00:00:00 2001 From: roby2014 Date: Mon, 23 Sep 2024 09:36:13 +0100 Subject: [PATCH 158/159] cargo: update to 0.14.0 --- mavlink-core/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/Cargo.toml b/mavlink-core/Cargo.toml index 75f4f449f5..413572b61f 100644 --- a/mavlink-core/Cargo.toml +++ b/mavlink-core/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "mavlink-core" -version = "0.13.2" +version = "0.14.0" authors = [ "Todd Stellanova", "Michal Podhradsky", From c80321bd78c95ae03ece4a6450c0858c9d985540 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jo=C3=A3o=20Ant=C3=B4nio=20Cardoso?= Date: Mon, 14 Oct 2024 17:34:04 -0300 Subject: [PATCH 159/159] mavlink-core: MAVLinkV1MessageRaw::header should not require mut --- mavlink-core/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 8af70ad89b..2b5626a263 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -325,7 +325,7 @@ impl MAVLinkV1MessageRaw { } #[inline] - pub fn header(&mut self) -> &[u8] { + pub fn header(&self) -> &[u8] { &self.0[1..=Self::HEADER_SIZE] }