From fd23f9153b9ff1a4a686770396453e36dc0666e3 Mon Sep 17 00:00:00 2001 From: Guillaume Binet Date: Sun, 10 Sep 2023 13:16:55 -0500 Subject: [PATCH 1/2] String parsing for char[] This allows messages like STATUSTEXT to parse nicely as an ArrayString in Rust. --- Cargo.toml | 2 ++ build/parser.rs | 38 ++++++++++++++++++++++++++++++-- src/lib.rs | 6 ++--- tests/v2_encode_decode_tests.rs | 39 +++++++++++++++++++++++++++++++++ 4 files changed, 80 insertions(+), 5 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 4f61729579..61baa3f13b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -34,6 +34,8 @@ 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 } +log = "*" +arrayvec = { version = "0.7.4", features = ["serde"] } [features] "all" = [ diff --git a/build/parser.rs b/build/parser.rs index e797e3882f..99474b99e5 100644 --- a/build/parser.rs +++ b/build/parser.rs @@ -461,7 +461,7 @@ 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 { - if field.enumtype.is_some() { + if field.enumtype.is_some() || matches!(field.mavtype, MavType::String(_)) { quote!(#[cfg_attr(feature = "serde", serde(default))]) } else { quote!(#[cfg_attr(feature = "serde", serde(default = "crate::RustDefault::rust_default"))]) @@ -770,6 +770,7 @@ pub enum MavType { Char, Float, Double, + String(usize), Array(Box, usize), } @@ -801,7 +802,11 @@ impl MavType { let start = s.find('[')?; let size = s[start + 1..(s.len() - 1)].parse::().ok()?; let mtype = Self::parse_type(&s[0..start])?; - Some(Array(Box::new(mtype), size)) + if mtype == Char { + Some(String(size)) + } else { + Some(Array(Box::new(mtype), size)) + } } else { None } @@ -825,6 +830,18 @@ impl MavType { Int64 => quote! {#val = #buf.get_i64_le();}, Float => quote! {#val = #buf.get_f32_le();}, Double => quote! {#val = #buf.get_f64_le();}, + String(length) => { + let r = Char.rust_reader("e!(let next_char), buf); + quote! { + for _ in 0..#length { + #r + if next_char == 0 { + break; + } + #val.push(next_char as char); + } + } + } Array(t, _) => { let r = t.rust_reader("e!(let val), buf); quote! { @@ -853,6 +870,15 @@ impl MavType { UInt64 => quote! {#buf.put_u64_le(#val);}, Int64 => quote! {#buf.put_i64_le(#val);}, Double => quote! {#buf.put_f64_le(#val);}, + String(_size) => { + let w = Char.rust_writer("e!(*val), buf); + quote! { + let slice = #val.as_bytes(); + for val in slice { + #w + } + } + } Array(t, _size) => { let w = t.rust_writer("e!(*val), buf); quote! { @@ -872,6 +898,7 @@ impl MavType { UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, UInt64 | Int64 | Double => 8, + String(size) => Char.len() * size, Array(t, size) => t.len() * size, } } @@ -884,6 +911,7 @@ impl MavType { UInt16 | Int16 => 2, UInt32 | Int32 | Float => 4, UInt64 | Int64 | Double => 8, + String(_) => Char.len(), Array(t, _) => t.len(), } } @@ -904,6 +932,7 @@ impl MavType { UInt64 => "uint64_t".into(), Int64 => "int64_t".into(), Double => "double".into(), + String(_) => "char".into(), Array(t, _) => t.primitive_type(), } } @@ -924,6 +953,7 @@ impl MavType { UInt64 => "u64".into(), Int64 => "i64".into(), Double => "f64".into(), + String(size) => format!("arrayvec::ArrayString<{}>", size), Array(t, size) => format!("[{};{}]", t.rust_type(), size), } } @@ -943,6 +973,7 @@ impl MavType { UInt64 => quote!(0_u64), Int64 => quote!(0_i64), Double => quote!(0.0_f64), + String(size) => quote!(arrayvec::ArrayString::<#size>::new_const()), Array(ty, size) => { let default_value = ty.emit_default_value(); quote!([#default_value; #size]) @@ -1397,6 +1428,9 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { crc.digest(field.name.as_bytes()); } crc.digest(" ".as_bytes()); + if let MavType::String(size) = field.mavtype { + crc.digest(&[size as u8]); + } if let MavType::Array(_, size) = field.mavtype { crc.digest(&[size as u8]); } diff --git a/src/lib.rs b/src/lib.rs index 248ea9c1e4..143a4b09ea 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -43,8 +43,9 @@ use utils::{remove_trailing_zeroes, RustDefault}; use serde::{Deserialize, Serialize}; use crate::{bytes::Bytes, error::ParserError}; - use crc_any::CRCu16; +use log::debug; +use log::error; // include generate definitions include!(concat!(env!("OUT_DIR"), "/mod.rs")); @@ -600,7 +601,6 @@ pub fn read_v2_raw_message( message.0[0] = MAV_STX_V2; reader.read_exact(message.mut_header())?; reader.read_exact(message.mut_payload_and_checksum_and_sign())?; - Ok(message) } @@ -611,7 +611,7 @@ pub fn read_v2_msg( loop { let message = read_v2_raw_message(read)?; if !message.has_valid_crc::() { - // bad crc: ignore message + error!("Bad CRC for message: {:?}", message); continue; } diff --git a/tests/v2_encode_decode_tests.rs b/tests/v2_encode_decode_tests.rs index db254607ab..9ada9ea696 100644 --- a/tests/v2_encode_decode_tests.rs +++ b/tests/v2_encode_decode_tests.rs @@ -60,6 +60,45 @@ mod test_v2_encode_decode { assert_eq!(&v[..], HEARTBEAT_V2); } + pub const STATUSTEXT_V2: &[u8] = &[ + mavlink::MAV_STX_V2, + 0x06, // payload is 6 bytes.`` + 0x00, + 0x00, + 0x05, + 0x2a, + 0x04, + 0xfd, // This is STATUSTEXT + 0x00, + 0x00, + 0x02, // Severity + 0x79, // "y" + 0x6f, // "o" + 0x75, // "u" + 0x70, // "p" + 0x69, // "i" + 0x49, // CRC + 0x00, // CRC + ]; + + /// It is in the V2 tests because of the trail of 0s that gets truncated at the end. + #[test] + pub fn test_read_string() { + let mut r = STATUSTEXT_V2; + let (_header, recv_msg) = + mavlink::read_v2_msg(&mut r).expect("Failed to parse COMMAND_LONG_TRUNCATED_V2"); + + if let mavlink::common::MavMessage::STATUSTEXT(recv_msg) = recv_msg { + assert_eq!( + recv_msg.severity, + mavlink::common::MavSeverity::MAV_SEVERITY_CRITICAL + ); + assert_eq!(recv_msg.text.as_str(), "youpi"); + } else { + panic!("Decoded wrong message type") + } + } + /// A COMMAND_LONG message with a truncated payload (allowed for empty fields) pub const COMMAND_LONG_TRUNCATED_V2: &[u8] = &[ mavlink::MAV_STX_V2, From 90f31da95014ba8450d3db7782f5e0864817756e Mon Sep 17 00:00:00 2001 From: Guillaume Binet Date: Sun, 10 Sep 2023 13:31:20 -0500 Subject: [PATCH 2/2] remove log dep --- Cargo.toml | 1 - src/lib.rs | 3 --- 2 files changed, 4 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 61baa3f13b..45cf39c7d5 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -34,7 +34,6 @@ 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 } -log = "*" arrayvec = { version = "0.7.4", features = ["serde"] } [features] diff --git a/src/lib.rs b/src/lib.rs index 143a4b09ea..d6fb63d869 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -44,8 +44,6 @@ use serde::{Deserialize, Serialize}; use crate::{bytes::Bytes, error::ParserError}; use crc_any::CRCu16; -use log::debug; -use log::error; // include generate definitions include!(concat!(env!("OUT_DIR"), "/mod.rs")); @@ -611,7 +609,6 @@ pub fn read_v2_msg( loop { let message = read_v2_raw_message(read)?; if !message.has_valid_crc::() { - error!("Bad CRC for message: {:?}", message); continue; }