From e83fa0f1a32ececfad358456591dbcb87a8ef14d Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 11 Mar 2026 16:24:13 +0100 Subject: [PATCH 1/5] refactor xml parser - splitt xml parser and code generator in 2 files - compleatly rewrite xml parser for better readebility and robustness - add proper error handling for parser errors - fix superseed not generating the note properly - add check if used enums exist - enum entries are now sorted by value instead of depth first include order - unknown tags cause an error --- mavlink-bindgen/src/error.rs | 56 ++ mavlink-bindgen/src/lib.rs | 1 + mavlink-bindgen/src/parser.rs | 841 +----------------- mavlink-bindgen/src/parser_new.rs | 835 +++++++++++++++++ .../tests/definitions/superseded.xml | 14 + ...apshots__superseded.xml@superseded.rs.snap | 46 +- 6 files changed, 989 insertions(+), 804 deletions(-) create mode 100644 mavlink-bindgen/src/parser_new.rs diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs index f972764614b..155839f70fd 100644 --- a/mavlink-bindgen/src/error.rs +++ b/mavlink-bindgen/src/error.rs @@ -1,5 +1,10 @@ +use std::num::{ParseFloatError, ParseIntError}; + +use quick_xml::events::Event; use thiserror::Error; +use crate::parser_new::MavXmlElement; + #[derive(Error, Debug)] pub enum BindGenError { /// Represents a failure to read the MAVLink definitions directory. @@ -26,4 +31,55 @@ pub enum BindGenError { source: std::io::Error, dest_path: std::path::PathBuf, }, + /// Occurs when a MAVLink XML definition that is not valid + #[error("MAVLink definition file {} is invalid: {source}", path.display())] + InvalidDefinitionFile { + source: MavXMLParseError, + path: std::path::PathBuf, + }, + /// Occurs when a MAVLink XML definition refences an enum that is not defined in it or its includes + #[error("MAVLink definition file {} references undefined enum {enum_name}", path.display())] + InvalidEnumReference { + enum_name: String, + path: std::path::PathBuf, + }, + #[error("bitflag enum field {field_name} of {message_name} must be able to fit all possible values for {enum_name}")] + BitflagEnumTypeToNarrow { + field_name: String, + message_name: String, + enum_name: String, + }, +} + +#[derive(Error, Debug)] +pub enum MavXMLParseError { + #[error("{0}")] + QuickXMLError(#[from] quick_xml::errors::Error), + #[error("{0}")] + ParseIntFailed(#[from] ParseIntError), + #[error("{0}")] + ParseFloatFailed(#[from] ParseFloatError), + #[error("Unexpected XML element {event:?} in {parent:?}")] + UnexpectedEvent { + event: Event<'static>, + parent: MavXmlElement, + }, + #[error("Extentsion tags may not contain elements")] + NoneSelfClosingExtTag, + #[error("Unexpected tag {element:?}")] + UnexpectedElement { element: MavXmlElement }, + #[error("Expected text")] + ExpectedText, + #[error("MAVLink definition file ends unexpectedly")] + UnexpectedEnd, + #[error("Param index must be between 1 and 7")] + ParamIndexOutOfRange, + #[error("Expected start of MAVLink definition")] + ExpectedMAVLink, + #[error("Invalid field type: {mav_type}")] + InvalidType { mav_type: String }, + #[error("Duplicate field '{field}' found in message '{message}' while generating bindings")] + DuplicateFieldName { field: String, message: String }, + #[error("Message '{message}' must have between 1 and 64 fields")] + InvalidFieldCount { message: String }, } diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index ae3fb9205d2..a5219f69766 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -8,6 +8,7 @@ use std::process::Command; pub mod binder; pub mod error; pub mod parser; +mod parser_new; mod util; #[derive(Debug)] diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index d1c5b2e9871..294afdfa109 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -11,15 +11,14 @@ use std::sync::LazyLock; use regex::Regex; -use quick_xml::{events::Event, Reader}; - use proc_macro2::{Ident, TokenStream}; use quote::{format_ident, quote}; #[cfg(feature = "serde")] use serde::{Deserialize, Serialize}; -use crate::error::BindGenError; +use crate::error::{BindGenError, MavXMLParseError}; +use crate::parser_new::parse_profile; use crate::util; static URL_REGEX: LazyLock = LazyLock::new(|| { @@ -42,7 +41,7 @@ pub struct MavProfile { } impl MavProfile { - fn add_message(&mut self, message: &MavMessage) { + pub(crate) fn add_message(&mut self, message: &MavMessage) { match self.messages.entry(message.name.clone()) { Entry::Occupied(entry) => { assert!( @@ -57,10 +56,10 @@ impl MavProfile { } } - fn add_enum(&mut self, enm: &MavEnum) { + pub(crate) fn add_enum(&mut self, enm: &MavEnum) { match self.enums.entry(enm.name.clone()) { Entry::Occupied(entry) => { - entry.into_mut().try_combine(enm); + entry.into_mut().combine(enm); } Entry::Vacant(entry) => { entry.insert(enm.clone()); @@ -68,46 +67,6 @@ impl MavProfile { } } - /// Go over all fields in the messages, and if you encounter an enum, - /// which is a bitmask, set the bitmask size based on field size - fn update_enums(mut self) -> Self { - for msg in self.messages.values_mut() { - for field in &mut msg.fields { - if let Some(enum_name) = &field.enumtype { - // find the corresponding enum - if let Some(enm) = self.enums.get_mut(enum_name) { - // Handle legacy definition where bitmask is defined as display="bitmask" - if field.display == Some("bitmask".to_string()) { - enm.bitmask = true; - } - - // it is a bitmask - if enm.bitmask { - enm.primitive = Some(field.mavtype.rust_primitive_type()); - - // check if all enum values can be stored in the fields - for entry in &enm.entries { - assert!( - entry.value.unwrap_or_default() <= field.mavtype.max_int_value(), - "bitflag enum field {} of {} must be able to fit all possible values for {}", - field.name, - msg.name, - enum_name, - ); - } - - // Fix fields in backwards manner - if field.display.is_none() { - field.display = Some("bitmask".to_string()); - } - } - } - } - } - } - self - } - //TODO verify this is no longer necessary since we're supporting both mavlink1 and mavlink2 // ///If we are not using Mavlink v2, remove messages with id's > 254 // fn update_messages(mut self) -> Self { @@ -540,18 +499,18 @@ impl MavEnum { self.primitive.is_some() } - 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| { - 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 combine(&mut self, enm: &Self) { + for enum_entry in &enm.entries { + if self + .entries + .iter() + .any(|elem| elem.name == enum_entry.name && elem.value == enum_entry.value) + { + panic!("Enum entry {} already exists", enum_entry.name) } } + self.entries.append(&mut enm.entries.clone()); + self.entries.sort_by_key(|entry| entry.value); } fn emit_defs(&self) -> Vec { @@ -1088,31 +1047,29 @@ impl MavMessage { /// Ensures that a message does not contain duplicate field names. /// /// Duplicate field names would generate invalid Rust structs. - fn validate_unique_fields(&self) { + pub(crate) fn validate_unique_fields(&self) -> Result<(), MavXMLParseError> { let mut seen: HashSet<&str> = HashSet::new(); for f in &self.fields { let name: &str = &f.name; - assert!( - seen.insert(name), - "Duplicate field '{}' found in message '{}' while generating bindings", - name, - self.name - ); + if !seen.insert(name) { + return Err(MavXMLParseError::DuplicateFieldName { + field: name.to_string(), + message: self.name.clone(), + }); + } } + Ok(()) } /// Ensure that the fields count is at least one and no more than 64 - fn validate_field_count(&self) { - assert!( - !self.fields.is_empty(), - "Message '{}' does not any fields", - self.name - ); - assert!( - self.fields.len() <= 64, - "Message '{}' has more then 64 fields", - self.name - ); + pub(crate) fn validate_field_count(&self) -> Result<(), MavXMLParseError> { + if self.fields.is_empty() || self.fields.len() >= 64 { + Err(MavXMLParseError::InvalidFieldCount { + message: self.name.clone(), + }) + } else { + Ok(()) + } } } @@ -1278,7 +1235,7 @@ pub enum MavType { } impl MavType { - fn parse_type(s: &str) -> Option { + pub(crate) fn parse_type(s: &str) -> Option { use self::MavType::*; match s { "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion), @@ -1394,7 +1351,7 @@ impl MavType { } } - fn max_int_value(&self) -> u64 { + pub(crate) fn max_int_value(&self) -> u64 { match self { Self::UInt8MavlinkVersion | Self::UInt8 => u8::MAX as u64, Self::UInt16 => u16::MAX as u64, @@ -1513,7 +1470,7 @@ impl MavType { } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[derive(Debug, PartialEq, Eq, Clone, Default)] +#[derive(Debug, PartialEq, Eq, Clone, Default, Copy)] pub enum MavDeprecationType { #[default] Deprecated, @@ -1560,569 +1517,6 @@ impl MavDeprecation { } } -#[derive(Debug, PartialEq, Eq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavSuperseded { - // YYYY-MM - pub since: String, - // maybe empty, may be encapuslated in `` and contain a wildcard - pub replaced_by: String, - pub note: Option, -} - -impl MavSuperseded { - pub fn emit_tokens(&self) -> TokenStream { - let since = &self.since; - let note = match &self.note { - Some(str) if str.is_empty() || str.ends_with(".") => str.clone(), - Some(str) => format!("{str}."), - None => String::new(), - }; - let replaced_by = if self.replaced_by.starts_with("`") { - format!("See {}", self.replaced_by) - } else if self.replaced_by.is_empty() { - String::new() - } else { - format!("See `{}`", self.replaced_by) - }; - let message = format!("{note} {replaced_by} (Superseded since {since})"); - quote!(#[superseded = #message]) - } -} - -#[derive(Debug, PartialEq, Eq, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[cfg_attr(feature = "serde", serde(tag = "type"))] -pub enum MavXmlElement { - Version, - Mavlink, - Dialect, - Include, - Enums, - Enum, - Entry, - Description, - Param, - Messages, - Message, - Field, - Deprecated, - Wip, - Extensions, - Superseded, -} - -const fn identify_element(s: &[u8]) -> Option { - use self::MavXmlElement::*; - match s { - 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), - b"superseded" => Some(Superseded), - _ => None, - } -} - -fn is_valid_parent(p: Option, s: MavXmlElement) -> bool { - use self::MavXmlElement::*; - match s { - Version => p == Some(Mavlink), - Mavlink => p.is_none(), - Dialect => p == Some(Mavlink), - Include => p == Some(Mavlink), - Enums => p == Some(Mavlink), - Enum => p == Some(Enums), - Entry => p == Some(Enum), - Description => p == Some(Entry) || p == Some(Message) || p == Some(Enum), - Param => p == Some(Entry), - Messages => p == Some(Mavlink), - Message => p == Some(Messages), - Field => p == Some(Message), - Deprecated => p == Some(Entry) || p == Some(Message) || p == Some(Enum), - Wip => p == Some(Entry) || p == Some(Message) || p == Some(Enum), - Extensions => p == Some(Message), - Superseded => p == Some(Entry) || p == Some(Message) || p == Some(Enum), - } -} - -pub fn parse_profile( - definitions_dir: &Path, - definition_file: &Path, - parsed_files: &mut HashSet, -) -> 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 - - let mut stack: Vec = vec![]; - - let mut text = None; - - let mut profile = MavProfile::default(); - let mut field = MavField::default(); - let mut message = MavMessage::default(); - let mut mavenum = MavEnum::default(); - let mut entry = MavEnumEntry::default(); - let mut param_index: Option = None; - let mut param_label: Option = None; - let mut param_units: Option = None; - let mut param_enum: Option = None; - let mut param_increment: Option = None; - let mut param_min_value: Option = None; - let mut param_max_value: Option = None; - let mut param_reserved = false; - let mut param_default: Option = None; - let mut deprecated: Option = None; - - let mut xml_filter = MavXmlFilter::default(); - let mut events: Vec> = Vec::new(); - let xml = std::fs::read_to_string(&in_path).map_err(|e| { - BindGenError::CouldNotReadDefinitionFile { - source: e, - path: in_path.clone(), - } - })?; - let mut reader = Reader::from_str(&xml); - reader.config_mut().trim_text(true); - reader.config_mut().expand_empty_elements = true; - - loop { - match reader.read_event() { - Ok(Event::Eof) => { - events.push(Ok(Event::Eof)); - break; - } - Ok(event) => events.push(Ok(event.into_owned())), - Err(why) => events.push(Err(why)), - } - } - xml_filter.filter(&mut events); - let mut is_in_extension = false; - for e in events { - match e { - Ok(Event::Start(bytes)) => { - let Some(id) = identify_element(bytes.name().into_inner()) else { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - }; - - assert!( - is_valid_parent(stack.last().copied(), id), - "not valid parent {:?} of {id:?}", - stack.last(), - ); - - match id { - MavXmlElement::Extensions => { - is_in_extension = true; - } - MavXmlElement::Message => { - message = MavMessage::default(); - } - MavXmlElement::Field => { - field = MavField { - is_extension: is_in_extension, - ..Default::default() - }; - } - MavXmlElement::Enum => { - mavenum = MavEnum::default(); - } - MavXmlElement::Entry => { - if mavenum.entries.is_empty() { - mavenum.deprecated = deprecated; - } - deprecated = None; - entry = MavEnumEntry::default(); - } - MavXmlElement::Param => { - param_index = None; - param_increment = None; - param_min_value = None; - param_max_value = None; - param_reserved = false; - param_default = None; - } - MavXmlElement::Deprecated => { - deprecated = Some(MavDeprecation { - replaced_by: None, - since: String::new(), - deprecation_type: MavDeprecationType::Deprecated, - note: None, - }); - } - MavXmlElement::Superseded => { - deprecated = Some(MavDeprecation { - replaced_by: Some(String::new()), - since: String::new(), - deprecation_type: MavDeprecationType::Superseded, - note: None, - }); - } - _ => (), - } - stack.push(id); - - for attr in bytes.attributes() { - let attr = attr.unwrap(); - match stack.last() { - Some(&MavXmlElement::Enum) => { - if attr.key.into_inner() == b"name" { - mavenum.name = to_pascal_case(attr.value); - //mavenum.name = attr.value.clone(); - } else if attr.key.into_inner() == b"bitmask" { - mavenum.bitmask = true; - } - } - Some(&MavXmlElement::Entry) => { - match attr.key.into_inner() { - b"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 - let (src, radix) = value - .strip_prefix("0x") - .map(|value| (value, 16)) - .unwrap_or((value.as_ref(), 10)); - entry.value = u64::from_str_radix(src, radix).ok(); - } - _ => (), - } - } - Some(&MavXmlElement::Message) => { - match attr.key.into_inner() { - b"name" => { - /*message.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() - }) - .collect::>() - .join(""); - */ - message.name = String::from_utf8_lossy(&attr.value).to_string(); - } - b"id" => { - message.id = - String::from_utf8_lossy(&attr.value).parse().unwrap(); - } - _ => (), - } - } - Some(&MavXmlElement::Field) => { - match attr.key.into_inner() { - b"name" => { - let name = String::from_utf8_lossy(&attr.value); - field.name = if name == "type" { - "mavtype".to_string() - } else { - name.to_string() - }; - } - b"type" => { - let r#type = String::from_utf8_lossy(&attr.value); - field.mavtype = MavType::parse_type(&r#type).unwrap(); - } - b"enum" => { - field.enumtype = Some(to_pascal_case(&attr.value)); - - // Update field display if enum is a bitmask - if let Some(e) = - profile.enums.get(field.enumtype.as_ref().unwrap()) - { - if e.bitmask { - field.display = Some("bitmask".to_string()); - } - } - } - b"display" => { - field.display = - Some(String::from_utf8_lossy(&attr.value).to_string()); - } - _ => (), - } - } - Some(&MavXmlElement::Param) => { - if entry.params.is_none() { - entry.params = Some(vec![]); - } - match attr.key.into_inner() { - b"index" => { - let value = String::from_utf8_lossy(&attr.value) - .parse() - .expect("failed to parse param index"); - assert!( - (1..=7).contains(&value), - "param index must be between 1 and 7" - ); - param_index = Some(value); - } - b"label" => { - param_label = - std::str::from_utf8(&attr.value).ok().map(str::to_owned); - } - b"increment" => { - param_increment = Some( - String::from_utf8_lossy(&attr.value) - .parse() - .expect("failed to parse param increment"), - ); - } - b"minValue" => { - param_min_value = Some( - String::from_utf8_lossy(&attr.value) - .parse() - .expect("failed to parse param minValue"), - ); - } - b"maxValue" => { - param_max_value = Some( - String::from_utf8_lossy(&attr.value) - .parse() - .expect("failed to parse param maxValue"), - ); - } - b"units" => { - param_units = - std::str::from_utf8(&attr.value).ok().map(str::to_owned); - } - b"enum" => { - param_enum = - std::str::from_utf8(&attr.value).ok().map(to_pascal_case); - } - b"reserved" => { - param_reserved = attr.value.as_ref() == b"true"; - } - b"default" => { - param_default = Some( - String::from_utf8_lossy(&attr.value) - .parse() - .expect("failed to parse param maxValue"), - ); - } - _ => (), - } - } - Some(&MavXmlElement::Deprecated) => match attr.key.into_inner() { - b"since" => { - deprecated.as_mut().unwrap().since = - String::from_utf8_lossy(&attr.value).to_string(); - } - b"replaced_by" => { - let value = String::from_utf8_lossy(&attr.value); - deprecated.as_mut().unwrap().replaced_by = if value.is_empty() { - None - } else { - Some(value.to_string()) - }; - } - _ => (), - }, - Some(&MavXmlElement::Superseded) => match attr.key.into_inner() { - b"since" => { - deprecated.as_mut().unwrap().since = - String::from_utf8_lossy(&attr.value).to_string(); - } - b"replaced_by" => { - deprecated.as_mut().unwrap().replaced_by = - Some(String::from_utf8_lossy(&attr.value).to_string()); - } - _ => (), - }, - _ => (), - } - } - } - Ok(Event::Text(bytes)) => { - let s = String::from_utf8_lossy(&bytes); - - use self::MavXmlElement::*; - match (stack.last(), stack.get(stack.len() - 2)) { - (Some(&Description), Some(&Message)) - | (Some(&Field), Some(&Message)) - | (Some(&Description), Some(&Enum)) - | (Some(&Description), Some(&Entry)) - | (Some(&Include), Some(&Mavlink)) - | (Some(&Version), Some(&Mavlink)) - | (Some(&Dialect), Some(&Mavlink)) - | (Some(&Param), Some(&Entry)) - | (Some(Deprecated), _) - | (Some(Superseded), _) => { - text = Some(text.map(|t| t + s.as_ref()).unwrap_or(s.to_string())); - } - data => { - panic!("unexpected text data {data:?} reading {s:?}"); - } - } - } - Ok(Event::GeneralRef(bytes)) => { - let entity = String::from_utf8_lossy(&bytes); - text = Some( - text.map(|t| format!("{t}&{entity};")) - .unwrap_or(format!("&{entity};")), - ); - } - Ok(Event::End(_)) => { - match stack.last() { - Some(&MavXmlElement::Field) => { - field.description = text.map(|t| t.replace('\n', " ")); - message.fields.push(field.clone()); - } - Some(&MavXmlElement::Entry) => { - entry.deprecated = deprecated; - deprecated = None; - mavenum.entries.push(entry.clone()); - } - Some(&MavXmlElement::Message) => { - message.deprecated = deprecated; - - deprecated = None; - is_in_extension = false; - // Follow mavlink ordering specification: https://mavlink.io/en/guide/serialization.html#field_reordering - let mut not_extension_fields = message.fields.clone(); - let mut extension_fields = message.fields.clone(); - - not_extension_fields.retain(|field| !field.is_extension); - extension_fields.retain(|field| field.is_extension); - - // Only not mavlink 1 fields need to be sorted - not_extension_fields.sort_by(|a, b| a.mavtype.compare(&b.mavtype)); - - // Update msg fields and add the new message - let mut msg = message.clone(); - msg.fields.clear(); - msg.fields.extend(not_extension_fields); - msg.fields.extend(extension_fields); - - // Validate there are no duplicate field names - msg.validate_unique_fields(); - // Validate field count must be between 1 and 64 - msg.validate_field_count(); - - profile.add_message(&msg); - } - Some(&MavXmlElement::Enum) => { - profile.add_enum(&mavenum); - } - Some(&MavXmlElement::Include) => { - let include = - PathBuf::from(text.map(|t| t.replace('\n', "")).unwrap_or_default()); - 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); - } - if profile.version.is_none() { - profile.version = included_profile.version; - } - } - } - Some(&MavXmlElement::Description) => match stack.get(stack.len() - 2) { - Some(&MavXmlElement::Message) => { - message.description = text.map(|t| t.replace('\n', " ")); - } - Some(&MavXmlElement::Enum) => { - mavenum.description = text.map(|t| t.replace('\n', " ")); - } - Some(&MavXmlElement::Entry) => { - entry.description = text.map(|t| t.replace('\n', " ")); - } - _ => (), - }, - Some(&MavXmlElement::Version) => { - if let Some(t) = text { - profile.version = - Some(t.parse().expect("Invalid minor version number format")); - } - } - Some(&MavXmlElement::Dialect) => { - if let Some(t) = text { - profile.dialect = - Some(t.parse().expect("Invalid dialect number format")); - } - } - Some(&MavXmlElement::Deprecated) => { - if let Some(t) = text { - deprecated.as_mut().unwrap().note = Some(t); - } - } - Some(&MavXmlElement::Param) => { - if let Some(params) = entry.params.as_mut() { - // Some messages can jump between values, like: 1, 2, 7 - let param_index = param_index.expect("entry params must have an index"); - while params.len() < param_index { - params.push(MavParam { - index: params.len() + 1, - description: None, - ..Default::default() - }); - } - if let Some((min, max)) = param_min_value.zip(param_max_value) { - assert!( - min <= max, - "param minValue must not be greater than maxValue" - ); - } - params[param_index - 1] = MavParam { - index: param_index, - description: text.map(|t| t.replace('\n', " ")), - label: param_label, - units: param_units, - enum_used: param_enum, - increment: param_increment, - max_value: param_max_value, - min_value: param_min_value, - reserved: param_reserved, - default: param_default, - }; - param_label = None; - param_units = None; - param_enum = None; - } - } - _ => (), - } - text = None; - stack.pop(); - // println!("{}-{}", indent(depth), name); - } - Err(e) => { - eprintln!("Error: {e}"); - break; - } - _ => {} - } - } - - //let profile = profile.update_messages(); //TODO verify no longer needed - 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( @@ -2177,161 +1571,6 @@ pub fn extra_crc(msg: &MavMessage) -> u8 { ((crcval & 0xFF) ^ (crcval >> 8)) as u8 } -#[cfg(not(feature = "mav2-message-extensions"))] -struct ExtensionFilter { - pub is_in: bool, -} - -struct MessageFilter { - pub is_in: bool, - pub messages: Vec, -} - -impl MessageFilter { - pub fn new() -> Self { - Self { - is_in: false, - messages: vec![ - // device_cap_flags is u32, when enum is u16, which is not handled by the parser yet - "STORM32_GIMBAL_MANAGER_INFORMATION".to_string(), - ], - } - } -} - -struct MavXmlFilter { - #[cfg(not(feature = "mav2-message-extensions"))] - extension_filter: ExtensionFilter, - message_filter: MessageFilter, -} - -impl Default for MavXmlFilter { - fn default() -> Self { - Self { - #[cfg(not(feature = "mav2-message-extensions"))] - extension_filter: ExtensionFilter { is_in: false }, - message_filter: MessageFilter::new(), - } - } -} - -impl MavXmlFilter { - pub fn filter(&mut self, elements: &mut Vec>) { - elements.retain(|x| self.filter_extension(x) && self.filter_messages(x)); - } - - #[cfg(feature = "mav2-message-extensions")] - pub fn filter_extension(&mut self, _element: &Result) -> bool { - true - } - - /// Ignore extension fields - #[cfg(not(feature = "mav2-message-extensions"))] - pub fn filter_extension(&mut self, element: &Result) -> bool { - match element { - Ok(content) => { - match content { - Event::Start(bytes) | Event::Empty(bytes) => { - let Some(id) = identify_element(bytes.name().into_inner()) else { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - }; - if id == MavXmlElement::Extensions { - self.extension_filter.is_in = true; - } - } - Event::End(bytes) => { - let Some(id) = identify_element(bytes.name().into_inner()) else { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - }; - - if id == MavXmlElement::Message { - self.extension_filter.is_in = false; - } - } - _ => {} - } - !self.extension_filter.is_in - } - Err(error) => panic!("Failed to filter XML: {error}"), - } - } - - /// Filters messages by their name - pub fn filter_messages(&mut self, element: &Result) -> bool { - match element { - Ok(content) => { - match content { - Event::Start(bytes) | Event::Empty(bytes) => { - let Some(id) = identify_element(bytes.name().into_inner()) else { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - }; - if id == MavXmlElement::Message { - for attr in bytes.attributes() { - let attr = attr.unwrap(); - if attr.key.into_inner() == b"name" { - let value = String::from_utf8_lossy(&attr.value).into_owned(); - if self.message_filter.messages.contains(&value) { - self.message_filter.is_in = true; - return false; - } - } - } - } - } - Event::End(bytes) => { - let Some(id) = identify_element(bytes.name().into_inner()) else { - panic!( - "unexpected element {:?}", - String::from_utf8_lossy(bytes.name().into_inner()) - ); - }; - - if id == MavXmlElement::Message && self.message_filter.is_in { - self.message_filter.is_in = false; - return false; - } - } - _ => {} - } - !self.message_filter.is_in - } - Err(error) => panic!("Failed to filter XML: {error}"), - } - } -} - -#[inline(always)] -fn to_pascal_case(text: impl AsRef<[u8]>) -> String { - let input = text.as_ref(); - let mut result = String::with_capacity(input.len()); - let mut capitalize = true; - - for &b in input { - if b == b'_' { - capitalize = true; - continue; - } - - if capitalize { - result.push((b as char).to_ascii_uppercase()); - capitalize = false; - } else { - result.push((b as char).to_ascii_lowercase()); - } - } - - result -} - #[cfg(test)] mod tests { use super::*; @@ -2428,11 +1667,11 @@ mod tests { deprecated: None, }; // Should not panic - msg.validate_unique_fields(); + msg.validate_unique_fields().unwrap(); } #[test] - #[should_panic(expected = "Duplicate field")] + #[should_panic(expected = "DuplicateFieldName")] fn validate_unique_fields_panics_on_duplicate() { let msg = MavMessage { id: 2, @@ -2459,7 +1698,7 @@ mod tests { deprecated: None, }; // Should panic due to duplicate field names - msg.validate_unique_fields(); + msg.validate_unique_fields().unwrap(); } #[test] @@ -2489,7 +1728,7 @@ mod tests { deprecated: None, }; // Should not panic - msg.validate_field_count(); + msg.validate_field_count().unwrap(); } #[test] @@ -2515,7 +1754,7 @@ mod tests { deprecated: None, }; // Should panic due to 65 fields - msg.validate_field_count(); + msg.validate_field_count().unwrap(); } #[test] @@ -2529,7 +1768,7 @@ mod tests { deprecated: None, }; // Should panic due to no fields - msg.validate_field_count(); + msg.validate_field_count().unwrap(); } #[test] diff --git a/mavlink-bindgen/src/parser_new.rs b/mavlink-bindgen/src/parser_new.rs new file mode 100644 index 00000000000..bb816105579 --- /dev/null +++ b/mavlink-bindgen/src/parser_new.rs @@ -0,0 +1,835 @@ +use std::{ + collections::{BTreeMap, HashSet}, + path::{Path, PathBuf}, +}; + +use quick_xml::{ + events::{BytesStart, Event}, + name::QName, + Reader, +}; + +use crate::{ + error::MavXMLParseError, + parser::{ + MavDeprecation, MavDeprecationType, MavEnum, MavEnumEntry, MavField, MavMessage, MavParam, + MavProfile, MavType, + }, + BindGenError, +}; + +#[derive(Debug, PartialEq, Eq, Clone, Copy)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +pub enum MavXmlElement { + Version, + Mavlink, + Dialect, + Include, + Enums, + Enum, + Entry, + Description, + Param, + Messages, + Message, + Field, + Deprecated, + Wip, + Extensions, + Superseded, +} + +const fn id_element(name: QName<'_>) -> Option { + use self::MavXmlElement::*; + match name.into_inner() { + 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), + b"superseded" => Some(Superseded), + _ => None, + } +} + +#[cfg(not(feature = "mav2-message-extensions"))] +struct ExtensionFilter { + pub is_in: bool, +} + +struct MessageFilter { + pub is_in: bool, + pub messages: Vec, +} + +struct MavXmlFilter { + #[cfg(not(feature = "mav2-message-extensions"))] + extension_filter: ExtensionFilter, + message_filter: MessageFilter, +} + +impl Default for MavXmlFilter { + fn default() -> Self { + Self { + #[cfg(not(feature = "mav2-message-extensions"))] + extension_filter: ExtensionFilter { is_in: false }, + message_filter: MessageFilter::new(), + } + } +} + +impl MessageFilter { + fn new() -> Self { + Self { + is_in: false, + messages: vec![ + // device_cap_flags is u32, when enum is u16, which is not handled by the parser yet + "STORM32_GIMBAL_MANAGER_INFORMATION".to_string(), + ], + } + } +} + +impl MavXmlFilter { + fn filter(&mut self, event: &Event) -> bool { + self.filter_extension(event) + && self.filter_messages(event) + && Self::filter_event_type(event) + } + + #[cfg(feature = "mav2-message-extensions")] + fn filter_extension(&mut self, _event: &Event) -> bool { + true + } + + /// Ignore extension fields + #[cfg(not(feature = "mav2-message-extensions"))] + fn filter_extension(&mut self, event: &Event) -> bool { + match event { + Event::Start(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Extensions) => { + self.extension_filter.is_in = true; + } + Event::End(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Message) => { + self.extension_filter.is_in = false; + } + _ => {} + } + !self.extension_filter.is_in + } + + /// Filters messages by their name + fn filter_messages(&mut self, event: &Event) -> bool { + match event { + Event::Start(bytes) if Some(MavXmlElement::Message) == id_element(bytes.name()) => { + let name_attr = bytes + .attributes() + .filter_map(|attr| attr.ok()) + .find(|attr| attr.key.into_inner() == b"name"); + if let Some(name_attr) = name_attr { + let value = String::from_utf8_lossy(&name_attr.value); + if self + .message_filter + .messages + .iter() + .any(|filter| filter.as_str() == value) + { + self.message_filter.is_in = true; + return false; + } + } + } + Event::End(bytes) + if Some(MavXmlElement::Message) == id_element(bytes.name()) + && self.message_filter.is_in => + { + self.message_filter.is_in = false; + return false; + } + _ => {} + } + !self.message_filter.is_in + } + + fn filter_event_type(event: &Event) -> bool { + !matches!( + event, + Event::Comment(_) | Event::Decl(_) | Event::CData(_) | Event::DocType(_) | Event::PI(_) + ) + } +} + +struct MavCompositeProfile { + profile: MavProfile, + includes: Vec, +} + +impl MavCompositeProfile { + fn resolve_includes( + mut self, + definitions_dir: &Path, + parsed_files: &mut HashSet, + ) -> Result { + for include in &self.includes { + let include = PathBuf::from(include.trim()); + 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() { + self.profile.add_message(message); + } + for enm in included_profile.enums.values() { + self.profile.add_enum(enm); + } + if self.profile.version.is_none() { + self.profile.version = included_profile.version; + } + } + } + Ok(self.profile) + } +} + +impl MavProfile { + /// Go over all fields in the messages, and if you encounter an enum, + /// which is a bitmask, set the bitmask size based on field size + fn update_bitmask_enum_fields(mut self, definition_file: &Path) -> Result { + for msg in self.messages.values_mut() { + for field in &mut msg.fields { + if let Some(enum_name) = &field.enumtype { + // find the corresponding enum + if let Some(enm) = self.enums.get_mut(enum_name) { + // Handle legacy definition where bitmask is defined as display="bitmask" + if field.display == Some("bitmask".to_string()) { + enm.bitmask = true; + } + + // it is a bitmask + if enm.bitmask { + enm.primitive = Some(field.mavtype.rust_primitive_type()); + + // check if all enum values can be stored in the fields + for entry in &enm.entries { + if entry.value.unwrap_or_default() > field.mavtype.max_int_value() { + return Err(BindGenError::BitflagEnumTypeToNarrow { + field_name: field.name.clone(), + message_name: msg.name.clone(), + enum_name: enum_name.to_string(), + }); + } + } + + // Fix fields in backwards manner + if field.display.is_none() { + field.display = Some("bitmask".to_string()); + } + } + } else { + return Err(BindGenError::InvalidEnumReference { + enum_name: enum_name.to_string(), + path: definition_file.to_path_buf(), + }); + } + } + } + } + Ok(self) + } +} + +pub fn parse_profile( + definitions_dir: &Path, + definition_file: &Path, + parsed_files: &mut HashSet, +) -> Result { + let is_top_level = parsed_files.is_empty(); + let in_path = Path::new(&definitions_dir).join(definition_file); + // Keep track of which files have been parsed + parsed_files.insert(in_path.clone()); + + let xml = std::fs::read_to_string(&in_path).map_err(|e| { + BindGenError::CouldNotReadDefinitionFile { + source: e, + path: in_path.clone(), + } + })?; + + let mut reader = Reader::from_str(&xml); + + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + + let mut parser = Parser::from_reader(reader); + + let profile = parser + .parse() + .map_err(|source| BindGenError::InvalidDefinitionFile { + source, + path: definition_file.to_path_buf(), + })?; + + let profile = profile.resolve_includes(definitions_dir, parsed_files)?; + + if is_top_level { + Ok(profile.update_bitmask_enum_fields(definition_file)?) + } else { + Ok(profile) + } +} + +struct Parser<'a> { + reader: Reader<&'a [u8]>, + filter: MavXmlFilter, +} + +impl<'a> Parser<'a> { + fn from_reader(reader: Reader<&'a [u8]>) -> Self { + let filter = MavXmlFilter::default(); + Parser { reader, filter } + } + + fn next_event(&mut self) -> Result, MavXMLParseError> { + loop { + match self.reader.read_event()? { + Event::Eof => return Err(MavXMLParseError::UnexpectedEnd), + ev => { + if self.filter.filter(&ev) { + return Ok(ev); + } + } + } + } + } + + fn parse(&mut self) -> Result { + match self.next_event()? { + Event::Start(bytes_start) + if id_element(bytes_start.name()) == Some(MavXmlElement::Mavlink) => + { + self.parse_mavlink() + } + _ => Err(MavXMLParseError::ExpectedMAVLink), + } + } + + fn parse_mavlink(&mut self) -> Result { + let mut profile = MavProfile::default(); + let mut includes = vec![]; + loop { + match self.next_event()? { + Event::Start(bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Include) => { + includes.push(self.parse_string_element(MavXmlElement::Include)?); + } + Some(MavXmlElement::Version) => { + profile.version = Some(self.parse_int_element(MavXmlElement::Version)?); + } + Some(MavXmlElement::Dialect) => { + profile.dialect = Some(self.parse_int_element(MavXmlElement::Dialect)?); + } + Some(MavXmlElement::Enums) => { + profile.enums = self.parse_enums()?; + } + Some(MavXmlElement::Messages) => profile.messages = self.parse_messages()?, + None => {} + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Mavlink) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Mavlink, + }) + } + } + } + Ok(MavCompositeProfile { profile, includes }) + } + + fn parse_string_option_element( + &mut self, + element: MavXmlElement, + ) -> Result, MavXMLParseError> { + let mut string = None; + + loop { + match self.next_event()? { + Event::Text(bytes_text) => { + let text = String::from_utf8_lossy(&bytes_text).replace('\n', " "); + string = Some( + string + .map(|t| format!("{t}{text}")) + .unwrap_or(text.to_string()), + ); + } + Event::GeneralRef(bytes_ref) => { + let entity = String::from_utf8_lossy(&bytes_ref); + string = Some( + string + .map(|t| format!("{t}&{entity};")) + .unwrap_or(format!("&{entity};")), + ); + } + Event::End(bytes_end) if id_element(bytes_end.name()) == Some(element) => break, + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: element, + }) + } + } + } + + Ok(string) + } + + fn parse_string_element(&mut self, element: MavXmlElement) -> Result { + self.parse_string_option_element(element)? + .ok_or(MavXMLParseError::ExpectedText) + } + + fn parse_int_element(&mut self, element: MavXmlElement) -> Result { + Ok(self.parse_string_element(element)?.parse()?) + } + + fn parse_deprecated( + &mut self, + bytes: &BytesStart<'_>, + kind: MavDeprecationType, + ) -> Result { + let mut deprecation = MavDeprecation { + deprecation_type: kind, + ..Default::default() + }; + + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"since" => { + deprecation.since = String::from_utf8_lossy(&attr.value).to_string(); + } + b"replaced_by" => { + let value = String::from_utf8_lossy(&attr.value); + deprecation.replaced_by = if value.is_empty() { + None + } else { + Some(value.to_string()) + }; + } + _ => (), + } + } + + let element = if kind == MavDeprecationType::Superseded { + MavXmlElement::Superseded + } else { + MavXmlElement::Deprecated + }; + + deprecation.note = self.parse_string_option_element(element)?; + + Ok(deprecation) + } + + fn parse_wip(&mut self) -> Result<(), MavXMLParseError> { + match self.next_event()? { + Event::End(bytes_end) if id_element(bytes_end.name()) == Some(MavXmlElement::Wip) => {} + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Wip, + }) + } + } + Ok(()) + } + + fn parse_enums(&mut self) -> Result, MavXMLParseError> { + let mut enums = BTreeMap::new(); + loop { + match self.next_event()? { + Event::Start(bytes_start) + if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Enum)) => + { + let mav_enum = self.parse_enum(bytes_start)?; + enums.insert(mav_enum.name.clone(), mav_enum); + } + Event::End(bytes_end) + if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Enums)) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Enums, + }) + } + } + } + Ok(enums) + } + + fn parse_enum(&mut self, bytes: BytesStart<'_>) -> Result { + let mut mav_enum = MavEnum::default(); + + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => { + mav_enum.name = to_pascal_case(&attr.value); + } + b"bitmask" => mav_enum.bitmask = true, + _ => {} + } + } + + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Entry) => { + mav_enum.entries.push(self.parse_entry(bytes_start)?); + } + Some(MavXmlElement::Description) => { + mav_enum.description = + Some(self.parse_string_element(MavXmlElement::Description)?); + } + Some(MavXmlElement::Deprecated) => { + mav_enum.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); + } + Some(MavXmlElement::Superseded) => { + mav_enum.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); + } + Some(MavXmlElement::Wip) => self.parse_wip()?, + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Enum, + }) + } + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Enum) => + { + break + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Enum, + }) + } + } + } + + Ok(mav_enum) + } + + fn parse_entry(&mut self, bytes: &BytesStart<'_>) -> Result { + let mut entry = MavEnumEntry::default(); + + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => entry.name = String::from_utf8_lossy(&attr.value).to_string(), + b"value" => { + let value = String::from_utf8_lossy(&attr.value); + let (src, radix) = value + .strip_prefix("0x") + .map(|value| (value, 16)) + .unwrap_or((value.as_ref(), 10)); + entry.value = Some(u64::from_str_radix(src, radix)?); + } + _ => {} + } + } + + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => { + match id_element(bytes_start.name()) { + Some(MavXmlElement::Description) => { + entry.description = + self.parse_string_option_element(MavXmlElement::Description)?; + } + Some(MavXmlElement::Deprecated) => { + entry.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); + } + Some(MavXmlElement::Superseded) => { + entry.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); + } + Some(MavXmlElement::Param) => { + let param = self.parse_param(bytes_start)?; + let param_index = param.index; + + // Some messages can jump between values, like: 1, 2, 7 + let params = entry.params.get_or_insert(Vec::with_capacity(7)); + while params.len() < param_index { + params.push(MavParam { + index: params.len() + 1, + ..Default::default() + }); + } + params[param_index - 1] = param; + } + Some(MavXmlElement::Wip) => _ = Some(self.parse_wip()?), + Some(element) => { + return Err(MavXMLParseError::UnexpectedElement { element }) + } + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Entry, + }) + } + } + } + Event::End(bytes_end) + if Some(MavXmlElement::Entry) == id_element(bytes_end.name()) => + { + break + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Entry, + }) + } + } + } + Ok(entry) + } + + fn parse_param(&mut self, bytes: &BytesStart<'_>) -> Result { + let mut param = MavParam::default(); + + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"index" => { + let value = String::from_utf8_lossy(&attr.value).parse()?; + param.index = value; + } + b"label" => { + param.label = std::str::from_utf8(&attr.value).ok().map(str::to_owned); + } + b"increment" => { + param.increment = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"minValue" => { + param.min_value = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"maxValue" => { + param.max_value = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"units" => { + param.units = std::str::from_utf8(&attr.value).ok().map(str::to_owned); + } + b"enum" => { + param.enum_used = std::str::from_utf8(&attr.value).ok().map(to_pascal_case); + } + b"reserved" => { + param.reserved = attr.value.as_ref() == b"true"; + } + b"default" => { + param.default = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + _ => (), + } + } + + if !(1..=7).contains(¶m.index) { + return Err(MavXMLParseError::ParamIndexOutOfRange); + } + + param.description = self.parse_string_option_element(MavXmlElement::Param)?; + Ok(param) + } + + fn parse_messages(&mut self) -> Result, MavXMLParseError> { + let mut messages = BTreeMap::new(); + loop { + match self.next_event()? { + Event::Start(bytes_start) + if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Message)) => + { + let msg = self.parse_message(bytes_start)?; + messages.insert(msg.name.clone(), msg); + } + Event::End(bytes_end) + if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Messages)) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Messages, + }) + } + } + } + Ok(messages) + } + + fn parse_message(&mut self, bytes: BytesStart<'_>) -> Result { + let mut message = MavMessage::default(); + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => message.name = String::from_utf8_lossy(&attr.value).to_string(), + b"id" => message.id = String::from_utf8_lossy(&attr.value).parse()?, + _ => {} + } + } + + let mut extention = false; + + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Description) => { + message.description = + Some(self.parse_string_element(MavXmlElement::Description)?); + } + Some(MavXmlElement::Field) => message + .fields + .push(self.parse_field(bytes_start, extention)?), + Some(MavXmlElement::Extensions) => match self.next_event()? { + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Extensions) => + { + extention = true; + } + _ => return Err(MavXMLParseError::NoneSelfClosingExtTag), + }, + Some(MavXmlElement::Deprecated) => { + message.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); + } + Some(MavXmlElement::Wip) => self.parse_wip()?, + Some(MavXmlElement::Superseded) => { + message.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); + } + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Message, + }) + } + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Message) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Message, + }) + } + } + } + + // Validate there are no duplicate field names + message.validate_unique_fields()?; + // Validate field count must be between 1 and 64 + message.validate_field_count()?; + + // Sort fields, first all none extension fields by type, then all extension fields in original order + message + .fields + .sort_by(|a, b| match (a.is_extension, b.is_extension) { + (false, false) => a.mavtype.compare(&b.mavtype), + (a, b) => a.cmp(&b), + }); + + Ok(message) + } + + fn parse_field( + &mut self, + bytes: &BytesStart<'_>, + is_extension: bool, + ) -> Result { + let mut field = MavField { + is_extension, + ..Default::default() + }; + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => { + let name = String::from_utf8_lossy(&attr.value); + field.name = if name == "type" { + "mavtype".to_string() + } else { + name.to_string() + }; + } + b"type" => { + let mav_type = String::from_utf8_lossy(&attr.value); + field.mavtype = + MavType::parse_type(&mav_type).ok_or(MavXMLParseError::InvalidType { + mav_type: mav_type.to_string(), + })?; + } + b"enum" => { + let enum_type = to_pascal_case(&attr.value); + field.enumtype = Some(enum_type); + } + b"display" => { + field.display = Some(String::from_utf8_lossy(&attr.value).to_string()); + } + _ => {} + } + } + + field.description = self.parse_string_option_element(MavXmlElement::Field)?; + + Ok(field) + } +} + +fn to_pascal_case(text: impl AsRef<[u8]>) -> String { + let input = text.as_ref(); + let mut result = String::with_capacity(input.len()); + let mut capitalize = true; + + for &b in input { + if b == b'_' { + capitalize = true; + continue; + } + + if capitalize { + result.push((b as char).to_ascii_uppercase()); + capitalize = false; + } else { + result.push((b as char).to_ascii_lowercase()); + } + } + result +} diff --git a/mavlink-bindgen/tests/definitions/superseded.xml b/mavlink-bindgen/tests/definitions/superseded.xml index cf2d599bbf1..dc883aea516 100644 --- a/mavlink-bindgen/tests/definitions/superseded.xml +++ b/mavlink-bindgen/tests/definitions/superseded.xml @@ -31,6 +31,20 @@ + + Enumeration of battery functions + + Battery function is unknown + + + + + Enumeration of battery types + + Not specified. + + + diff --git a/mavlink-bindgen/tests/snapshots/e2e_snapshots__superseded.xml@superseded.rs.snap b/mavlink-bindgen/tests/snapshots/e2e_snapshots__superseded.xml@superseded.rs.snap index 9853569cc7c..3d28b4d507c 100644 --- a/mavlink-bindgen/tests/snapshots/e2e_snapshots__superseded.xml@superseded.rs.snap +++ b/mavlink-bindgen/tests/snapshots/e2e_snapshots__superseded.xml@superseded.rs.snap @@ -31,9 +31,49 @@ use ts_rs::TS; #[cfg_attr(feature = "serde", serde(tag = "type"))] #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] #[repr(u32)] +#[doc = "Enumeration of battery functions"] +pub enum MavBatteryFunction { + #[doc = "Battery function is unknown"] + MAV_BATTERY_FUNCTION_UNKNOWN = 0, +} +impl MavBatteryFunction { + pub const DEFAULT: Self = Self::MAV_BATTERY_FUNCTION_UNKNOWN; +} +impl Default for MavBatteryFunction { + fn default() -> Self { + Self::DEFAULT + } +} +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[repr(u32)] +#[doc = "Enumeration of battery types"] +pub enum MavBatteryType { + #[doc = "Not specified."] + MAV_BATTERY_TYPE_UNKNOWN = 0, +} +impl MavBatteryType { + pub const DEFAULT: Self = Self::MAV_BATTERY_TYPE_UNKNOWN; +} +impl Default for MavBatteryType { + fn default() -> Self { + Self::DEFAULT + } +} +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[repr(u32)] #[doc = "Coordinate frames used by MAVLink. Not all frames are supported by all commands, messages, or vehicles."] pub enum MavFrame { - #[deprecated = " See `MAV_FRAME_GLOBAL` (Superseded since 2024-03)"] + #[deprecated = "Use MAV_FRAME_GLOBAL in COMMAND_INT (and elsewhere) as a synonymous replacement. See `MAV_FRAME_GLOBAL` (Superseded since 2024-03)"] #[doc = "Global (WGS84) coordinate frame (scaled) + altitude relative to mean sea level (MSL)."] MAV_FRAME_GLOBAL_INT = 5, } @@ -74,7 +114,7 @@ impl Default for MavRoi { Self::DEFAULT } } -#[deprecated = " See `BATTERY_INFO` (Superseded since 2024-02)"] +#[deprecated = "The BATTERY_INFO message is better aligned with UAVCAN messages, and in any case is useful even if a battery is not \"smart\". See `BATTERY_INFO` (Superseded since 2024-02)"] #[doc = "Smart Battery information (static/infrequent update). Use for updates from: smart battery to flight stack, flight stack to GCS. Use BATTERY_STATUS for the frequent battery updates."] #[doc = ""] #[doc = "ID: 370"] @@ -238,7 +278,7 @@ pub enum MavMessage { #[doc = "Smart Battery information (static/infrequent update). Use for updates from: smart battery to flight stack, flight stack to GCS. Use BATTERY_STATUS for the frequent battery updates."] #[doc = ""] #[doc = "ID: 370"] - #[deprecated = " See `BATTERY_INFO` (Superseded since 2024-02)"] + #[deprecated = "The BATTERY_INFO message is better aligned with UAVCAN messages, and in any case is useful even if a battery is not \"smart\". See `BATTERY_INFO` (Superseded since 2024-02)"] SMART_BATTERY_INFO(SMART_BATTERY_INFO_DATA), } impl MavMessage { From f9d134f968af698ab0a041557681f64e4f180779 Mon Sep 17 00:00:00 2001 From: pv42 Date: Wed, 11 Mar 2026 16:26:25 +0100 Subject: [PATCH 2/5] bindgen: rename parser to codegen and parser_new to parser --- mavlink-bindgen/src/codegen.rs | 1892 ++++++++++++++++++++++ mavlink-bindgen/src/error.rs | 2 +- mavlink-bindgen/src/lib.rs | 6 +- mavlink-bindgen/src/parser.rs | 2427 ++++++++--------------------- mavlink-bindgen/src/parser_new.rs | 835 ---------- 5 files changed, 2581 insertions(+), 2581 deletions(-) create mode 100644 mavlink-bindgen/src/codegen.rs delete mode 100644 mavlink-bindgen/src/parser_new.rs diff --git a/mavlink-bindgen/src/codegen.rs b/mavlink-bindgen/src/codegen.rs new file mode 100644 index 00000000000..8de5c3444db --- /dev/null +++ b/mavlink-bindgen/src/codegen.rs @@ -0,0 +1,1892 @@ +use crc_any::CRCu16; +use std::cmp::Ordering; +use std::collections::btree_map::Entry; +use std::collections::{BTreeMap, HashSet}; +use std::default::Default; +use std::fmt::Display; +use std::io::Write; +use std::path::{Path, PathBuf}; +use std::str::FromStr; +use std::sync::LazyLock; + +use regex::Regex; + +use proc_macro2::{Ident, TokenStream}; +use quote::{format_ident, quote}; + +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + +use crate::error::{BindGenError, MavXMLParseError}; +use crate::parser::parse_profile; +use crate::util; + +static URL_REGEX: LazyLock = LazyLock::new(|| { + Regex::new(concat!( + r"(https?://", // url scheme + r"([-a-zA-Z0-9@:%._\+~#=]{2,256}\.)+", // one or more subdomains + r"[a-zA-Z]{2,63}", // root domain + r"\b([-a-zA-Z0-9@:%_\+.~#?&/=]*[-a-zA-Z0-9@:%_\+~#?&/=])?)", // optional query or url fragments + )) + .expect("failed to build regex") +}); + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavProfile { + pub messages: BTreeMap, + pub enums: BTreeMap, + pub version: Option, + pub dialect: Option, +} + +impl MavProfile { + pub(crate) fn add_message(&mut self, message: &MavMessage) { + match self.messages.entry(message.name.clone()) { + Entry::Occupied(entry) => { + assert!( + entry.get() == message, + "Message '{}' defined twice but definitions are different", + message.name + ); + } + Entry::Vacant(entry) => { + entry.insert(message.clone()); + } + } + } + + pub(crate) fn add_enum(&mut self, enm: &MavEnum) { + match self.enums.entry(enm.name.clone()) { + Entry::Occupied(entry) => { + entry.into_mut().combine(enm); + } + Entry::Vacant(entry) => { + entry.insert(enm.clone()); + } + } + } + + //TODO verify this is no longer necessary since we're supporting both mavlink1 and mavlink2 + // ///If we are not using Mavlink v2, remove messages with id's > 254 + // fn update_messages(mut self) -> Self { + // //println!("Updating messages"); + // let msgs = self.messages.into_iter().filter( + // |x| x.id <= 254).collect::>(); + // self.messages = msgs; + // self + // } + + /// Simple header comment + #[inline(always)] + fn emit_comments(&self, dialect_name: &str) -> TokenStream { + let message = format!("MAVLink {dialect_name} dialect."); + quote!( + #![doc = #message] + #![doc = ""] + #![doc = "This file was automatically generated, do not edit."] + ) + } + + /// Emit rust messages + #[inline(always)] + fn emit_msgs(&self) -> Vec { + self.messages + .values() + .map(|d| d.emit_rust(self.version.is_some())) + .collect() + } + + /// Emit rust enums + #[inline(always)] + fn emit_enums(&self) -> Vec { + self.enums.values().map(|d| d.emit_rust()).collect() + } + + #[inline(always)] + fn emit_deprecations(&self) -> Vec { + self.messages + .values() + .map(|msg| { + msg.deprecated + .as_ref() + .map(|d| d.emit_tokens()) + .unwrap_or_default() + }) + .collect() + } + + /// Get list of original message names + #[inline(always)] + fn emit_enum_names(&self) -> Vec { + self.messages + .values() + .map(|msg| { + let name = format_ident!("{}", msg.name); + quote!(#name) + }) + .collect() + } + + /// Emit message names with "_DATA" at the end + #[inline(always)] + fn emit_struct_names(&self) -> Vec { + self.messages + .values() + .map(|msg| msg.emit_struct_name()) + .collect() + } + + fn emit_rust(&self, dialect_name: &str) -> TokenStream { + //TODO verify that id_width of u8 is OK even in mavlink v1 + let id_width = format_ident!("u32"); + + let comment = self.emit_comments(dialect_name); + let mav_minor_version = self.emit_minor_version(); + let mav_dialect_number = self.emit_dialect_number(); + let msgs = self.emit_msgs(); + let deprecations = self.emit_deprecations(); + let enum_names = self.emit_enum_names(); + let struct_names = self.emit_struct_names(); + let enums = self.emit_enums(); + + let variant_docs = self.emit_variant_description(); + + let mav_message = + self.emit_mav_message(&variant_docs, &deprecations, &enum_names, &struct_names); + let mav_message_all_ids = self.emit_mav_message_all_ids(); + let mav_message_all_messages = self.emit_mav_message_all_messages(); + 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, &struct_names); + let mav_message_random_from_id = + self.emit_mav_message_random_from_id(&enum_names, &struct_names); + let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); + let mav_message_target_system_id = self.emit_mav_message_target_system_id(); + let mav_message_target_component_id = self.emit_mav_message_target_component_id(); + + quote! { + #comment + #![allow(deprecated)] + #![allow(clippy::match_single_binding)] + #[allow(unused_imports)] + use num_derive::{FromPrimitive, ToPrimitive}; + #[allow(unused_imports)] + use num_traits::{FromPrimitive, ToPrimitive}; + #[allow(unused_imports)] + use bitflags::{bitflags, Flags}; + #[allow(unused_imports)] + use mavlink_core::{MavlinkVersion, Message, MessageData, bytes::Bytes, bytes_mut::BytesMut, types::CharArray}; + + #[cfg(feature = "serde")] + use serde::{Serialize, Deserialize}; + + #[cfg(feature = "arbitrary")] + use arbitrary::Arbitrary; + + #[cfg(feature = "ts-rs")] + use ts_rs::TS; + + #mav_minor_version + #mav_dialect_number + + #(#enums)* + + #(#msgs)* + + #[derive(Clone, PartialEq, Debug)] + #mav_message + + impl MavMessage { + #mav_message_all_ids + #mav_message_all_messages + } + + impl Message for MavMessage { + #mav_message_parse + #mav_message_name + #mav_message_id + #mav_message_id_from_name + #mav_message_default_from_id + #mav_message_random_from_id + #mav_message_serialize + #mav_message_crc + #mav_message_target_system_id + #mav_message_target_component_id + } + } + } + + #[inline(always)] + fn emit_mav_message( + &self, + docs: &[TokenStream], + deprecations: &[TokenStream], + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { + quote! { + #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] + #[cfg_attr(feature = "ts-rs", derive(TS))] + #[cfg_attr(feature = "ts-rs", ts(export))] + #[repr(u32)] + pub enum MavMessage { + #(#docs #deprecations #enums(#structs),)* + } + } + } + + fn emit_variant_description(&self) -> Vec { + self.messages + .values() + .map(|msg| { + let mut ts = TokenStream::new(); + + if let Some(doc) = msg.description.as_ref() { + let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." }); + let doc = URL_REGEX.replace_all(&doc, "<$1>"); + ts.extend(quote!(#[doc = #doc])); + + // Leave a blank line before the message ID for readability. + ts.extend(quote!(#[doc = ""])); + } + + let id = format!("ID: {}", msg.id); + ts.extend(quote!(#[doc = #id])); + + ts + }) + .collect() + } + + #[inline(always)] + fn emit_mav_message_all_ids(&self) -> TokenStream { + let mut message_ids = self.messages.values().map(|m| m.id).collect::>(); + message_ids.sort(); + + quote!( + pub const fn all_ids() -> &'static [u32] { + &[#(#message_ids),*] + } + ) + } + + #[inline(always)] + fn emit_minor_version(&self) -> TokenStream { + if let Some(version) = self.version { + quote! (pub const MINOR_MAVLINK_VERSION: u8 = #version;) + } else { + TokenStream::default() + } + } + + #[inline(always)] + fn emit_dialect_number(&self) -> TokenStream { + if let Some(dialect) = self.dialect { + quote! (pub const DIALECT_NUMBER: u8 = #dialect;) + } else { + TokenStream::default() + } + } + + #[inline(always)] + fn emit_mav_message_parse( + &self, + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { + let id_width = format_ident!("u32"); + + quote! { + fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { + match id { + #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* + _ => { + Err(::mavlink_core::error::ParserError::UnknownMessage { id }) + }, + } + } + } + } + + #[inline(always)] + fn emit_mav_message_crc(&self, id_width: &Ident, structs: &[TokenStream]) -> TokenStream { + quote! { + fn extra_crc(id: #id_width) -> u8 { + match id { + #(#structs::ID => #structs::EXTRA_CRC,)* + _ => { + 0 + }, + } + } + } + } + + #[inline(always)] + fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { + quote! { + fn message_name(&self) -> &'static str { + match self { + #(Self::#enums(..) => #structs::NAME,)* + } + } + } + } + + #[inline(always)] + 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(..) => #structs::ID,)* + } + } + } + } + + #[inline(always)] + fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream { + quote! { + fn message_id_from_name(name: &str) -> Option { + match name { + #(#structs::NAME => Some(#structs::ID),)* + _ => { + None + } + } + } + } + } + + #[inline(always)] + fn emit_mav_message_default_from_id( + &self, + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { + quote! { + fn default_message_from_id(id: u32) -> Option { + match id { + #(#structs::ID => Some(Self::#enums(#structs::default())),)* + _ => { + None + } + } + } + } + } + + #[inline(always)] + fn emit_mav_message_random_from_id( + &self, + enums: &[TokenStream], + structs: &[TokenStream], + ) -> TokenStream { + quote! { + #[cfg(feature = "arbitrary")] + fn random_message_from_id(id: u32, rng: &mut R) -> Option { + match id { + #(#structs::ID => Some(Self::#enums(#structs::random(rng))),)* + _ => None, + } + } + } + } + + #[inline(always)] + fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { + quote! { + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + match self { + #(Self::#enums(body) => body.ser(version, bytes),)* + } + } + } + } + + #[inline(always)] + fn emit_mav_message_target_system_id(&self) -> TokenStream { + let arms: Vec = self + .messages + .values() + .filter(|msg| msg.fields.iter().any(|f| f.name == "target_system")) + .map(|msg| { + let variant = format_ident!("{}", msg.name); + quote!(Self::#variant(inner) => Some(inner.target_system),) + }) + .collect(); + + quote! { + fn target_system_id(&self) -> Option { + match self { + #(#arms)* + _ => None, + } + } + } + } + + #[inline(always)] + fn emit_mav_message_target_component_id(&self) -> TokenStream { + let arms: Vec = self + .messages + .values() + .filter(|msg| msg.fields.iter().any(|f| f.name == "target_component")) + .map(|msg| { + let variant = format_ident!("{}", msg.name); + quote!(Self::#variant(inner) => Some(inner.target_component),) + }) + .collect(); + + quote! { + fn target_component_id(&self) -> Option { + match self { + #(#arms)* + _ => None, + } + } + } + } + + #[inline(always)] + fn emit_mav_message_all_messages(&self) -> TokenStream { + let mut entries = self + .messages + .values() + .map(|msg| (msg.id, msg.emit_struct_name())) + .collect::>(); + + entries.sort_by_key(|(id, _)| *id); + + let pairs = entries + .into_iter() + .map(|(_, struct_name)| quote!((#struct_name::NAME, #struct_name::ID))) + .collect::>(); + + quote! { + pub const fn all_messages() -> &'static [(&'static str, u32)] { + &[#(#pairs),*] + } + } + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavEnum { + pub name: String, + pub description: Option, + pub entries: Vec, + /// If contains Some, the string represents the primitive type (size) for bitflags. + /// If no fields use this enum, the bitmask is true, but primitive is None. In this case + /// regular enum is generated as primitive is unknown. + pub primitive: Option, + pub bitmask: bool, + pub deprecated: Option, +} + +impl MavEnum { + /// Returns true when this enum will be emitted as a `bitflags` struct. + fn is_generated_as_bitflags(&self) -> bool { + self.primitive.is_some() + } + + fn combine(&mut self, enm: &Self) { + for enum_entry in &enm.entries { + if self + .entries + .iter() + .any(|elem| elem.name == enum_entry.name && elem.value == enum_entry.value) + { + panic!("Enum entry {} already exists", enum_entry.name) + } + } + self.entries.append(&mut enm.entries.clone()); + self.entries.sort_by_key(|entry| entry.value); + } + + fn emit_defs(&self) -> Vec { + let mut cnt = 0u64; + self.entries + .iter() + .map(|enum_entry| { + let name = format_ident!("{}", enum_entry.name.clone()); + let value; + + let deprecation = enum_entry.emit_deprecation(); + + let description = if let Some(description) = enum_entry.description.as_ref() { + let description = URL_REGEX.replace_all(description, "<$1>"); + quote!(#[doc = #description]) + } else { + quote!() + }; + + let params_doc = enum_entry.emit_params(); + + if let Some(tmp_value) = enum_entry.value { + cnt = cnt.max(tmp_value); + let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); + value = quote!(#tmp); + } else { + cnt += 1; + value = quote!(#cnt); + } + + if self.is_generated_as_bitflags() { + quote! { + #deprecation + #description + #params_doc + const #name = #value; + } + } else { + quote! { + #deprecation + #description + #params_doc + #name = #value, + } + } + }) + .collect() + } + + #[inline(always)] + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); + quote!(#name) + } + + #[inline(always)] + fn emit_const_default(&self) -> TokenStream { + let default = format_ident!("{}", self.entries[0].name); + quote!(pub const DEFAULT: Self = Self::#default;) + } + + #[inline(always)] + fn emit_deprecation(&self) -> TokenStream { + self.deprecated + .as_ref() + .map(|d| d.emit_tokens()) + .unwrap_or_default() + } + + fn emit_rust(&self) -> TokenStream { + let defs = self.emit_defs(); + let enum_name = self.emit_name(); + let const_default = self.emit_const_default(); + + let deprecated = self.emit_deprecation(); + + let description = if let Some(description) = self.description.as_ref() { + let desc = URL_REGEX.replace_all(description, "<$1>"); + quote!(#[doc = #desc]) + } else { + quote!() + }; + + let mav_bool_impl = if self.name == "MavBool" + && self + .entries + .iter() + .any(|entry| entry.name == "MAV_BOOL_TRUE") + { + if self.is_generated_as_bitflags() { + quote!( + pub fn as_bool(&self) -> bool { + self.contains(Self::MAV_BOOL_TRUE) + } + ) + } else { + quote!( + pub fn as_bool(&self) -> bool { + *self == Self::MAV_BOOL_TRUE + } + ) + } + } else { + quote!() + }; + + let enum_def; + if let Some(primitive) = self.primitive.clone() { + let primitive = format_ident!("{}", primitive); + enum_def = quote! { + bitflags!{ + #[cfg_attr(feature = "ts-rs", derive(TS))] + #[cfg_attr(feature = "ts-rs", ts(export, type = "number"))] + #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] + #[derive(Debug, Copy, Clone, PartialEq)] + #deprecated + #description + pub struct #enum_name: #primitive { + #(#defs)* + } + } + }; + } else { + enum_def = quote! { + #[cfg_attr(feature = "ts-rs", derive(TS))] + #[cfg_attr(feature = "ts-rs", ts(export))] + #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] + #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr(feature = "serde", serde(tag = "type"))] + #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] + #[repr(u32)] + #deprecated + #description + pub enum #enum_name { + #(#defs)* + } + }; + } + + quote! { + #enum_def + + impl #enum_name { + #const_default + #mav_bool_impl + } + + impl Default for #enum_name { + fn default() -> Self { + Self::DEFAULT + } + } + } + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavEnumEntry { + pub value: Option, + pub name: String, + pub description: Option, + pub params: Option>, + pub deprecated: Option, +} + +impl MavEnumEntry { + #[inline(always)] + fn emit_deprecation(&self) -> TokenStream { + self.deprecated + .as_ref() + .map(|d| d.emit_tokens()) + .unwrap_or_default() + } + + #[inline(always)] + fn emit_params(&self) -> TokenStream { + let Some(params) = &self.params else { + return quote!(); + }; + let any_value_range = params.iter().any(|p| { + p.min_value.is_some() + || p.max_value.is_some() + || p.increment.is_some() + || p.enum_used.is_some() + || (p.reserved && p.default.is_some()) + }); + let any_units = params.iter().any(|p| p.units.is_some()); + let lines = params + .iter() + .map(|param| param.emit_doc_row(any_value_range, any_units)); + let mut table_header = "| Parameter | Description |".to_string(); + let mut table_hl = "| --------- | ----------- |".to_string(); + if any_value_range { + table_header += " Values |"; + table_hl += " ------ |"; + } + if any_units { + table_header += " Units |"; + table_hl += " ----- |"; + } + quote! { + #[doc = ""] + #[doc = "# Parameters"] + #[doc = ""] + #[doc = #table_header] + #[doc = #table_hl] + #(#lines)* + } + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavParam { + pub index: usize, + pub description: Option, + pub label: Option, + pub units: Option, + pub enum_used: Option, + pub increment: Option, + pub min_value: Option, + pub max_value: Option, + pub reserved: bool, + pub default: Option, +} + +fn format_number_range(min: Option, max: Option, inc: Option) -> String { + match (min, max, inc) { + (Some(min), Some(max), Some(inc)) => { + if min + inc == max { + format!("{min}, {max}") + } else if min + 2. * inc == max { + format!("{}, {}, {}", min, min + inc, max) + } else { + format!("{}, {}, .. , {}", min, min + inc, max) + } + } + (Some(min), Some(max), None) => format!("{min} .. {max}"), + (Some(min), None, Some(inc)) => format!("{}, {}, ..", min, min + inc), + (None, Some(max), Some(inc)) => format!(".., {}, {}", max - inc, max), + (Some(min), None, None) => format!("≥ {min}"), + (None, Some(max), None) => format!("≤ {max}"), + (None, None, Some(inc)) => format!("Multiples of {inc}"), + (None, None, None) => String::new(), + } +} + +impl MavParam { + fn format_valid_values(&self) -> String { + if let (true, Some(default)) = (self.reserved, self.default) { + format!("Reserved (use {default})") + } else if let Some(enum_used) = &self.enum_used { + format!("[`{enum_used}`]") + } else { + format_number_range(self.min_value, self.max_value, self.increment) + } + } + + fn emit_doc_row(&self, value_range_col: bool, units_col: bool) -> TokenStream { + let label = if let Some(label) = &self.label { + format!("{} ({})", self.index, label) + } else { + format!("{}", self.index) + }; + let mut line = format!( + "| {label:10}| {:12}|", + self.description.as_deref().unwrap_or_default() + ); + if value_range_col { + let range = self.format_valid_values(); + line += &format!(" {range} |"); + } + if units_col { + let units = self.units.clone().unwrap_or_default(); + line += &format!(" {units} |"); + } + quote! {#[doc = #line]} + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavMessage { + pub id: u32, + pub name: String, + pub description: Option, + pub fields: Vec, + pub deprecated: Option, +} + +impl MavMessage { + /// Return Token of "MESSAGE_NAME_DATA + /// for mavlink struct data + fn emit_struct_name(&self) -> TokenStream { + let name = format_ident!("{}", format!("{}_DATA", self.name)); + quote!(#name) + } + + #[inline(always)] + fn emit_name_types(&self) -> (Vec, usize) { + let mut encoded_payload_len: usize = 0; + let field_toks = self + .fields + .iter() + .map(|field| { + let nametype = field.emit_name_type(); + encoded_payload_len += field.mavtype.len(); + + let description = field.emit_description(); + + // 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 { + if field.enumtype.is_some() { + quote!(#[cfg_attr(feature = "serde", serde(default))]) + } else { + quote!(#[cfg_attr(feature = "serde", serde(default = "crate::utils::RustDefault::rust_default"))]) + } + } else { + quote!() + }; + + let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) { + quote!( + #[cfg_attr(feature = "serde", serde(with = "serde_arrays"))] + #[cfg_attr(feature = "ts-rs", ts(type = "Array"))] + ) + } else if matches!(field.mavtype, MavType::CharArray(_)) { + quote!( + #[cfg_attr(feature = "ts-rs", ts(type = "string"))] + ) + } else { + quote!() + }; + + quote! { + #description + #serde_default + #serde_with_attr + #nametype + } + }) + .collect::>(); + (field_toks, encoded_payload_len) + } + + /// Generate description for the given message + #[inline(always)] + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); + if let Some(doc) = self.description.as_ref() { + let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." }); + // create hyperlinks + let doc = URL_REGEX.replace_all(&doc, "<$1>"); + ts.extend(quote!(#[doc = #doc])); + // Leave a blank line before the message ID for readability. + ts.extend(quote!(#[doc = ""])); + } + let id = format!("ID: {}", self.id); + ts.extend(quote!(#[doc = #id])); + ts + } + + #[inline(always)] + fn emit_serialize_vars(&self) -> TokenStream { + let (base_fields, ext_fields): (Vec<_>, Vec<_>) = + self.fields.iter().partition(|f| !f.is_extension); + let ser_vars = base_fields.iter().map(|f| f.rust_writer()); + let ser_ext_vars = ext_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) { + #(#ser_ext_vars)* + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } + } + + #[inline(always)] + fn emit_deserialize_vars(&self) -> TokenStream { + let deser_vars = self + .fields + .iter() + .map(|f| f.rust_reader()) + .collect::>(); + + if deser_vars.is_empty() { + // struct has no fields + quote! { + Ok(Self::default()) + } + } else { + quote! { + 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); + Bytes::new(&payload_buf) + } else { + // fast zero copy + Bytes::new(__input) + }; + + let mut __struct = Self::default(); + #(#deser_vars)* + Ok(__struct) + } + } + } + + #[inline(always)] + 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() + } + } + } + } + + #[inline(always)] + fn emit_deprecation(&self) -> TokenStream { + self.deprecated + .as_ref() + .map(|d| d.emit_tokens()) + .unwrap_or_default() + } + + #[inline(always)] + fn emit_const_default(&self, dialect_has_version: bool) -> TokenStream { + let initializers = self + .fields + .iter() + .map(|field| field.emit_default_initializer(dialect_has_version)); + quote!(pub const DEFAULT: Self = Self { #(#initializers)* };) + } + + fn emit_rust(&self, dialect_has_version: bool) -> 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, payload_encoded_len) = self.emit_name_types(); + assert!( + payload_encoded_len <= 255, + "maximum payload length is 255 bytes" + ); + + let deser_vars = self.emit_deserialize_vars(); + let serialize_vars = self.emit_serialize_vars(); + let const_default = self.emit_const_default(dialect_has_version); + let default_impl = self.emit_default_impl(); + + let deprecation = self.emit_deprecation(); + + let description = self.emit_description(); + + quote! { + #deprecation + #description + #[derive(Debug, Clone, PartialEq)] + #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] + #[cfg_attr(feature = "ts-rs", derive(TS))] + #[cfg_attr(feature = "ts-rs", ts(export))] + pub struct #msg_name { + #(#name_types)* + } + + impl #msg_name { + pub const ENCODED_LEN: usize = #payload_encoded_len; + #const_default + + #[cfg(feature = "arbitrary")] + pub fn random(rng: &mut R) -> Self { + use arbitrary::{Unstructured, Arbitrary}; + let mut buf = [0u8; 1024]; + rng.fill_bytes(&mut buf); + let mut unstructured = Unstructured::new(&buf); + Self::arbitrary(&mut unstructured).unwrap_or_default() + } + } + + #default_impl + + 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 = #payload_encoded_len; + + fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { + #deser_vars + } + + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + #serialize_vars + } + } + } + } + + /// Ensures that a message does not contain duplicate field names. + /// + /// Duplicate field names would generate invalid Rust structs. + pub(crate) fn validate_unique_fields(&self) -> Result<(), MavXMLParseError> { + let mut seen: HashSet<&str> = HashSet::new(); + for f in &self.fields { + let name: &str = &f.name; + if !seen.insert(name) { + return Err(MavXMLParseError::DuplicateFieldName { + field: name.to_string(), + message: self.name.clone(), + }); + } + } + Ok(()) + } + + /// Ensure that the fields count is at least one and no more than 64 + pub(crate) fn validate_field_count(&self) -> Result<(), MavXMLParseError> { + if self.fields.is_empty() || self.fields.len() >= 64 { + Err(MavXMLParseError::InvalidFieldCount { + message: self.name.clone(), + }) + } else { + Ok(()) + } + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavField { + pub mavtype: MavType, + pub name: String, + pub description: Option, + pub enumtype: Option, + pub display: Option, + pub is_extension: bool, +} + +impl MavField { + /// Emit rust name of a given field + #[inline(always)] + fn emit_name(&self) -> TokenStream { + let name = format_ident!("{}", self.name); + quote!(#name) + } + + /// Emit rust type of the field + #[inline(always)] + fn emit_type(&self) -> TokenStream { + let mavtype; + if matches!(self.mavtype, MavType::Array(_, _)) { + let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); + mavtype = quote!(#rt); + } else if let Some(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 + } + + /// Generate description for the given field + #[inline(always)] + fn emit_description(&self) -> TokenStream { + let mut ts = TokenStream::new(); + if let Some(val) = self.description.as_ref() { + let desc = URL_REGEX.replace_all(val, "<$1>"); + ts.extend(quote!(#[doc = #desc])); + } + ts + } + + /// Combine rust name and type of a given field + #[inline(always)] + 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) -> TokenStream { + let mut name = "self.".to_string() + &self.name.clone(); + if self.enumtype.is_some() { + // 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() as "; + name += &self.mavtype.rust_type(); + } else { + panic!("Display option not implemented"); + } + } else { + // 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) + } + + /// Emit reader + 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 { + // 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 + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); + let enum_name_ident = format_ident!("{}", enum_name); + quote! { + #tmp + #name = #enum_name_ident::from_bits(tmp as <#enum_name_ident as Flags>::Bits) + .ok_or(::mavlink_core::error::ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u64 })?; + } + } else { + panic!("Display option not implemented"); + } + } else { + // handle enum by FromPrimitive + let tmp = self.mavtype.rust_reader("e!(let tmp), buf); + let val = format_ident!("from_{}", &self.mavtype.rust_type()); + quote!( + #tmp + #name = FromPrimitive::#val(tmp) + .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u64 })?; + ) + } + } else { + self.mavtype.rust_reader(&name, buf) + } + } + + #[inline(always)] + fn emit_default_initializer(&self, dialect_has_version: bool) -> 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(dialect_has_version); + quote!(#field: #default_value,) + } else if let Some(enumname) = &self.enumtype { + let ty = TokenStream::from_str(enumname).unwrap(); + quote!(#field: #ty::DEFAULT,) + } else { + let default_value = self.mavtype.emit_default_value(dialect_has_version); + quote!(#field: #default_value,) + } + } +} + +#[derive(Debug, PartialEq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub enum MavType { + UInt8MavlinkVersion, + #[default] + UInt8, + UInt16, + UInt32, + UInt64, + Int8, + Int16, + Int32, + Int64, + Char, + Float, + Double, + CharArray(usize), + Array(Box, usize), +} + +impl MavType { + pub(crate) fn parse_type(s: &str) -> Option { + use self::MavType::*; + match s { + "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion), + "uint8_t" => Some(UInt8), + "uint16_t" => Some(UInt16), + "uint32_t" => Some(UInt32), + "uint64_t" => Some(UInt64), + "int8_t" => Some(Int8), + "int16_t" => Some(Int16), + "int32_t" => Some(Int32), + "int64_t" => Some(Int64), + "char" => Some(Char), + "float" => Some(Float), + "Double" => Some(Double), + "double" => Some(Double), + _ if s.starts_with("char[") => { + let start = 4; + let size = s[start + 1..(s.len() - 1)].parse::().ok()?; + Some(CharArray(size)) + } + _ if s.ends_with(']') => { + 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)) + } + _ => None, + } + } + + /// Emit reader of a given type + pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream { + use self::MavType::*; + match self { + 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()?;}, + UInt64 => quote! {#val = #buf.get_u64_le()?;}, + UInt8MavlinkVersion => quote! {#val = #buf.get_u8()?;}, + Int8 => quote! {#val = #buf.get_i8()?;}, + Int16 => quote! {#val = #buf.get_i16_le()?;}, + Int32 => quote! {#val = #buf.get_i32_le()?;}, + Int64 => quote! {#val = #buf.get_i64_le()?;}, + Float => quote! {#val = #buf.get_f32_le()?;}, + Double => quote! {#val = #buf.get_f64_le()?;}, + CharArray(size) => { + quote! { + let mut tmp = [0_u8; #size]; + for v in &mut tmp { + *v = #buf.get_u8()?; + } + #val = CharArray::new(tmp); + } + } + Array(t, _) => { + let r = t.rust_reader("e!(let val), buf); + quote! { + for v in &mut #val { + #r + *v = val; + } + } + } + } + } + + /// Emit writer of a given type + pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream { + use self::MavType::*; + match self { + UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, + UInt8 => quote! {#buf.put_u8(#val);}, + 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);}, + Int16 => quote! {#buf.put_i16_le(#val);}, + Int32 => quote! {#buf.put_i32_le(#val);}, + Float => quote! {#buf.put_f32_le(#val);}, + UInt64 => quote! {#buf.put_u64_le(#val);}, + Int64 => quote! {#buf.put_i64_le(#val);}, + Double => quote! {#buf.put_f64_le(#val);}, + CharArray(_) => { + let w = Char.rust_writer("e!(*val), buf); + quote! { + for val in &#val { + #w + } + } + } + Array(t, _size) => { + let w = t.rust_writer("e!(*val), buf); + quote! { + for val in &#val { + #w + } + } + } + } + } + + /// Size of a given Mavtype + fn len(&self) -> usize { + use self::MavType::*; + match self { + UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, + UInt16 | Int16 => 2, + UInt32 | Int32 | Float => 4, + UInt64 | Int64 | Double => 8, + CharArray(size) => *size, + Array(t, size) => t.len() * size, + } + } + + pub(crate) fn max_int_value(&self) -> u64 { + match self { + Self::UInt8MavlinkVersion | Self::UInt8 => u8::MAX as u64, + Self::UInt16 => u16::MAX as u64, + Self::UInt32 => u32::MAX as u64, + Self::UInt64 => u64::MAX, + Self::Int8 | Self::Char | Self::CharArray(_) => i8::MAX as u64, + Self::Int16 => i16::MAX as u64, + Self::Int32 => i32::MAX as u64, + Self::Int64 => i64::MAX as u64, + // maximum precisly representable value minus 1 for float types + Self::Float => (1 << f32::MANTISSA_DIGITS) - 1, + Self::Double => (1 << f64::MANTISSA_DIGITS) - 1, + Self::Array(mav_type, _) => mav_type.max_int_value(), + } + } + + /// Used for ordering of types + fn order_len(&self) -> usize { + use self::MavType::*; + match self { + UInt8MavlinkVersion | UInt8 | Int8 | Char | CharArray(_) => 1, + UInt16 | Int16 => 2, + UInt32 | Int32 | Float => 4, + UInt64 | Int64 | Double => 8, + Array(t, _) => t.len(), + } + } + + /// Used for crc calculation + pub fn primitive_type(&self) -> String { + use self::MavType::*; + match self { + UInt8MavlinkVersion => "uint8_t".into(), + UInt8 => "uint8_t".into(), + Int8 => "int8_t".into(), + Char => "char".into(), + UInt16 => "uint16_t".into(), + Int16 => "int16_t".into(), + UInt32 => "uint32_t".into(), + Int32 => "int32_t".into(), + Float => "float".into(), + UInt64 => "uint64_t".into(), + Int64 => "int64_t".into(), + Double => "double".into(), + CharArray(_) => "char".into(), + Array(t, _) => t.primitive_type(), + } + } + + /// Return rust equivalent of a given Mavtype + /// Used for generating struct fields. + pub fn rust_type(&self) -> String { + use self::MavType::*; + match self { + UInt8 | UInt8MavlinkVersion => "u8".into(), + Int8 => "i8".into(), + Char => "u8".into(), + UInt16 => "u16".into(), + Int16 => "i16".into(), + UInt32 => "u32".into(), + Int32 => "i32".into(), + Float => "f32".into(), + UInt64 => "u64".into(), + Int64 => "i64".into(), + Double => "f64".into(), + CharArray(size) => format!("CharArray<{size}>"), + Array(t, size) => format!("[{};{}]", t.rust_type(), size), + } + } + + pub fn emit_default_value(&self, dialect_has_version: bool) -> TokenStream { + use self::MavType::*; + match self { + UInt8 => quote!(0_u8), + UInt8MavlinkVersion => { + if dialect_has_version { + quote!(MINOR_MAVLINK_VERSION) + } else { + 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), + CharArray(size) => quote!(CharArray::new([0_u8; #size])), + Array(ty, size) => { + let default_value = ty.emit_default_value(dialect_has_version); + quote!([#default_value; #size]) + } + } + } + + /// 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(); + (-(len as isize)).cmp(&(-(other.order_len() as isize))) + } +} + +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[derive(Debug, PartialEq, Eq, Clone, Default, Copy)] +pub enum MavDeprecationType { + #[default] + Deprecated, + Superseded, +} + +impl Display for MavDeprecationType { + fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { + match self { + Self::Deprecated => f.write_str("Deprecated"), + Self::Superseded => f.write_str("Superseded"), + } + } +} + +#[derive(Debug, PartialEq, Eq, Clone, Default)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +pub struct MavDeprecation { + // YYYY-MM + pub since: String, + pub replaced_by: Option, + pub deprecation_type: MavDeprecationType, + pub note: Option, +} + +impl MavDeprecation { + pub fn emit_tokens(&self) -> TokenStream { + let since = &self.since; + let note = match &self.note { + Some(str) if str.is_empty() || str.ends_with(".") => str.clone(), + Some(str) => format!("{str}."), + None => String::new(), + }; + let replaced_by = match &self.replaced_by { + Some(str) if str.starts_with('`') => format!("See {str}"), + Some(str) => format!("See `{str}`"), + None => String::new(), + }; + let message = format!( + "{note} {replaced_by} ({} since {since})", + self.deprecation_type + ); + quote!(#[deprecated = #message]) + } +} + +/// 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, +) -> Result<(), BindGenError> { + let mut parsed_files: HashSet = HashSet::new(); + let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?; + + let dialect_name = util::to_dialect_name(definition_file); + + // rust file + let rust_tokens = profile.emit_rust(&dialect_name); + writeln!(output_rust, "{rust_tokens}").unwrap(); + + Ok(()) +} + +/// CRC operates over names of the message and names of its fields +/// Hence we have to preserve the original uppercase names delimited with an underscore +/// For field names, we replace "type" with "mavtype" to make it rust compatible (this is +/// needed for generating sensible rust code), but for calculating crc function we have to +/// use the original name "type" +pub fn extra_crc(msg: &MavMessage) -> u8 { + // calculate a 8-bit checksum of the key fields of a message, so we + // can detect incompatible XML changes + let mut crc = CRCu16::crc16mcrf4cc(); + + crc.digest(msg.name.as_bytes()); + crc.digest(b" "); + + let mut f = msg.fields.clone(); + // only mavlink 1 fields should be part of the extra_crc + f.retain(|f| !f.is_extension); + f.sort_by(|a, b| a.mavtype.compare(&b.mavtype)); + for field in &f { + crc.digest(field.mavtype.primitive_type().as_bytes()); + crc.digest(b" "); + if field.name == "mavtype" { + crc.digest(b"type"); + } else { + crc.digest(field.name.as_bytes()); + } + crc.digest(b" "); + if let MavType::Array(_, size) | MavType::CharArray(size) = field.mavtype { + crc.digest(&[size as u8]); + } + } + + let crcval = crc.get_crc(); + ((crcval & 0xFF) ^ (crcval >> 8)) as u8 +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn emits_target_id_match_arms() { + // Build a minimal profile containing one message with target fields and one without + let mut profile = MavProfile::default(); + + let msg_with_targets = MavMessage { + id: 300, + name: "COMMAND_INT".to_string(), + description: None, + fields: vec![ + MavField { + mavtype: MavType::UInt8, + name: "target_system".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + MavField { + mavtype: MavType::UInt8, + name: "target_component".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + ], + deprecated: None, + }; + + let msg_without_targets = MavMessage { + id: 0, + name: "HEARTBEAT".to_string(), + description: None, + fields: vec![MavField { + mavtype: MavType::UInt32, + name: "custom_mode".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }], + deprecated: None, + }; + + profile.add_message(&msg_with_targets); + profile.add_message(&msg_without_targets); + + let tokens = profile.emit_rust("common"); + let mut code = tokens.to_string(); + code.retain(|c| !c.is_whitespace()); + + // Check the code contains the target_system/component_id functions + assert!(code.contains("fntarget_system_id(&self)->Option")); + assert!(code.contains("fntarget_component_id(&self)->Option")); + + // Check the generated impl contains arms referencing COMMAND_INT(inner).target_system/component + assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_system)")); + assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_component)")); + + // Ensure a message without target fields returns None + assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_system)")); + assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_component)")); + } + + #[test] + fn validate_unique_fields_allows_unique() { + let msg = MavMessage { + id: 1, + name: "FOO".to_string(), + description: None, + fields: vec![ + MavField { + mavtype: MavType::UInt8, + name: "a".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + MavField { + mavtype: MavType::UInt16, + name: "b".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + ], + deprecated: None, + }; + // Should not panic + msg.validate_unique_fields().unwrap(); + } + + #[test] + #[should_panic(expected = "DuplicateFieldName")] + fn validate_unique_fields_panics_on_duplicate() { + let msg = MavMessage { + id: 2, + name: "BAR".to_string(), + description: None, + fields: vec![ + MavField { + mavtype: MavType::UInt8, + name: "target_system".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + MavField { + mavtype: MavType::UInt8, + name: "target_system".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + ], + deprecated: None, + }; + // Should panic due to duplicate field names + msg.validate_unique_fields().unwrap(); + } + + #[test] + fn validate_field_count_ok() { + let msg = MavMessage { + id: 2, + name: "FOO".to_string(), + description: None, + fields: vec![ + MavField { + mavtype: MavType::UInt8, + name: "a".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + MavField { + mavtype: MavType::UInt8, + name: "b".to_string(), + description: None, + enumtype: None, + display: None, + is_extension: false, + }, + ], + deprecated: None, + }; + // Should not panic + msg.validate_field_count().unwrap(); + } + + #[test] + #[should_panic] + fn validate_field_count_too_many() { + let mut fields = vec![]; + for i in 0..65 { + let field = MavField { + mavtype: MavType::UInt8, + name: format!("field_{i}"), + description: None, + enumtype: None, + display: None, + is_extension: false, + }; + fields.push(field); + } + let msg = MavMessage { + id: 2, + name: "BAZ".to_string(), + description: None, + fields, + deprecated: None, + }; + // Should panic due to 65 fields + msg.validate_field_count().unwrap(); + } + + #[test] + #[should_panic] + fn validate_field_count_empty() { + let msg = MavMessage { + id: 2, + name: "BAM".to_string(), + description: None, + fields: vec![], + deprecated: None, + }; + // Should panic due to no fields + msg.validate_field_count().unwrap(); + } + + #[test] + fn test_fmt_mav_param_values() { + let enum_param = MavParam { + enum_used: Some("ENUM_NAME".to_string()), + ..Default::default() + }; + assert_eq!(enum_param.format_valid_values(), "[`ENUM_NAME`]"); + + let reserved_param = MavParam { + reserved: true, + default: Some(f32::NAN), + ..Default::default() + }; + assert_eq!(reserved_param.format_valid_values(), "Reserved (use NaN)"); + + let unrestricted_param = MavParam::default(); + assert_eq!(unrestricted_param.format_valid_values(), ""); + + let int_param = MavParam { + increment: Some(1.0), + ..Default::default() + }; + assert_eq!(int_param.format_valid_values(), "Multiples of 1"); + + let pos_param = MavParam { + min_value: Some(0.0), + ..Default::default() + }; + assert_eq!(pos_param.format_valid_values(), "≥ 0"); + + let max_param = MavParam { + max_value: Some(5.5), + ..Default::default() + }; + assert_eq!(max_param.format_valid_values(), "≤ 5.5"); + + let pos_int_param = MavParam { + min_value: Some(0.0), + increment: Some(1.0), + ..Default::default() + }; + assert_eq!(pos_int_param.format_valid_values(), "0, 1, .."); + + let max_inc_param = MavParam { + increment: Some(1.0), + max_value: Some(360.0), + ..Default::default() + }; + assert_eq!(max_inc_param.format_valid_values(), ".., 359, 360"); + + let range_param = MavParam { + min_value: Some(0.0), + max_value: Some(10.0), + ..Default::default() + }; + assert_eq!(range_param.format_valid_values(), "0 .. 10"); + + let int_range_param = MavParam { + min_value: Some(0.0), + max_value: Some(10.0), + increment: Some(1.0), + ..Default::default() + }; + assert_eq!(int_range_param.format_valid_values(), "0, 1, .. , 10"); + + let close_inc_range_param = MavParam { + min_value: Some(-2.0), + max_value: Some(2.0), + increment: Some(2.0), + ..Default::default() + }; + assert_eq!(close_inc_range_param.format_valid_values(), "-2, 0, 2"); + + let bin_range_param = MavParam { + min_value: Some(0.0), + max_value: Some(1.0), + increment: Some(1.0), + ..Default::default() + }; + assert_eq!(bin_range_param.format_valid_values(), "0, 1"); + } + + #[test] + fn test_emit_doc_row() { + let param = MavParam { + index: 3, + label: Some("test param".to_string()), + min_value: Some(0.0), + units: Some("m/s".to_string()), + ..Default::default() + }; + // test with all variations of columns + assert_eq!( + param.emit_doc_row(false, false).to_string(), + quote! {#[doc = "| 3 (test param)| |"]}.to_string() + ); + assert_eq!( + param.emit_doc_row(false, true).to_string(), + quote! {#[doc = "| 3 (test param)| | m/s |"]}.to_string() + ); + assert_eq!( + param.emit_doc_row(true, false).to_string(), + quote! {#[doc = "| 3 (test param)| | ≥ 0 |"]}.to_string() + ); + assert_eq!( + param.emit_doc_row(true, true).to_string(), + quote! {#[doc = "| 3 (test param)| | ≥ 0 | m/s |"]}.to_string() + ); + + let unlabeled_param = MavParam { + index: 2, + ..Default::default() + }; + assert_eq!( + unlabeled_param.emit_doc_row(false, false).to_string(), + quote! {#[doc = "| 2 | |"]}.to_string() + ); + } +} diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs index 155839f70fd..0cd502d30c8 100644 --- a/mavlink-bindgen/src/error.rs +++ b/mavlink-bindgen/src/error.rs @@ -3,7 +3,7 @@ use std::num::{ParseFloatError, ParseIntError}; use quick_xml::events::Event; use thiserror::Error; -use crate::parser_new::MavXmlElement; +use crate::parser::MavXmlElement; #[derive(Error, Debug)] pub enum BindGenError { diff --git a/mavlink-bindgen/src/lib.rs b/mavlink-bindgen/src/lib.rs index a5219f69766..c40dacc188c 100644 --- a/mavlink-bindgen/src/lib.rs +++ b/mavlink-bindgen/src/lib.rs @@ -6,9 +6,9 @@ use std::path::{Path, PathBuf}; use std::process::Command; pub mod binder; +pub mod codegen; pub mod error; -pub mod parser; -mod parser_new; +mod parser; mod util; #[derive(Debug)] @@ -174,7 +174,7 @@ fn generate_single_file, P2: AsRef>( })?); // codegen - parser::generate(definitions_dir, &definition_filename, &mut outf)?; + codegen::generate(definitions_dir, &definition_filename, &mut outf)?; Ok(GeneratedBinding { module_name, diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 294afdfa109..4873ba86cfd 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -1,1892 +1,835 @@ -use crc_any::CRCu16; -use std::cmp::Ordering; -use std::collections::btree_map::Entry; -use std::collections::{BTreeMap, HashSet}; -use std::default::Default; -use std::fmt::Display; -use std::io::Write; -use std::path::{Path, PathBuf}; -use std::str::FromStr; -use std::sync::LazyLock; - -use regex::Regex; - -use proc_macro2::{Ident, TokenStream}; -use quote::{format_ident, quote}; - -#[cfg(feature = "serde")] -use serde::{Deserialize, Serialize}; - -use crate::error::{BindGenError, MavXMLParseError}; -use crate::parser_new::parse_profile; -use crate::util; - -static URL_REGEX: LazyLock = LazyLock::new(|| { - Regex::new(concat!( - r"(https?://", // url scheme - r"([-a-zA-Z0-9@:%._\+~#=]{2,256}\.)+", // one or more subdomains - r"[a-zA-Z]{2,63}", // root domain - r"\b([-a-zA-Z0-9@:%_\+.~#?&/=]*[-a-zA-Z0-9@:%_\+~#?&/=])?)", // optional query or url fragments - )) - .expect("failed to build regex") -}); - -#[derive(Debug, PartialEq, Clone, Default)] +use std::{ + collections::{BTreeMap, HashSet}, + path::{Path, PathBuf}, +}; + +use quick_xml::{ + events::{BytesStart, Event}, + name::QName, + Reader, +}; + +use crate::{ + codegen::{ + MavDeprecation, MavDeprecationType, MavEnum, MavEnumEntry, MavField, MavMessage, MavParam, + MavProfile, MavType, + }, + error::MavXMLParseError, + BindGenError, +}; + +#[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavProfile { - pub messages: BTreeMap, - pub enums: BTreeMap, - pub version: Option, - pub dialect: Option, +#[cfg_attr(feature = "serde", serde(tag = "type"))] +pub enum MavXmlElement { + Version, + Mavlink, + Dialect, + Include, + Enums, + Enum, + Entry, + Description, + Param, + Messages, + Message, + Field, + Deprecated, + Wip, + Extensions, + Superseded, } -impl MavProfile { - pub(crate) fn add_message(&mut self, message: &MavMessage) { - match self.messages.entry(message.name.clone()) { - Entry::Occupied(entry) => { - assert!( - entry.get() == message, - "Message '{}' defined twice but definitions are different", - message.name - ); - } - Entry::Vacant(entry) => { - entry.insert(message.clone()); - } - } - } - - pub(crate) fn add_enum(&mut self, enm: &MavEnum) { - match self.enums.entry(enm.name.clone()) { - Entry::Occupied(entry) => { - entry.into_mut().combine(enm); - } - Entry::Vacant(entry) => { - entry.insert(enm.clone()); - } - } - } - - //TODO verify this is no longer necessary since we're supporting both mavlink1 and mavlink2 - // ///If we are not using Mavlink v2, remove messages with id's > 254 - // fn update_messages(mut self) -> Self { - // //println!("Updating messages"); - // let msgs = self.messages.into_iter().filter( - // |x| x.id <= 254).collect::>(); - // self.messages = msgs; - // self - // } - - /// Simple header comment - #[inline(always)] - fn emit_comments(&self, dialect_name: &str) -> TokenStream { - let message = format!("MAVLink {dialect_name} dialect."); - quote!( - #![doc = #message] - #![doc = ""] - #![doc = "This file was automatically generated, do not edit."] - ) - } - - /// Emit rust messages - #[inline(always)] - fn emit_msgs(&self) -> Vec { - self.messages - .values() - .map(|d| d.emit_rust(self.version.is_some())) - .collect() - } - - /// Emit rust enums - #[inline(always)] - fn emit_enums(&self) -> Vec { - self.enums.values().map(|d| d.emit_rust()).collect() - } - - #[inline(always)] - fn emit_deprecations(&self) -> Vec { - self.messages - .values() - .map(|msg| { - msg.deprecated - .as_ref() - .map(|d| d.emit_tokens()) - .unwrap_or_default() - }) - .collect() +const fn id_element(name: QName<'_>) -> Option { + use self::MavXmlElement::*; + match name.into_inner() { + 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), + b"superseded" => Some(Superseded), + _ => None, } +} - /// Get list of original message names - #[inline(always)] - fn emit_enum_names(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let name = format_ident!("{}", msg.name); - quote!(#name) - }) - .collect() - } +#[cfg(not(feature = "mav2-message-extensions"))] +struct ExtensionFilter { + pub is_in: bool, +} - /// Emit message names with "_DATA" at the end - #[inline(always)] - fn emit_struct_names(&self) -> Vec { - self.messages - .values() - .map(|msg| msg.emit_struct_name()) - .collect() - } +struct MessageFilter { + pub is_in: bool, + pub messages: Vec, +} - fn emit_rust(&self, dialect_name: &str) -> TokenStream { - //TODO verify that id_width of u8 is OK even in mavlink v1 - let id_width = format_ident!("u32"); - - let comment = self.emit_comments(dialect_name); - let mav_minor_version = self.emit_minor_version(); - let mav_dialect_number = self.emit_dialect_number(); - let msgs = self.emit_msgs(); - let deprecations = self.emit_deprecations(); - let enum_names = self.emit_enum_names(); - let struct_names = self.emit_struct_names(); - let enums = self.emit_enums(); - - let variant_docs = self.emit_variant_description(); - - let mav_message = - self.emit_mav_message(&variant_docs, &deprecations, &enum_names, &struct_names); - let mav_message_all_ids = self.emit_mav_message_all_ids(); - let mav_message_all_messages = self.emit_mav_message_all_messages(); - 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, &struct_names); - let mav_message_random_from_id = - self.emit_mav_message_random_from_id(&enum_names, &struct_names); - let mav_message_serialize = self.emit_mav_message_serialize(&enum_names); - let mav_message_target_system_id = self.emit_mav_message_target_system_id(); - let mav_message_target_component_id = self.emit_mav_message_target_component_id(); - - quote! { - #comment - #![allow(deprecated)] - #![allow(clippy::match_single_binding)] - #[allow(unused_imports)] - use num_derive::{FromPrimitive, ToPrimitive}; - #[allow(unused_imports)] - use num_traits::{FromPrimitive, ToPrimitive}; - #[allow(unused_imports)] - use bitflags::{bitflags, Flags}; - #[allow(unused_imports)] - use mavlink_core::{MavlinkVersion, Message, MessageData, bytes::Bytes, bytes_mut::BytesMut, types::CharArray}; - - #[cfg(feature = "serde")] - use serde::{Serialize, Deserialize}; - - #[cfg(feature = "arbitrary")] - use arbitrary::Arbitrary; - - #[cfg(feature = "ts-rs")] - use ts_rs::TS; - - #mav_minor_version - #mav_dialect_number - - #(#enums)* - - #(#msgs)* - - #[derive(Clone, PartialEq, Debug)] - #mav_message - - impl MavMessage { - #mav_message_all_ids - #mav_message_all_messages - } +struct MavXmlFilter { + #[cfg(not(feature = "mav2-message-extensions"))] + extension_filter: ExtensionFilter, + message_filter: MessageFilter, +} - impl Message for MavMessage { - #mav_message_parse - #mav_message_name - #mav_message_id - #mav_message_id_from_name - #mav_message_default_from_id - #mav_message_random_from_id - #mav_message_serialize - #mav_message_crc - #mav_message_target_system_id - #mav_message_target_component_id - } +impl Default for MavXmlFilter { + fn default() -> Self { + Self { + #[cfg(not(feature = "mav2-message-extensions"))] + extension_filter: ExtensionFilter { is_in: false }, + message_filter: MessageFilter::new(), } } +} - #[inline(always)] - fn emit_mav_message( - &self, - docs: &[TokenStream], - deprecations: &[TokenStream], - enums: &[TokenStream], - structs: &[TokenStream], - ) -> TokenStream { - quote! { - #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] - #[cfg_attr(feature = "serde", serde(tag = "type"))] - #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] - #[cfg_attr(feature = "ts-rs", derive(TS))] - #[cfg_attr(feature = "ts-rs", ts(export))] - #[repr(u32)] - pub enum MavMessage { - #(#docs #deprecations #enums(#structs),)* - } +impl MessageFilter { + fn new() -> Self { + Self { + is_in: false, + messages: vec![ + // device_cap_flags is u32, when enum is u16, which is not handled by the parser yet + "STORM32_GIMBAL_MANAGER_INFORMATION".to_string(), + ], } } +} - fn emit_variant_description(&self) -> Vec { - self.messages - .values() - .map(|msg| { - let mut ts = TokenStream::new(); - - if let Some(doc) = msg.description.as_ref() { - let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." }); - let doc = URL_REGEX.replace_all(&doc, "<$1>"); - ts.extend(quote!(#[doc = #doc])); - - // Leave a blank line before the message ID for readability. - ts.extend(quote!(#[doc = ""])); +impl MavXmlFilter { + fn filter(&mut self, event: &Event) -> bool { + self.filter_extension(event) + && self.filter_messages(event) + && Self::filter_event_type(event) + } + + #[cfg(feature = "mav2-message-extensions")] + fn filter_extension(&mut self, _event: &Event) -> bool { + true + } + + /// Ignore extension fields + #[cfg(not(feature = "mav2-message-extensions"))] + fn filter_extension(&mut self, event: &Event) -> bool { + match event { + Event::Start(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Extensions) => { + self.extension_filter.is_in = true; + } + Event::End(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Message) => { + self.extension_filter.is_in = false; + } + _ => {} + } + !self.extension_filter.is_in + } + + /// Filters messages by their name + fn filter_messages(&mut self, event: &Event) -> bool { + match event { + Event::Start(bytes) if Some(MavXmlElement::Message) == id_element(bytes.name()) => { + let name_attr = bytes + .attributes() + .filter_map(|attr| attr.ok()) + .find(|attr| attr.key.into_inner() == b"name"); + if let Some(name_attr) = name_attr { + let value = String::from_utf8_lossy(&name_attr.value); + if self + .message_filter + .messages + .iter() + .any(|filter| filter.as_str() == value) + { + self.message_filter.is_in = true; + return false; + } } - - let id = format!("ID: {}", msg.id); - ts.extend(quote!(#[doc = #id])); - - ts - }) - .collect() - } - - #[inline(always)] - fn emit_mav_message_all_ids(&self) -> TokenStream { - let mut message_ids = self.messages.values().map(|m| m.id).collect::>(); - message_ids.sort(); - - quote!( - pub const fn all_ids() -> &'static [u32] { - &[#(#message_ids),*] } - ) - } - - #[inline(always)] - fn emit_minor_version(&self) -> TokenStream { - if let Some(version) = self.version { - quote! (pub const MINOR_MAVLINK_VERSION: u8 = #version;) - } else { - TokenStream::default() - } - } - - #[inline(always)] - fn emit_dialect_number(&self) -> TokenStream { - if let Some(dialect) = self.dialect { - quote! (pub const DIALECT_NUMBER: u8 = #dialect;) - } else { - TokenStream::default() - } - } - - #[inline(always)] - fn emit_mav_message_parse( - &self, - enums: &[TokenStream], - structs: &[TokenStream], - ) -> TokenStream { - let id_width = format_ident!("u32"); - - quote! { - fn parse(version: MavlinkVersion, id: #id_width, payload: &[u8]) -> Result { - match id { - #(#structs::ID => #structs::deser(version, payload).map(Self::#enums),)* - _ => { - Err(::mavlink_core::error::ParserError::UnknownMessage { id }) - }, - } + Event::End(bytes) + if Some(MavXmlElement::Message) == id_element(bytes.name()) + && self.message_filter.is_in => + { + self.message_filter.is_in = false; + return false; } + _ => {} } + !self.message_filter.is_in } - #[inline(always)] - fn emit_mav_message_crc(&self, id_width: &Ident, structs: &[TokenStream]) -> TokenStream { - quote! { - fn extra_crc(id: #id_width) -> u8 { - match id { - #(#structs::ID => #structs::EXTRA_CRC,)* - _ => { - 0 - }, - } - } - } + fn filter_event_type(event: &Event) -> bool { + !matches!( + event, + Event::Comment(_) | Event::Decl(_) | Event::CData(_) | Event::DocType(_) | Event::PI(_) + ) } +} - #[inline(always)] - fn emit_mav_message_name(&self, enums: &[TokenStream], structs: &[TokenStream]) -> TokenStream { - quote! { - fn message_name(&self) -> &'static str { - match self { - #(Self::#enums(..) => #structs::NAME,)* - } - } - } - } +struct MavCompositeProfile { + profile: MavProfile, + includes: Vec, +} - #[inline(always)] - 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(..) => #structs::ID,)* +impl MavCompositeProfile { + fn resolve_includes( + mut self, + definitions_dir: &Path, + parsed_files: &mut HashSet, + ) -> Result { + for include in &self.includes { + let include = PathBuf::from(include.trim()); + 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() { + self.profile.add_message(message); } - } - } - } - - #[inline(always)] - fn emit_mav_message_id_from_name(&self, structs: &[TokenStream]) -> TokenStream { - quote! { - fn message_id_from_name(name: &str) -> Option { - match name { - #(#structs::NAME => Some(#structs::ID),)* - _ => { - None - } + for enm in included_profile.enums.values() { + self.profile.add_enum(enm); + } + if self.profile.version.is_none() { + self.profile.version = included_profile.version; } } } + Ok(self.profile) } +} - #[inline(always)] - fn emit_mav_message_default_from_id( - &self, - enums: &[TokenStream], - structs: &[TokenStream], - ) -> TokenStream { - quote! { - fn default_message_from_id(id: u32) -> Option { - match id { - #(#structs::ID => Some(Self::#enums(#structs::default())),)* - _ => { - None +impl MavProfile { + /// Go over all fields in the messages, and if you encounter an enum, + /// which is a bitmask, set the bitmask size based on field size + fn update_bitmask_enum_fields(mut self, definition_file: &Path) -> Result { + for msg in self.messages.values_mut() { + for field in &mut msg.fields { + if let Some(enum_name) = &field.enumtype { + // find the corresponding enum + if let Some(enm) = self.enums.get_mut(enum_name) { + // Handle legacy definition where bitmask is defined as display="bitmask" + if field.display == Some("bitmask".to_string()) { + enm.bitmask = true; + } + + // it is a bitmask + if enm.bitmask { + enm.primitive = Some(field.mavtype.rust_primitive_type()); + + // check if all enum values can be stored in the fields + for entry in &enm.entries { + if entry.value.unwrap_or_default() > field.mavtype.max_int_value() { + return Err(BindGenError::BitflagEnumTypeToNarrow { + field_name: field.name.clone(), + message_name: msg.name.clone(), + enum_name: enum_name.to_string(), + }); + } + } + + // Fix fields in backwards manner + if field.display.is_none() { + field.display = Some("bitmask".to_string()); + } + } + } else { + return Err(BindGenError::InvalidEnumReference { + enum_name: enum_name.to_string(), + path: definition_file.to_path_buf(), + }); } } } } + Ok(self) } +} - #[inline(always)] - fn emit_mav_message_random_from_id( - &self, - enums: &[TokenStream], - structs: &[TokenStream], - ) -> TokenStream { - quote! { - #[cfg(feature = "arbitrary")] - fn random_message_from_id(id: u32, rng: &mut R) -> Option { - match id { - #(#structs::ID => Some(Self::#enums(#structs::random(rng))),)* - _ => None, - } - } - } - } +pub fn parse_profile( + definitions_dir: &Path, + definition_file: &Path, + parsed_files: &mut HashSet, +) -> Result { + let is_top_level = parsed_files.is_empty(); + let in_path = Path::new(&definitions_dir).join(definition_file); + // Keep track of which files have been parsed + parsed_files.insert(in_path.clone()); - #[inline(always)] - fn emit_mav_message_serialize(&self, enums: &Vec) -> TokenStream { - quote! { - fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { - match self { - #(Self::#enums(body) => body.ser(version, bytes),)* - } - } + let xml = std::fs::read_to_string(&in_path).map_err(|e| { + BindGenError::CouldNotReadDefinitionFile { + source: e, + path: in_path.clone(), } - } + })?; - #[inline(always)] - fn emit_mav_message_target_system_id(&self) -> TokenStream { - let arms: Vec = self - .messages - .values() - .filter(|msg| msg.fields.iter().any(|f| f.name == "target_system")) - .map(|msg| { - let variant = format_ident!("{}", msg.name); - quote!(Self::#variant(inner) => Some(inner.target_system),) - }) - .collect(); - - quote! { - fn target_system_id(&self) -> Option { - match self { - #(#arms)* - _ => None, - } - } - } - } + let mut reader = Reader::from_str(&xml); - #[inline(always)] - fn emit_mav_message_target_component_id(&self) -> TokenStream { - let arms: Vec = self - .messages - .values() - .filter(|msg| msg.fields.iter().any(|f| f.name == "target_component")) - .map(|msg| { - let variant = format_ident!("{}", msg.name); - quote!(Self::#variant(inner) => Some(inner.target_component),) - }) - .collect(); - - quote! { - fn target_component_id(&self) -> Option { - match self { - #(#arms)* - _ => None, - } - } - } - } + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; - #[inline(always)] - fn emit_mav_message_all_messages(&self) -> TokenStream { - let mut entries = self - .messages - .values() - .map(|msg| (msg.id, msg.emit_struct_name())) - .collect::>(); + let mut parser = Parser::from_reader(reader); - entries.sort_by_key(|(id, _)| *id); + let profile = parser + .parse() + .map_err(|source| BindGenError::InvalidDefinitionFile { + source, + path: definition_file.to_path_buf(), + })?; - let pairs = entries - .into_iter() - .map(|(_, struct_name)| quote!((#struct_name::NAME, #struct_name::ID))) - .collect::>(); + let profile = profile.resolve_includes(definitions_dir, parsed_files)?; - quote! { - pub const fn all_messages() -> &'static [(&'static str, u32)] { - &[#(#pairs),*] - } - } + if is_top_level { + Ok(profile.update_bitmask_enum_fields(definition_file)?) + } else { + Ok(profile) } } -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavEnum { - pub name: String, - pub description: Option, - pub entries: Vec, - /// If contains Some, the string represents the primitive type (size) for bitflags. - /// If no fields use this enum, the bitmask is true, but primitive is None. In this case - /// regular enum is generated as primitive is unknown. - pub primitive: Option, - pub bitmask: bool, - pub deprecated: Option, +struct Parser<'a> { + reader: Reader<&'a [u8]>, + filter: MavXmlFilter, } -impl MavEnum { - /// Returns true when this enum will be emitted as a `bitflags` struct. - fn is_generated_as_bitflags(&self) -> bool { - self.primitive.is_some() - } - - fn combine(&mut self, enm: &Self) { - for enum_entry in &enm.entries { - if self - .entries - .iter() - .any(|elem| elem.name == enum_entry.name && elem.value == enum_entry.value) - { - panic!("Enum entry {} already exists", enum_entry.name) - } - } - self.entries.append(&mut enm.entries.clone()); - self.entries.sort_by_key(|entry| entry.value); +impl<'a> Parser<'a> { + fn from_reader(reader: Reader<&'a [u8]>) -> Self { + let filter = MavXmlFilter::default(); + Parser { reader, filter } } - fn emit_defs(&self) -> Vec { - let mut cnt = 0u64; - self.entries - .iter() - .map(|enum_entry| { - let name = format_ident!("{}", enum_entry.name.clone()); - let value; - - let deprecation = enum_entry.emit_deprecation(); - - let description = if let Some(description) = enum_entry.description.as_ref() { - let description = URL_REGEX.replace_all(description, "<$1>"); - quote!(#[doc = #description]) - } else { - quote!() - }; - - let params_doc = enum_entry.emit_params(); - - if let Some(tmp_value) = enum_entry.value { - cnt = cnt.max(tmp_value); - let tmp = TokenStream::from_str(&tmp_value.to_string()).unwrap(); - value = quote!(#tmp); - } else { - cnt += 1; - value = quote!(#cnt); - } - - if self.is_generated_as_bitflags() { - quote! { - #deprecation - #description - #params_doc - const #name = #value; - } - } else { - quote! { - #deprecation - #description - #params_doc - #name = #value, + fn next_event(&mut self) -> Result, MavXMLParseError> { + loop { + match self.reader.read_event()? { + Event::Eof => return Err(MavXMLParseError::UnexpectedEnd), + ev => { + if self.filter.filter(&ev) { + return Ok(ev); } } - }) - .collect() - } - - #[inline(always)] - fn emit_name(&self) -> TokenStream { - let name = format_ident!("{}", self.name); - quote!(#name) - } - - #[inline(always)] - fn emit_const_default(&self) -> TokenStream { - let default = format_ident!("{}", self.entries[0].name); - quote!(pub const DEFAULT: Self = Self::#default;) + } + } } - #[inline(always)] - fn emit_deprecation(&self) -> TokenStream { - self.deprecated - .as_ref() - .map(|d| d.emit_tokens()) - .unwrap_or_default() + fn parse(&mut self) -> Result { + match self.next_event()? { + Event::Start(bytes_start) + if id_element(bytes_start.name()) == Some(MavXmlElement::Mavlink) => + { + self.parse_mavlink() + } + _ => Err(MavXMLParseError::ExpectedMAVLink), + } } - fn emit_rust(&self) -> TokenStream { - let defs = self.emit_defs(); - let enum_name = self.emit_name(); - let const_default = self.emit_const_default(); - - let deprecated = self.emit_deprecation(); - - let description = if let Some(description) = self.description.as_ref() { - let desc = URL_REGEX.replace_all(description, "<$1>"); - quote!(#[doc = #desc]) - } else { - quote!() - }; - - let mav_bool_impl = if self.name == "MavBool" - && self - .entries - .iter() - .any(|entry| entry.name == "MAV_BOOL_TRUE") - { - if self.is_generated_as_bitflags() { - quote!( - pub fn as_bool(&self) -> bool { - self.contains(Self::MAV_BOOL_TRUE) + fn parse_mavlink(&mut self) -> Result { + let mut profile = MavProfile::default(); + let mut includes = vec![]; + loop { + match self.next_event()? { + Event::Start(bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Include) => { + includes.push(self.parse_string_element(MavXmlElement::Include)?); } - ) - } else { - quote!( - pub fn as_bool(&self) -> bool { - *self == Self::MAV_BOOL_TRUE + Some(MavXmlElement::Version) => { + profile.version = Some(self.parse_int_element(MavXmlElement::Version)?); } - ) - } - } else { - quote!() - }; - - let enum_def; - if let Some(primitive) = self.primitive.clone() { - let primitive = format_ident!("{}", primitive); - enum_def = quote! { - bitflags!{ - #[cfg_attr(feature = "ts-rs", derive(TS))] - #[cfg_attr(feature = "ts-rs", ts(export, type = "number"))] - #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] - #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] - #[derive(Debug, Copy, Clone, PartialEq)] - #deprecated - #description - pub struct #enum_name: #primitive { - #(#defs)* + Some(MavXmlElement::Dialect) => { + profile.dialect = Some(self.parse_int_element(MavXmlElement::Dialect)?); + } + Some(MavXmlElement::Enums) => { + profile.enums = self.parse_enums()?; } + Some(MavXmlElement::Messages) => profile.messages = self.parse_messages()?, + None => {} + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Mavlink) => + { + break; } - }; - } else { - enum_def = quote! { - #[cfg_attr(feature = "ts-rs", derive(TS))] - #[cfg_attr(feature = "ts-rs", ts(export))] - #[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] - #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] - #[cfg_attr(feature = "serde", serde(tag = "type"))] - #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] - #[repr(u32)] - #deprecated - #description - pub enum #enum_name { - #(#defs)* + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Mavlink, + }) } - }; + } } + Ok(MavCompositeProfile { profile, includes }) + } - quote! { - #enum_def - - impl #enum_name { - #const_default - #mav_bool_impl - } + fn parse_string_option_element( + &mut self, + element: MavXmlElement, + ) -> Result, MavXMLParseError> { + let mut string = None; - impl Default for #enum_name { - fn default() -> Self { - Self::DEFAULT + loop { + match self.next_event()? { + Event::Text(bytes_text) => { + let text = String::from_utf8_lossy(&bytes_text).replace('\n', " "); + string = Some( + string + .map(|t| format!("{t}{text}")) + .unwrap_or(text.to_string()), + ); + } + Event::GeneralRef(bytes_ref) => { + let entity = String::from_utf8_lossy(&bytes_ref); + string = Some( + string + .map(|t| format!("{t}&{entity};")) + .unwrap_or(format!("&{entity};")), + ); + } + Event::End(bytes_end) if id_element(bytes_end.name()) == Some(element) => break, + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: element, + }) } } } - } -} -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavEnumEntry { - pub value: Option, - pub name: String, - pub description: Option, - pub params: Option>, - pub deprecated: Option, -} + Ok(string) + } -impl MavEnumEntry { - #[inline(always)] - fn emit_deprecation(&self) -> TokenStream { - self.deprecated - .as_ref() - .map(|d| d.emit_tokens()) - .unwrap_or_default() + fn parse_string_element(&mut self, element: MavXmlElement) -> Result { + self.parse_string_option_element(element)? + .ok_or(MavXMLParseError::ExpectedText) } - #[inline(always)] - fn emit_params(&self) -> TokenStream { - let Some(params) = &self.params else { - return quote!(); - }; - let any_value_range = params.iter().any(|p| { - p.min_value.is_some() - || p.max_value.is_some() - || p.increment.is_some() - || p.enum_used.is_some() - || (p.reserved && p.default.is_some()) - }); - let any_units = params.iter().any(|p| p.units.is_some()); - let lines = params - .iter() - .map(|param| param.emit_doc_row(any_value_range, any_units)); - let mut table_header = "| Parameter | Description |".to_string(); - let mut table_hl = "| --------- | ----------- |".to_string(); - if any_value_range { - table_header += " Values |"; - table_hl += " ------ |"; - } - if any_units { - table_header += " Units |"; - table_hl += " ----- |"; - } - quote! { - #[doc = ""] - #[doc = "# Parameters"] - #[doc = ""] - #[doc = #table_header] - #[doc = #table_hl] - #(#lines)* - } + fn parse_int_element(&mut self, element: MavXmlElement) -> Result { + Ok(self.parse_string_element(element)?.parse()?) } -} -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavParam { - pub index: usize, - pub description: Option, - pub label: Option, - pub units: Option, - pub enum_used: Option, - pub increment: Option, - pub min_value: Option, - pub max_value: Option, - pub reserved: bool, - pub default: Option, -} + fn parse_deprecated( + &mut self, + bytes: &BytesStart<'_>, + kind: MavDeprecationType, + ) -> Result { + let mut deprecation = MavDeprecation { + deprecation_type: kind, + ..Default::default() + }; -fn format_number_range(min: Option, max: Option, inc: Option) -> String { - match (min, max, inc) { - (Some(min), Some(max), Some(inc)) => { - if min + inc == max { - format!("{min}, {max}") - } else if min + 2. * inc == max { - format!("{}, {}, {}", min, min + inc, max) - } else { - format!("{}, {}, .. , {}", min, min + inc, max) + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"since" => { + deprecation.since = String::from_utf8_lossy(&attr.value).to_string(); + } + b"replaced_by" => { + let value = String::from_utf8_lossy(&attr.value); + deprecation.replaced_by = if value.is_empty() { + None + } else { + Some(value.to_string()) + }; + } + _ => (), } } - (Some(min), Some(max), None) => format!("{min} .. {max}"), - (Some(min), None, Some(inc)) => format!("{}, {}, ..", min, min + inc), - (None, Some(max), Some(inc)) => format!(".., {}, {}", max - inc, max), - (Some(min), None, None) => format!("≥ {min}"), - (None, Some(max), None) => format!("≤ {max}"), - (None, None, Some(inc)) => format!("Multiples of {inc}"), - (None, None, None) => String::new(), - } -} -impl MavParam { - fn format_valid_values(&self) -> String { - if let (true, Some(default)) = (self.reserved, self.default) { - format!("Reserved (use {default})") - } else if let Some(enum_used) = &self.enum_used { - format!("[`{enum_used}`]") + let element = if kind == MavDeprecationType::Superseded { + MavXmlElement::Superseded } else { - format_number_range(self.min_value, self.max_value, self.increment) - } - } - - fn emit_doc_row(&self, value_range_col: bool, units_col: bool) -> TokenStream { - let label = if let Some(label) = &self.label { - format!("{} ({})", self.index, label) - } else { - format!("{}", self.index) + MavXmlElement::Deprecated }; - let mut line = format!( - "| {label:10}| {:12}|", - self.description.as_deref().unwrap_or_default() - ); - if value_range_col { - let range = self.format_valid_values(); - line += &format!(" {range} |"); - } - if units_col { - let units = self.units.clone().unwrap_or_default(); - line += &format!(" {units} |"); - } - quote! {#[doc = #line]} - } -} - -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavMessage { - pub id: u32, - pub name: String, - pub description: Option, - pub fields: Vec, - pub deprecated: Option, -} -impl MavMessage { - /// Return Token of "MESSAGE_NAME_DATA - /// for mavlink struct data - fn emit_struct_name(&self) -> TokenStream { - let name = format_ident!("{}", format!("{}_DATA", self.name)); - quote!(#name) - } + deprecation.note = self.parse_string_option_element(element)?; - #[inline(always)] - fn emit_name_types(&self) -> (Vec, usize) { - let mut encoded_payload_len: usize = 0; - let field_toks = self - .fields - .iter() - .map(|field| { - let nametype = field.emit_name_type(); - encoded_payload_len += field.mavtype.len(); - - let description = field.emit_description(); - - // 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 { - if field.enumtype.is_some() { - quote!(#[cfg_attr(feature = "serde", serde(default))]) - } else { - quote!(#[cfg_attr(feature = "serde", serde(default = "crate::utils::RustDefault::rust_default"))]) - } - } else { - quote!() - }; - - let serde_with_attr = if matches!(field.mavtype, MavType::Array(_, _)) { - quote!( - #[cfg_attr(feature = "serde", serde(with = "serde_arrays"))] - #[cfg_attr(feature = "ts-rs", ts(type = "Array"))] - ) - } else if matches!(field.mavtype, MavType::CharArray(_)) { - quote!( - #[cfg_attr(feature = "ts-rs", ts(type = "string"))] - ) - } else { - quote!() - }; - - quote! { - #description - #serde_default - #serde_with_attr - #nametype - } - }) - .collect::>(); - (field_toks, encoded_payload_len) + Ok(deprecation) } - /// Generate description for the given message - #[inline(always)] - fn emit_description(&self) -> TokenStream { - let mut ts = TokenStream::new(); - if let Some(doc) = self.description.as_ref() { - let doc = format!("{doc}{}", if doc.ends_with('.') { "" } else { "." }); - // create hyperlinks - let doc = URL_REGEX.replace_all(&doc, "<$1>"); - ts.extend(quote!(#[doc = #doc])); - // Leave a blank line before the message ID for readability. - ts.extend(quote!(#[doc = ""])); + fn parse_wip(&mut self) -> Result<(), MavXMLParseError> { + match self.next_event()? { + Event::End(bytes_end) if id_element(bytes_end.name()) == Some(MavXmlElement::Wip) => {} + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Wip, + }) + } } - let id = format!("ID: {}", self.id); - ts.extend(quote!(#[doc = #id])); - ts + Ok(()) } - #[inline(always)] - fn emit_serialize_vars(&self) -> TokenStream { - let (base_fields, ext_fields): (Vec<_>, Vec<_>) = - self.fields.iter().partition(|f| !f.is_extension); - let ser_vars = base_fields.iter().map(|f| f.rust_writer()); - let ser_ext_vars = ext_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) { - #(#ser_ext_vars)* - let len = __tmp.len(); - ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) - } else { - __tmp.len() + fn parse_enums(&mut self) -> Result, MavXMLParseError> { + let mut enums = BTreeMap::new(); + loop { + match self.next_event()? { + Event::Start(bytes_start) + if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Enum)) => + { + let mav_enum = self.parse_enum(bytes_start)?; + enums.insert(mav_enum.name.clone(), mav_enum); + } + Event::End(bytes_end) + if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Enums)) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Enums, + }) + } } } + Ok(enums) } - #[inline(always)] - fn emit_deserialize_vars(&self) -> TokenStream { - let deser_vars = self - .fields - .iter() - .map(|f| f.rust_reader()) - .collect::>(); - - if deser_vars.is_empty() { - // struct has no fields - quote! { - Ok(Self::default()) - } - } else { - quote! { - 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); - Bytes::new(&payload_buf) - } else { - // fast zero copy - Bytes::new(__input) - }; - - let mut __struct = Self::default(); - #(#deser_vars)* - Ok(__struct) + fn parse_enum(&mut self, bytes: BytesStart<'_>) -> Result { + let mut mav_enum = MavEnum::default(); + + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => { + mav_enum.name = to_pascal_case(&attr.value); + } + b"bitmask" => mav_enum.bitmask = true, + _ => {} } } - } - #[inline(always)] - 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() + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Entry) => { + mav_enum.entries.push(self.parse_entry(bytes_start)?); + } + Some(MavXmlElement::Description) => { + mav_enum.description = + Some(self.parse_string_element(MavXmlElement::Description)?); + } + Some(MavXmlElement::Deprecated) => { + mav_enum.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); + } + Some(MavXmlElement::Superseded) => { + mav_enum.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); + } + Some(MavXmlElement::Wip) => self.parse_wip()?, + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Enum, + }) + } + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Enum) => + { + break + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Enum, + }) } } } - } - #[inline(always)] - fn emit_deprecation(&self) -> TokenStream { - self.deprecated - .as_ref() - .map(|d| d.emit_tokens()) - .unwrap_or_default() + Ok(mav_enum) } - #[inline(always)] - fn emit_const_default(&self, dialect_has_version: bool) -> TokenStream { - let initializers = self - .fields - .iter() - .map(|field| field.emit_default_initializer(dialect_has_version)); - quote!(pub const DEFAULT: Self = Self { #(#initializers)* };) - } - - fn emit_rust(&self, dialect_has_version: bool) -> 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, payload_encoded_len) = self.emit_name_types(); - assert!( - payload_encoded_len <= 255, - "maximum payload length is 255 bytes" - ); - - let deser_vars = self.emit_deserialize_vars(); - let serialize_vars = self.emit_serialize_vars(); - let const_default = self.emit_const_default(dialect_has_version); - let default_impl = self.emit_default_impl(); - - let deprecation = self.emit_deprecation(); - - let description = self.emit_description(); - - quote! { - #deprecation - #description - #[derive(Debug, Clone, PartialEq)] - #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] - #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] - #[cfg_attr(feature = "ts-rs", derive(TS))] - #[cfg_attr(feature = "ts-rs", ts(export))] - pub struct #msg_name { - #(#name_types)* - } + fn parse_entry(&mut self, bytes: &BytesStart<'_>) -> Result { + let mut entry = MavEnumEntry::default(); - impl #msg_name { - pub const ENCODED_LEN: usize = #payload_encoded_len; - #const_default - - #[cfg(feature = "arbitrary")] - pub fn random(rng: &mut R) -> Self { - use arbitrary::{Unstructured, Arbitrary}; - let mut buf = [0u8; 1024]; - rng.fill_bytes(&mut buf); - let mut unstructured = Unstructured::new(&buf); - Self::arbitrary(&mut unstructured).unwrap_or_default() + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => entry.name = String::from_utf8_lossy(&attr.value).to_string(), + b"value" => { + let value = String::from_utf8_lossy(&attr.value); + let (src, radix) = value + .strip_prefix("0x") + .map(|value| (value, 16)) + .unwrap_or((value.as_ref(), 10)); + entry.value = Some(u64::from_str_radix(src, radix)?); } - } - - #default_impl - - 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 = #payload_encoded_len; - - fn deser(_version: MavlinkVersion, __input: &[u8]) -> Result { - #deser_vars + _ => {} + } + } + + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => { + match id_element(bytes_start.name()) { + Some(MavXmlElement::Description) => { + entry.description = + self.parse_string_option_element(MavXmlElement::Description)?; + } + Some(MavXmlElement::Deprecated) => { + entry.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); + } + Some(MavXmlElement::Superseded) => { + entry.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); + } + Some(MavXmlElement::Param) => { + let param = self.parse_param(bytes_start)?; + let param_index = param.index; + + // Some messages can jump between values, like: 1, 2, 7 + let params = entry.params.get_or_insert(Vec::with_capacity(7)); + while params.len() < param_index { + params.push(MavParam { + index: params.len() + 1, + ..Default::default() + }); + } + params[param_index - 1] = param; + } + Some(MavXmlElement::Wip) => _ = Some(self.parse_wip()?), + Some(element) => { + return Err(MavXMLParseError::UnexpectedElement { element }) + } + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Entry, + }) + } + } } - - fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { - #serialize_vars + Event::End(bytes_end) + if Some(MavXmlElement::Entry) == id_element(bytes_end.name()) => + { + break + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Entry, + }) } } } + Ok(entry) } - /// Ensures that a message does not contain duplicate field names. - /// - /// Duplicate field names would generate invalid Rust structs. - pub(crate) fn validate_unique_fields(&self) -> Result<(), MavXMLParseError> { - let mut seen: HashSet<&str> = HashSet::new(); - for f in &self.fields { - let name: &str = &f.name; - if !seen.insert(name) { - return Err(MavXMLParseError::DuplicateFieldName { - field: name.to_string(), - message: self.name.clone(), - }); - } - } - Ok(()) - } + fn parse_param(&mut self, bytes: &BytesStart<'_>) -> Result { + let mut param = MavParam::default(); - /// Ensure that the fields count is at least one and no more than 64 - pub(crate) fn validate_field_count(&self) -> Result<(), MavXMLParseError> { - if self.fields.is_empty() || self.fields.len() >= 64 { - Err(MavXMLParseError::InvalidFieldCount { - message: self.name.clone(), - }) - } else { - Ok(()) - } - } -} - -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavField { - pub mavtype: MavType, - pub name: String, - pub description: Option, - pub enumtype: Option, - pub display: Option, - pub is_extension: bool, -} - -impl MavField { - /// Emit rust name of a given field - #[inline(always)] - fn emit_name(&self) -> TokenStream { - let name = format_ident!("{}", self.name); - quote!(#name) - } - - /// Emit rust type of the field - #[inline(always)] - fn emit_type(&self) -> TokenStream { - let mavtype; - if matches!(self.mavtype, MavType::Array(_, _)) { - let rt = TokenStream::from_str(&self.mavtype.rust_type()).unwrap(); - mavtype = quote!(#rt); - } else if let Some(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); + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"index" => { + let value = String::from_utf8_lossy(&attr.value).parse()?; + param.index = value; + } + b"label" => { + param.label = std::str::from_utf8(&attr.value).ok().map(str::to_owned); + } + b"increment" => { + param.increment = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"minValue" => { + param.min_value = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"maxValue" => { + param.max_value = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + b"units" => { + param.units = std::str::from_utf8(&attr.value).ok().map(str::to_owned); + } + b"enum" => { + param.enum_used = std::str::from_utf8(&attr.value).ok().map(to_pascal_case); + } + b"reserved" => { + param.reserved = attr.value.as_ref() == b"true"; + } + b"default" => { + param.default = Some(String::from_utf8_lossy(&attr.value).parse()?); + } + _ => (), + } } - mavtype - } - /// Generate description for the given field - #[inline(always)] - fn emit_description(&self) -> TokenStream { - let mut ts = TokenStream::new(); - if let Some(val) = self.description.as_ref() { - let desc = URL_REGEX.replace_all(val, "<$1>"); - ts.extend(quote!(#[doc = #desc])); + if !(1..=7).contains(¶m.index) { + return Err(MavXMLParseError::ParamIndexOutOfRange); } - ts - } - /// Combine rust name and type of a given field - #[inline(always)] - fn emit_name_type(&self) -> TokenStream { - let name = self.emit_name(); - let fieldtype = self.emit_type(); - quote!(pub #name: #fieldtype,) + param.description = self.parse_string_option_element(MavXmlElement::Param)?; + Ok(param) } - /// Emit writer - fn rust_writer(&self) -> TokenStream { - let mut name = "self.".to_string() + &self.name.clone(); - if self.enumtype.is_some() { - // 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() as "; - name += &self.mavtype.rust_type(); - } else { - panic!("Display option not implemented"); - } - } else { - // an enum, have to use "*foo as u8" cast - name += " as "; - name += &self.mavtype.rust_type(); + fn parse_messages(&mut self) -> Result, MavXMLParseError> { + let mut messages = BTreeMap::new(); + loop { + match self.next_event()? { + Event::Start(bytes_start) + if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Message)) => + { + let msg = self.parse_message(bytes_start)?; + messages.insert(msg.name.clone(), msg); } - } - } - 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) -> 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 { - // 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 - let tmp = self.mavtype.rust_reader("e!(let tmp), buf); - let enum_name_ident = format_ident!("{}", enum_name); - quote! { - #tmp - #name = #enum_name_ident::from_bits(tmp as <#enum_name_ident as Flags>::Bits) - .ok_or(::mavlink_core::error::ParserError::InvalidFlag { flag_type: #enum_name, value: tmp as u64 })?; - } - } else { - panic!("Display option not implemented"); + Event::End(bytes_end) + if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Messages)) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Messages, + }) } - } else { - // handle enum by FromPrimitive - let tmp = self.mavtype.rust_reader("e!(let tmp), buf); - let val = format_ident!("from_{}", &self.mavtype.rust_type()); - quote!( - #tmp - #name = FromPrimitive::#val(tmp) - .ok_or(::mavlink_core::error::ParserError::InvalidEnum { enum_type: #enum_name, value: tmp as u64 })?; - ) } - } else { - self.mavtype.rust_reader(&name, buf) } + Ok(messages) } - #[inline(always)] - fn emit_default_initializer(&self, dialect_has_version: bool) -> 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(dialect_has_version); - quote!(#field: #default_value,) - } else if let Some(enumname) = &self.enumtype { - let ty = TokenStream::from_str(enumname).unwrap(); - quote!(#field: #ty::DEFAULT,) - } else { - let default_value = self.mavtype.emit_default_value(dialect_has_version); - quote!(#field: #default_value,) - } - } -} - -#[derive(Debug, PartialEq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub enum MavType { - UInt8MavlinkVersion, - #[default] - UInt8, - UInt16, - UInt32, - UInt64, - Int8, - Int16, - Int32, - Int64, - Char, - Float, - Double, - CharArray(usize), - Array(Box, usize), -} - -impl MavType { - pub(crate) fn parse_type(s: &str) -> Option { - use self::MavType::*; - match s { - "uint8_t_mavlink_version" => Some(UInt8MavlinkVersion), - "uint8_t" => Some(UInt8), - "uint16_t" => Some(UInt16), - "uint32_t" => Some(UInt32), - "uint64_t" => Some(UInt64), - "int8_t" => Some(Int8), - "int16_t" => Some(Int16), - "int32_t" => Some(Int32), - "int64_t" => Some(Int64), - "char" => Some(Char), - "float" => Some(Float), - "Double" => Some(Double), - "double" => Some(Double), - _ if s.starts_with("char[") => { - let start = 4; - let size = s[start + 1..(s.len() - 1)].parse::().ok()?; - Some(CharArray(size)) + fn parse_message(&mut self, bytes: BytesStart<'_>) -> Result { + let mut message = MavMessage::default(); + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => message.name = String::from_utf8_lossy(&attr.value).to_string(), + b"id" => message.id = String::from_utf8_lossy(&attr.value).parse()?, + _ => {} } - _ if s.ends_with(']') => { - 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)) - } - _ => None, } - } - /// Emit reader of a given type - pub fn rust_reader(&self, val: &TokenStream, buf: Ident) -> TokenStream { - use self::MavType::*; - match self { - 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()?;}, - UInt64 => quote! {#val = #buf.get_u64_le()?;}, - UInt8MavlinkVersion => quote! {#val = #buf.get_u8()?;}, - Int8 => quote! {#val = #buf.get_i8()?;}, - Int16 => quote! {#val = #buf.get_i16_le()?;}, - Int32 => quote! {#val = #buf.get_i32_le()?;}, - Int64 => quote! {#val = #buf.get_i64_le()?;}, - Float => quote! {#val = #buf.get_f32_le()?;}, - Double => quote! {#val = #buf.get_f64_le()?;}, - CharArray(size) => { - quote! { - let mut tmp = [0_u8; #size]; - for v in &mut tmp { - *v = #buf.get_u8()?; + let mut extention = false; + + loop { + match self.next_event()? { + ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { + Some(MavXmlElement::Description) => { + message.description = + Some(self.parse_string_element(MavXmlElement::Description)?); } - #val = CharArray::new(tmp); - } - } - Array(t, _) => { - let r = t.rust_reader("e!(let val), buf); - quote! { - for v in &mut #val { - #r - *v = val; + Some(MavXmlElement::Field) => message + .fields + .push(self.parse_field(bytes_start, extention)?), + Some(MavXmlElement::Extensions) => match self.next_event()? { + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Extensions) => + { + extention = true; + } + _ => return Err(MavXMLParseError::NoneSelfClosingExtTag), + }, + Some(MavXmlElement::Deprecated) => { + message.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, + ); } - } - } - } - } - - /// Emit writer of a given type - pub fn rust_writer(&self, val: &TokenStream, buf: Ident) -> TokenStream { - use self::MavType::*; - match self { - UInt8MavlinkVersion => quote! {#buf.put_u8(#val);}, - UInt8 => quote! {#buf.put_u8(#val);}, - 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);}, - Int16 => quote! {#buf.put_i16_le(#val);}, - Int32 => quote! {#buf.put_i32_le(#val);}, - Float => quote! {#buf.put_f32_le(#val);}, - UInt64 => quote! {#buf.put_u64_le(#val);}, - Int64 => quote! {#buf.put_i64_le(#val);}, - Double => quote! {#buf.put_f64_le(#val);}, - CharArray(_) => { - let w = Char.rust_writer("e!(*val), buf); - quote! { - for val in &#val { - #w + Some(MavXmlElement::Wip) => self.parse_wip()?, + Some(MavXmlElement::Superseded) => { + message.deprecated = Some( + self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, + ); } - } - } - Array(t, _size) => { - let w = t.rust_writer("e!(*val), buf); - quote! { - for val in &#val { - #w + Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), + None => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.clone().into_owned(), + parent: MavXmlElement::Message, + }) } + }, + Event::End(bytes_end) + if id_element(bytes_end.name()) == Some(MavXmlElement::Message) => + { + break; + } + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Message, + }) } } } - } - - /// Size of a given Mavtype - fn len(&self) -> usize { - use self::MavType::*; - match self { - UInt8MavlinkVersion | UInt8 | Int8 | Char => 1, - UInt16 | Int16 => 2, - UInt32 | Int32 | Float => 4, - UInt64 | Int64 | Double => 8, - CharArray(size) => *size, - Array(t, size) => t.len() * size, - } - } - pub(crate) fn max_int_value(&self) -> u64 { - match self { - Self::UInt8MavlinkVersion | Self::UInt8 => u8::MAX as u64, - Self::UInt16 => u16::MAX as u64, - Self::UInt32 => u32::MAX as u64, - Self::UInt64 => u64::MAX, - Self::Int8 | Self::Char | Self::CharArray(_) => i8::MAX as u64, - Self::Int16 => i16::MAX as u64, - Self::Int32 => i32::MAX as u64, - Self::Int64 => i64::MAX as u64, - // maximum precisly representable value minus 1 for float types - Self::Float => (1 << f32::MANTISSA_DIGITS) - 1, - Self::Double => (1 << f64::MANTISSA_DIGITS) - 1, - Self::Array(mav_type, _) => mav_type.max_int_value(), - } - } + // Validate there are no duplicate field names + message.validate_unique_fields()?; + // Validate field count must be between 1 and 64 + message.validate_field_count()?; - /// Used for ordering of types - fn order_len(&self) -> usize { - use self::MavType::*; - match self { - UInt8MavlinkVersion | UInt8 | Int8 | Char | CharArray(_) => 1, - UInt16 | Int16 => 2, - UInt32 | Int32 | Float => 4, - UInt64 | Int64 | Double => 8, - Array(t, _) => t.len(), - } - } - - /// Used for crc calculation - pub fn primitive_type(&self) -> String { - use self::MavType::*; - match self { - UInt8MavlinkVersion => "uint8_t".into(), - UInt8 => "uint8_t".into(), - Int8 => "int8_t".into(), - Char => "char".into(), - UInt16 => "uint16_t".into(), - Int16 => "int16_t".into(), - UInt32 => "uint32_t".into(), - Int32 => "int32_t".into(), - Float => "float".into(), - UInt64 => "uint64_t".into(), - Int64 => "int64_t".into(), - Double => "double".into(), - CharArray(_) => "char".into(), - Array(t, _) => t.primitive_type(), - } - } + // Sort fields, first all none extension fields by type, then all extension fields in original order + message + .fields + .sort_by(|a, b| match (a.is_extension, b.is_extension) { + (false, false) => a.mavtype.compare(&b.mavtype), + (a, b) => a.cmp(&b), + }); - /// Return rust equivalent of a given Mavtype - /// Used for generating struct fields. - pub fn rust_type(&self) -> String { - use self::MavType::*; - match self { - UInt8 | UInt8MavlinkVersion => "u8".into(), - Int8 => "i8".into(), - Char => "u8".into(), - UInt16 => "u16".into(), - Int16 => "i16".into(), - UInt32 => "u32".into(), - Int32 => "i32".into(), - Float => "f32".into(), - UInt64 => "u64".into(), - Int64 => "i64".into(), - Double => "f64".into(), - CharArray(size) => format!("CharArray<{size}>"), - Array(t, size) => format!("[{};{}]", t.rust_type(), size), - } + Ok(message) } - pub fn emit_default_value(&self, dialect_has_version: bool) -> TokenStream { - use self::MavType::*; - match self { - UInt8 => quote!(0_u8), - UInt8MavlinkVersion => { - if dialect_has_version { - quote!(MINOR_MAVLINK_VERSION) - } else { - quote!(0_u8) + fn parse_field( + &mut self, + bytes: &BytesStart<'_>, + is_extension: bool, + ) -> Result { + let mut field = MavField { + is_extension, + ..Default::default() + }; + for attr in bytes.attributes().filter_map(|attr| attr.ok()) { + match attr.key.into_inner() { + b"name" => { + let name = String::from_utf8_lossy(&attr.value); + field.name = if name == "type" { + "mavtype".to_string() + } else { + name.to_string() + }; } - } - 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), - CharArray(size) => quote!(CharArray::new([0_u8; #size])), - Array(ty, size) => { - let default_value = ty.emit_default_value(dialect_has_version); - quote!([#default_value; #size]) + b"type" => { + let mav_type = String::from_utf8_lossy(&attr.value); + field.mavtype = + MavType::parse_type(&mav_type).ok_or(MavXMLParseError::InvalidType { + mav_type: mav_type.to_string(), + })?; + } + b"enum" => { + let enum_type = to_pascal_case(&attr.value); + field.enumtype = Some(enum_type); + } + b"display" => { + field.display = Some(String::from_utf8_lossy(&attr.value).to_string()); + } + _ => {} } } - } - /// 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(), - } - } + field.description = self.parse_string_option_element(MavXmlElement::Field)?; - /// Compare two MavTypes - pub fn compare(&self, other: &Self) -> Ordering { - let len = self.order_len(); - (-(len as isize)).cmp(&(-(other.order_len() as isize))) + Ok(field) } } -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[derive(Debug, PartialEq, Eq, Clone, Default, Copy)] -pub enum MavDeprecationType { - #[default] - Deprecated, - Superseded, -} +fn to_pascal_case(text: impl AsRef<[u8]>) -> String { + let input = text.as_ref(); + let mut result = String::with_capacity(input.len()); + let mut capitalize = true; -impl Display for MavDeprecationType { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - match self { - Self::Deprecated => f.write_str("Deprecated"), - Self::Superseded => f.write_str("Superseded"), + for &b in input { + if b == b'_' { + capitalize = true; + continue; } - } -} - -#[derive(Debug, PartialEq, Eq, Clone, Default)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -pub struct MavDeprecation { - // YYYY-MM - pub since: String, - pub replaced_by: Option, - pub deprecation_type: MavDeprecationType, - pub note: Option, -} -impl MavDeprecation { - pub fn emit_tokens(&self) -> TokenStream { - let since = &self.since; - let note = match &self.note { - Some(str) if str.is_empty() || str.ends_with(".") => str.clone(), - Some(str) => format!("{str}."), - None => String::new(), - }; - let replaced_by = match &self.replaced_by { - Some(str) if str.starts_with('`') => format!("See {str}"), - Some(str) => format!("See `{str}`"), - None => String::new(), - }; - let message = format!( - "{note} {replaced_by} ({} since {since})", - self.deprecation_type - ); - quote!(#[deprecated = #message]) - } -} - -/// 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, -) -> Result<(), BindGenError> { - let mut parsed_files: HashSet = HashSet::new(); - let profile = parse_profile(definitions_dir, definition_file, &mut parsed_files)?; - - let dialect_name = util::to_dialect_name(definition_file); - - // rust file - let rust_tokens = profile.emit_rust(&dialect_name); - writeln!(output_rust, "{rust_tokens}").unwrap(); - - Ok(()) -} - -/// CRC operates over names of the message and names of its fields -/// Hence we have to preserve the original uppercase names delimited with an underscore -/// For field names, we replace "type" with "mavtype" to make it rust compatible (this is -/// needed for generating sensible rust code), but for calculating crc function we have to -/// use the original name "type" -pub fn extra_crc(msg: &MavMessage) -> u8 { - // calculate a 8-bit checksum of the key fields of a message, so we - // can detect incompatible XML changes - let mut crc = CRCu16::crc16mcrf4cc(); - - crc.digest(msg.name.as_bytes()); - crc.digest(b" "); - - let mut f = msg.fields.clone(); - // only mavlink 1 fields should be part of the extra_crc - f.retain(|f| !f.is_extension); - f.sort_by(|a, b| a.mavtype.compare(&b.mavtype)); - for field in &f { - crc.digest(field.mavtype.primitive_type().as_bytes()); - crc.digest(b" "); - if field.name == "mavtype" { - crc.digest(b"type"); + if capitalize { + result.push((b as char).to_ascii_uppercase()); + capitalize = false; } else { - crc.digest(field.name.as_bytes()); - } - crc.digest(b" "); - if let MavType::Array(_, size) | MavType::CharArray(size) = field.mavtype { - crc.digest(&[size as u8]); + result.push((b as char).to_ascii_lowercase()); } } - - let crcval = crc.get_crc(); - ((crcval & 0xFF) ^ (crcval >> 8)) as u8 -} - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn emits_target_id_match_arms() { - // Build a minimal profile containing one message with target fields and one without - let mut profile = MavProfile::default(); - - let msg_with_targets = MavMessage { - id: 300, - name: "COMMAND_INT".to_string(), - description: None, - fields: vec![ - MavField { - mavtype: MavType::UInt8, - name: "target_system".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - MavField { - mavtype: MavType::UInt8, - name: "target_component".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - ], - deprecated: None, - }; - - let msg_without_targets = MavMessage { - id: 0, - name: "HEARTBEAT".to_string(), - description: None, - fields: vec![MavField { - mavtype: MavType::UInt32, - name: "custom_mode".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }], - deprecated: None, - }; - - profile.add_message(&msg_with_targets); - profile.add_message(&msg_without_targets); - - let tokens = profile.emit_rust("common"); - let mut code = tokens.to_string(); - code.retain(|c| !c.is_whitespace()); - - // Check the code contains the target_system/component_id functions - assert!(code.contains("fntarget_system_id(&self)->Option")); - assert!(code.contains("fntarget_component_id(&self)->Option")); - - // Check the generated impl contains arms referencing COMMAND_INT(inner).target_system/component - assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_system)")); - assert!(code.contains("Self::COMMAND_INT(inner)=>Some(inner.target_component)")); - - // Ensure a message without target fields returns None - assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_system)")); - assert!(!code.contains("Self::HEARTBEAT(inner)=>Some(inner.target_component)")); - } - - #[test] - fn validate_unique_fields_allows_unique() { - let msg = MavMessage { - id: 1, - name: "FOO".to_string(), - description: None, - fields: vec![ - MavField { - mavtype: MavType::UInt8, - name: "a".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - MavField { - mavtype: MavType::UInt16, - name: "b".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - ], - deprecated: None, - }; - // Should not panic - msg.validate_unique_fields().unwrap(); - } - - #[test] - #[should_panic(expected = "DuplicateFieldName")] - fn validate_unique_fields_panics_on_duplicate() { - let msg = MavMessage { - id: 2, - name: "BAR".to_string(), - description: None, - fields: vec![ - MavField { - mavtype: MavType::UInt8, - name: "target_system".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - MavField { - mavtype: MavType::UInt8, - name: "target_system".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - ], - deprecated: None, - }; - // Should panic due to duplicate field names - msg.validate_unique_fields().unwrap(); - } - - #[test] - fn validate_field_count_ok() { - let msg = MavMessage { - id: 2, - name: "FOO".to_string(), - description: None, - fields: vec![ - MavField { - mavtype: MavType::UInt8, - name: "a".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - MavField { - mavtype: MavType::UInt8, - name: "b".to_string(), - description: None, - enumtype: None, - display: None, - is_extension: false, - }, - ], - deprecated: None, - }; - // Should not panic - msg.validate_field_count().unwrap(); - } - - #[test] - #[should_panic] - fn validate_field_count_too_many() { - let mut fields = vec![]; - for i in 0..65 { - let field = MavField { - mavtype: MavType::UInt8, - name: format!("field_{i}"), - description: None, - enumtype: None, - display: None, - is_extension: false, - }; - fields.push(field); - } - let msg = MavMessage { - id: 2, - name: "BAZ".to_string(), - description: None, - fields, - deprecated: None, - }; - // Should panic due to 65 fields - msg.validate_field_count().unwrap(); - } - - #[test] - #[should_panic] - fn validate_field_count_empty() { - let msg = MavMessage { - id: 2, - name: "BAM".to_string(), - description: None, - fields: vec![], - deprecated: None, - }; - // Should panic due to no fields - msg.validate_field_count().unwrap(); - } - - #[test] - fn test_fmt_mav_param_values() { - let enum_param = MavParam { - enum_used: Some("ENUM_NAME".to_string()), - ..Default::default() - }; - assert_eq!(enum_param.format_valid_values(), "[`ENUM_NAME`]"); - - let reserved_param = MavParam { - reserved: true, - default: Some(f32::NAN), - ..Default::default() - }; - assert_eq!(reserved_param.format_valid_values(), "Reserved (use NaN)"); - - let unrestricted_param = MavParam::default(); - assert_eq!(unrestricted_param.format_valid_values(), ""); - - let int_param = MavParam { - increment: Some(1.0), - ..Default::default() - }; - assert_eq!(int_param.format_valid_values(), "Multiples of 1"); - - let pos_param = MavParam { - min_value: Some(0.0), - ..Default::default() - }; - assert_eq!(pos_param.format_valid_values(), "≥ 0"); - - let max_param = MavParam { - max_value: Some(5.5), - ..Default::default() - }; - assert_eq!(max_param.format_valid_values(), "≤ 5.5"); - - let pos_int_param = MavParam { - min_value: Some(0.0), - increment: Some(1.0), - ..Default::default() - }; - assert_eq!(pos_int_param.format_valid_values(), "0, 1, .."); - - let max_inc_param = MavParam { - increment: Some(1.0), - max_value: Some(360.0), - ..Default::default() - }; - assert_eq!(max_inc_param.format_valid_values(), ".., 359, 360"); - - let range_param = MavParam { - min_value: Some(0.0), - max_value: Some(10.0), - ..Default::default() - }; - assert_eq!(range_param.format_valid_values(), "0 .. 10"); - - let int_range_param = MavParam { - min_value: Some(0.0), - max_value: Some(10.0), - increment: Some(1.0), - ..Default::default() - }; - assert_eq!(int_range_param.format_valid_values(), "0, 1, .. , 10"); - - let close_inc_range_param = MavParam { - min_value: Some(-2.0), - max_value: Some(2.0), - increment: Some(2.0), - ..Default::default() - }; - assert_eq!(close_inc_range_param.format_valid_values(), "-2, 0, 2"); - - let bin_range_param = MavParam { - min_value: Some(0.0), - max_value: Some(1.0), - increment: Some(1.0), - ..Default::default() - }; - assert_eq!(bin_range_param.format_valid_values(), "0, 1"); - } - - #[test] - fn test_emit_doc_row() { - let param = MavParam { - index: 3, - label: Some("test param".to_string()), - min_value: Some(0.0), - units: Some("m/s".to_string()), - ..Default::default() - }; - // test with all variations of columns - assert_eq!( - param.emit_doc_row(false, false).to_string(), - quote! {#[doc = "| 3 (test param)| |"]}.to_string() - ); - assert_eq!( - param.emit_doc_row(false, true).to_string(), - quote! {#[doc = "| 3 (test param)| | m/s |"]}.to_string() - ); - assert_eq!( - param.emit_doc_row(true, false).to_string(), - quote! {#[doc = "| 3 (test param)| | ≥ 0 |"]}.to_string() - ); - assert_eq!( - param.emit_doc_row(true, true).to_string(), - quote! {#[doc = "| 3 (test param)| | ≥ 0 | m/s |"]}.to_string() - ); - - let unlabeled_param = MavParam { - index: 2, - ..Default::default() - }; - assert_eq!( - unlabeled_param.emit_doc_row(false, false).to_string(), - quote! {#[doc = "| 2 | |"]}.to_string() - ); - } + result } diff --git a/mavlink-bindgen/src/parser_new.rs b/mavlink-bindgen/src/parser_new.rs deleted file mode 100644 index bb816105579..00000000000 --- a/mavlink-bindgen/src/parser_new.rs +++ /dev/null @@ -1,835 +0,0 @@ -use std::{ - collections::{BTreeMap, HashSet}, - path::{Path, PathBuf}, -}; - -use quick_xml::{ - events::{BytesStart, Event}, - name::QName, - Reader, -}; - -use crate::{ - error::MavXMLParseError, - parser::{ - MavDeprecation, MavDeprecationType, MavEnum, MavEnumEntry, MavField, MavMessage, MavParam, - MavProfile, MavType, - }, - BindGenError, -}; - -#[derive(Debug, PartialEq, Eq, Clone, Copy)] -#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[cfg_attr(feature = "serde", serde(tag = "type"))] -pub enum MavXmlElement { - Version, - Mavlink, - Dialect, - Include, - Enums, - Enum, - Entry, - Description, - Param, - Messages, - Message, - Field, - Deprecated, - Wip, - Extensions, - Superseded, -} - -const fn id_element(name: QName<'_>) -> Option { - use self::MavXmlElement::*; - match name.into_inner() { - 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), - b"superseded" => Some(Superseded), - _ => None, - } -} - -#[cfg(not(feature = "mav2-message-extensions"))] -struct ExtensionFilter { - pub is_in: bool, -} - -struct MessageFilter { - pub is_in: bool, - pub messages: Vec, -} - -struct MavXmlFilter { - #[cfg(not(feature = "mav2-message-extensions"))] - extension_filter: ExtensionFilter, - message_filter: MessageFilter, -} - -impl Default for MavXmlFilter { - fn default() -> Self { - Self { - #[cfg(not(feature = "mav2-message-extensions"))] - extension_filter: ExtensionFilter { is_in: false }, - message_filter: MessageFilter::new(), - } - } -} - -impl MessageFilter { - fn new() -> Self { - Self { - is_in: false, - messages: vec![ - // device_cap_flags is u32, when enum is u16, which is not handled by the parser yet - "STORM32_GIMBAL_MANAGER_INFORMATION".to_string(), - ], - } - } -} - -impl MavXmlFilter { - fn filter(&mut self, event: &Event) -> bool { - self.filter_extension(event) - && self.filter_messages(event) - && Self::filter_event_type(event) - } - - #[cfg(feature = "mav2-message-extensions")] - fn filter_extension(&mut self, _event: &Event) -> bool { - true - } - - /// Ignore extension fields - #[cfg(not(feature = "mav2-message-extensions"))] - fn filter_extension(&mut self, event: &Event) -> bool { - match event { - Event::Start(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Extensions) => { - self.extension_filter.is_in = true; - } - Event::End(bytes) if id_element(bytes.name()) == Some(MavXmlElement::Message) => { - self.extension_filter.is_in = false; - } - _ => {} - } - !self.extension_filter.is_in - } - - /// Filters messages by their name - fn filter_messages(&mut self, event: &Event) -> bool { - match event { - Event::Start(bytes) if Some(MavXmlElement::Message) == id_element(bytes.name()) => { - let name_attr = bytes - .attributes() - .filter_map(|attr| attr.ok()) - .find(|attr| attr.key.into_inner() == b"name"); - if let Some(name_attr) = name_attr { - let value = String::from_utf8_lossy(&name_attr.value); - if self - .message_filter - .messages - .iter() - .any(|filter| filter.as_str() == value) - { - self.message_filter.is_in = true; - return false; - } - } - } - Event::End(bytes) - if Some(MavXmlElement::Message) == id_element(bytes.name()) - && self.message_filter.is_in => - { - self.message_filter.is_in = false; - return false; - } - _ => {} - } - !self.message_filter.is_in - } - - fn filter_event_type(event: &Event) -> bool { - !matches!( - event, - Event::Comment(_) | Event::Decl(_) | Event::CData(_) | Event::DocType(_) | Event::PI(_) - ) - } -} - -struct MavCompositeProfile { - profile: MavProfile, - includes: Vec, -} - -impl MavCompositeProfile { - fn resolve_includes( - mut self, - definitions_dir: &Path, - parsed_files: &mut HashSet, - ) -> Result { - for include in &self.includes { - let include = PathBuf::from(include.trim()); - 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() { - self.profile.add_message(message); - } - for enm in included_profile.enums.values() { - self.profile.add_enum(enm); - } - if self.profile.version.is_none() { - self.profile.version = included_profile.version; - } - } - } - Ok(self.profile) - } -} - -impl MavProfile { - /// Go over all fields in the messages, and if you encounter an enum, - /// which is a bitmask, set the bitmask size based on field size - fn update_bitmask_enum_fields(mut self, definition_file: &Path) -> Result { - for msg in self.messages.values_mut() { - for field in &mut msg.fields { - if let Some(enum_name) = &field.enumtype { - // find the corresponding enum - if let Some(enm) = self.enums.get_mut(enum_name) { - // Handle legacy definition where bitmask is defined as display="bitmask" - if field.display == Some("bitmask".to_string()) { - enm.bitmask = true; - } - - // it is a bitmask - if enm.bitmask { - enm.primitive = Some(field.mavtype.rust_primitive_type()); - - // check if all enum values can be stored in the fields - for entry in &enm.entries { - if entry.value.unwrap_or_default() > field.mavtype.max_int_value() { - return Err(BindGenError::BitflagEnumTypeToNarrow { - field_name: field.name.clone(), - message_name: msg.name.clone(), - enum_name: enum_name.to_string(), - }); - } - } - - // Fix fields in backwards manner - if field.display.is_none() { - field.display = Some("bitmask".to_string()); - } - } - } else { - return Err(BindGenError::InvalidEnumReference { - enum_name: enum_name.to_string(), - path: definition_file.to_path_buf(), - }); - } - } - } - } - Ok(self) - } -} - -pub fn parse_profile( - definitions_dir: &Path, - definition_file: &Path, - parsed_files: &mut HashSet, -) -> Result { - let is_top_level = parsed_files.is_empty(); - let in_path = Path::new(&definitions_dir).join(definition_file); - // Keep track of which files have been parsed - parsed_files.insert(in_path.clone()); - - let xml = std::fs::read_to_string(&in_path).map_err(|e| { - BindGenError::CouldNotReadDefinitionFile { - source: e, - path: in_path.clone(), - } - })?; - - let mut reader = Reader::from_str(&xml); - - reader.config_mut().trim_text(true); - reader.config_mut().expand_empty_elements = true; - - let mut parser = Parser::from_reader(reader); - - let profile = parser - .parse() - .map_err(|source| BindGenError::InvalidDefinitionFile { - source, - path: definition_file.to_path_buf(), - })?; - - let profile = profile.resolve_includes(definitions_dir, parsed_files)?; - - if is_top_level { - Ok(profile.update_bitmask_enum_fields(definition_file)?) - } else { - Ok(profile) - } -} - -struct Parser<'a> { - reader: Reader<&'a [u8]>, - filter: MavXmlFilter, -} - -impl<'a> Parser<'a> { - fn from_reader(reader: Reader<&'a [u8]>) -> Self { - let filter = MavXmlFilter::default(); - Parser { reader, filter } - } - - fn next_event(&mut self) -> Result, MavXMLParseError> { - loop { - match self.reader.read_event()? { - Event::Eof => return Err(MavXMLParseError::UnexpectedEnd), - ev => { - if self.filter.filter(&ev) { - return Ok(ev); - } - } - } - } - } - - fn parse(&mut self) -> Result { - match self.next_event()? { - Event::Start(bytes_start) - if id_element(bytes_start.name()) == Some(MavXmlElement::Mavlink) => - { - self.parse_mavlink() - } - _ => Err(MavXMLParseError::ExpectedMAVLink), - } - } - - fn parse_mavlink(&mut self) -> Result { - let mut profile = MavProfile::default(); - let mut includes = vec![]; - loop { - match self.next_event()? { - Event::Start(bytes_start) => match id_element(bytes_start.name()) { - Some(MavXmlElement::Include) => { - includes.push(self.parse_string_element(MavXmlElement::Include)?); - } - Some(MavXmlElement::Version) => { - profile.version = Some(self.parse_int_element(MavXmlElement::Version)?); - } - Some(MavXmlElement::Dialect) => { - profile.dialect = Some(self.parse_int_element(MavXmlElement::Dialect)?); - } - Some(MavXmlElement::Enums) => { - profile.enums = self.parse_enums()?; - } - Some(MavXmlElement::Messages) => profile.messages = self.parse_messages()?, - None => {} - Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), - }, - Event::End(bytes_end) - if id_element(bytes_end.name()) == Some(MavXmlElement::Mavlink) => - { - break; - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Mavlink, - }) - } - } - } - Ok(MavCompositeProfile { profile, includes }) - } - - fn parse_string_option_element( - &mut self, - element: MavXmlElement, - ) -> Result, MavXMLParseError> { - let mut string = None; - - loop { - match self.next_event()? { - Event::Text(bytes_text) => { - let text = String::from_utf8_lossy(&bytes_text).replace('\n', " "); - string = Some( - string - .map(|t| format!("{t}{text}")) - .unwrap_or(text.to_string()), - ); - } - Event::GeneralRef(bytes_ref) => { - let entity = String::from_utf8_lossy(&bytes_ref); - string = Some( - string - .map(|t| format!("{t}&{entity};")) - .unwrap_or(format!("&{entity};")), - ); - } - Event::End(bytes_end) if id_element(bytes_end.name()) == Some(element) => break, - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: element, - }) - } - } - } - - Ok(string) - } - - fn parse_string_element(&mut self, element: MavXmlElement) -> Result { - self.parse_string_option_element(element)? - .ok_or(MavXMLParseError::ExpectedText) - } - - fn parse_int_element(&mut self, element: MavXmlElement) -> Result { - Ok(self.parse_string_element(element)?.parse()?) - } - - fn parse_deprecated( - &mut self, - bytes: &BytesStart<'_>, - kind: MavDeprecationType, - ) -> Result { - let mut deprecation = MavDeprecation { - deprecation_type: kind, - ..Default::default() - }; - - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"since" => { - deprecation.since = String::from_utf8_lossy(&attr.value).to_string(); - } - b"replaced_by" => { - let value = String::from_utf8_lossy(&attr.value); - deprecation.replaced_by = if value.is_empty() { - None - } else { - Some(value.to_string()) - }; - } - _ => (), - } - } - - let element = if kind == MavDeprecationType::Superseded { - MavXmlElement::Superseded - } else { - MavXmlElement::Deprecated - }; - - deprecation.note = self.parse_string_option_element(element)?; - - Ok(deprecation) - } - - fn parse_wip(&mut self) -> Result<(), MavXMLParseError> { - match self.next_event()? { - Event::End(bytes_end) if id_element(bytes_end.name()) == Some(MavXmlElement::Wip) => {} - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Wip, - }) - } - } - Ok(()) - } - - fn parse_enums(&mut self) -> Result, MavXMLParseError> { - let mut enums = BTreeMap::new(); - loop { - match self.next_event()? { - Event::Start(bytes_start) - if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Enum)) => - { - let mav_enum = self.parse_enum(bytes_start)?; - enums.insert(mav_enum.name.clone(), mav_enum); - } - Event::End(bytes_end) - if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Enums)) => - { - break; - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Enums, - }) - } - } - } - Ok(enums) - } - - fn parse_enum(&mut self, bytes: BytesStart<'_>) -> Result { - let mut mav_enum = MavEnum::default(); - - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"name" => { - mav_enum.name = to_pascal_case(&attr.value); - } - b"bitmask" => mav_enum.bitmask = true, - _ => {} - } - } - - loop { - match self.next_event()? { - ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { - Some(MavXmlElement::Entry) => { - mav_enum.entries.push(self.parse_entry(bytes_start)?); - } - Some(MavXmlElement::Description) => { - mav_enum.description = - Some(self.parse_string_element(MavXmlElement::Description)?); - } - Some(MavXmlElement::Deprecated) => { - mav_enum.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, - ); - } - Some(MavXmlElement::Superseded) => { - mav_enum.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, - ); - } - Some(MavXmlElement::Wip) => self.parse_wip()?, - Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), - None => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.clone().into_owned(), - parent: MavXmlElement::Enum, - }) - } - }, - Event::End(bytes_end) - if id_element(bytes_end.name()) == Some(MavXmlElement::Enum) => - { - break - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Enum, - }) - } - } - } - - Ok(mav_enum) - } - - fn parse_entry(&mut self, bytes: &BytesStart<'_>) -> Result { - let mut entry = MavEnumEntry::default(); - - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"name" => entry.name = String::from_utf8_lossy(&attr.value).to_string(), - b"value" => { - let value = String::from_utf8_lossy(&attr.value); - let (src, radix) = value - .strip_prefix("0x") - .map(|value| (value, 16)) - .unwrap_or((value.as_ref(), 10)); - entry.value = Some(u64::from_str_radix(src, radix)?); - } - _ => {} - } - } - - loop { - match self.next_event()? { - ref ev @ Event::Start(ref bytes_start) => { - match id_element(bytes_start.name()) { - Some(MavXmlElement::Description) => { - entry.description = - self.parse_string_option_element(MavXmlElement::Description)?; - } - Some(MavXmlElement::Deprecated) => { - entry.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, - ); - } - Some(MavXmlElement::Superseded) => { - entry.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, - ); - } - Some(MavXmlElement::Param) => { - let param = self.parse_param(bytes_start)?; - let param_index = param.index; - - // Some messages can jump between values, like: 1, 2, 7 - let params = entry.params.get_or_insert(Vec::with_capacity(7)); - while params.len() < param_index { - params.push(MavParam { - index: params.len() + 1, - ..Default::default() - }); - } - params[param_index - 1] = param; - } - Some(MavXmlElement::Wip) => _ = Some(self.parse_wip()?), - Some(element) => { - return Err(MavXMLParseError::UnexpectedElement { element }) - } - None => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.clone().into_owned(), - parent: MavXmlElement::Entry, - }) - } - } - } - Event::End(bytes_end) - if Some(MavXmlElement::Entry) == id_element(bytes_end.name()) => - { - break - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Entry, - }) - } - } - } - Ok(entry) - } - - fn parse_param(&mut self, bytes: &BytesStart<'_>) -> Result { - let mut param = MavParam::default(); - - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"index" => { - let value = String::from_utf8_lossy(&attr.value).parse()?; - param.index = value; - } - b"label" => { - param.label = std::str::from_utf8(&attr.value).ok().map(str::to_owned); - } - b"increment" => { - param.increment = Some(String::from_utf8_lossy(&attr.value).parse()?); - } - b"minValue" => { - param.min_value = Some(String::from_utf8_lossy(&attr.value).parse()?); - } - b"maxValue" => { - param.max_value = Some(String::from_utf8_lossy(&attr.value).parse()?); - } - b"units" => { - param.units = std::str::from_utf8(&attr.value).ok().map(str::to_owned); - } - b"enum" => { - param.enum_used = std::str::from_utf8(&attr.value).ok().map(to_pascal_case); - } - b"reserved" => { - param.reserved = attr.value.as_ref() == b"true"; - } - b"default" => { - param.default = Some(String::from_utf8_lossy(&attr.value).parse()?); - } - _ => (), - } - } - - if !(1..=7).contains(¶m.index) { - return Err(MavXMLParseError::ParamIndexOutOfRange); - } - - param.description = self.parse_string_option_element(MavXmlElement::Param)?; - Ok(param) - } - - fn parse_messages(&mut self) -> Result, MavXMLParseError> { - let mut messages = BTreeMap::new(); - loop { - match self.next_event()? { - Event::Start(bytes_start) - if matches!(id_element(bytes_start.name()), Some(MavXmlElement::Message)) => - { - let msg = self.parse_message(bytes_start)?; - messages.insert(msg.name.clone(), msg); - } - Event::End(bytes_end) - if matches!(id_element(bytes_end.name()), Some(MavXmlElement::Messages)) => - { - break; - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Messages, - }) - } - } - } - Ok(messages) - } - - fn parse_message(&mut self, bytes: BytesStart<'_>) -> Result { - let mut message = MavMessage::default(); - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"name" => message.name = String::from_utf8_lossy(&attr.value).to_string(), - b"id" => message.id = String::from_utf8_lossy(&attr.value).parse()?, - _ => {} - } - } - - let mut extention = false; - - loop { - match self.next_event()? { - ref ev @ Event::Start(ref bytes_start) => match id_element(bytes_start.name()) { - Some(MavXmlElement::Description) => { - message.description = - Some(self.parse_string_element(MavXmlElement::Description)?); - } - Some(MavXmlElement::Field) => message - .fields - .push(self.parse_field(bytes_start, extention)?), - Some(MavXmlElement::Extensions) => match self.next_event()? { - Event::End(bytes_end) - if id_element(bytes_end.name()) == Some(MavXmlElement::Extensions) => - { - extention = true; - } - _ => return Err(MavXMLParseError::NoneSelfClosingExtTag), - }, - Some(MavXmlElement::Deprecated) => { - message.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Deprecated)?, - ); - } - Some(MavXmlElement::Wip) => self.parse_wip()?, - Some(MavXmlElement::Superseded) => { - message.deprecated = Some( - self.parse_deprecated(bytes_start, MavDeprecationType::Superseded)?, - ); - } - Some(element) => return Err(MavXMLParseError::UnexpectedElement { element }), - None => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.clone().into_owned(), - parent: MavXmlElement::Message, - }) - } - }, - Event::End(bytes_end) - if id_element(bytes_end.name()) == Some(MavXmlElement::Message) => - { - break; - } - ev => { - return Err(MavXMLParseError::UnexpectedEvent { - event: ev.into_owned(), - parent: MavXmlElement::Message, - }) - } - } - } - - // Validate there are no duplicate field names - message.validate_unique_fields()?; - // Validate field count must be between 1 and 64 - message.validate_field_count()?; - - // Sort fields, first all none extension fields by type, then all extension fields in original order - message - .fields - .sort_by(|a, b| match (a.is_extension, b.is_extension) { - (false, false) => a.mavtype.compare(&b.mavtype), - (a, b) => a.cmp(&b), - }); - - Ok(message) - } - - fn parse_field( - &mut self, - bytes: &BytesStart<'_>, - is_extension: bool, - ) -> Result { - let mut field = MavField { - is_extension, - ..Default::default() - }; - for attr in bytes.attributes().filter_map(|attr| attr.ok()) { - match attr.key.into_inner() { - b"name" => { - let name = String::from_utf8_lossy(&attr.value); - field.name = if name == "type" { - "mavtype".to_string() - } else { - name.to_string() - }; - } - b"type" => { - let mav_type = String::from_utf8_lossy(&attr.value); - field.mavtype = - MavType::parse_type(&mav_type).ok_or(MavXMLParseError::InvalidType { - mav_type: mav_type.to_string(), - })?; - } - b"enum" => { - let enum_type = to_pascal_case(&attr.value); - field.enumtype = Some(enum_type); - } - b"display" => { - field.display = Some(String::from_utf8_lossy(&attr.value).to_string()); - } - _ => {} - } - } - - field.description = self.parse_string_option_element(MavXmlElement::Field)?; - - Ok(field) - } -} - -fn to_pascal_case(text: impl AsRef<[u8]>) -> String { - let input = text.as_ref(); - let mut result = String::with_capacity(input.len()); - let mut capitalize = true; - - for &b in input { - if b == b'_' { - capitalize = true; - continue; - } - - if capitalize { - result.push((b as char).to_ascii_uppercase()); - capitalize = false; - } else { - result.push((b as char).to_ascii_lowercase()); - } - } - result -} From e467cd131fdfa6c77e284798550235efc46693c8 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 13 Mar 2026 15:08:14 +0100 Subject: [PATCH 3/5] add test for parser errors and snap test for including, change parser to be consumed upon parsing, simplify ext tag error --- mavlink-bindgen/src/error.rs | 4 +- mavlink-bindgen/src/parser.rs | 164 +++++++- .../tests/definitions/includee.xml | 18 + .../tests/definitions/includer.xml | 18 + mavlink-bindgen/tests/e2e_snapshots.rs | 5 + ...e_snapshots__includer.xml@includer.rs.snap | 385 ++++++++++++++++++ .../e2e_snapshots__includer.xml@mod.rs.snap | 14 + 7 files changed, 602 insertions(+), 6 deletions(-) create mode 100644 mavlink-bindgen/tests/definitions/includee.xml create mode 100644 mavlink-bindgen/tests/definitions/includer.xml create mode 100644 mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap create mode 100644 mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@mod.rs.snap diff --git a/mavlink-bindgen/src/error.rs b/mavlink-bindgen/src/error.rs index 0cd502d30c8..45e950c1602 100644 --- a/mavlink-bindgen/src/error.rs +++ b/mavlink-bindgen/src/error.rs @@ -31,7 +31,7 @@ pub enum BindGenError { source: std::io::Error, dest_path: std::path::PathBuf, }, - /// Occurs when a MAVLink XML definition that is not valid + /// Occurs when a MAVLink XML definition is not valid #[error("MAVLink definition file {} is invalid: {source}", path.display())] InvalidDefinitionFile { source: MavXMLParseError, @@ -64,8 +64,6 @@ pub enum MavXMLParseError { event: Event<'static>, parent: MavXmlElement, }, - #[error("Extentsion tags may not contain elements")] - NoneSelfClosingExtTag, #[error("Unexpected tag {element:?}")] UnexpectedElement { element: MavXmlElement }, #[error("Expected text")] diff --git a/mavlink-bindgen/src/parser.rs b/mavlink-bindgen/src/parser.rs index 4873ba86cfd..9347447ca58 100644 --- a/mavlink-bindgen/src/parser.rs +++ b/mavlink-bindgen/src/parser.rs @@ -18,6 +18,9 @@ use crate::{ BindGenError, }; +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; + #[derive(Debug, PartialEq, Eq, Clone, Copy)] #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde", serde(tag = "type"))] @@ -269,7 +272,7 @@ pub fn parse_profile( reader.config_mut().trim_text(true); reader.config_mut().expand_empty_elements = true; - let mut parser = Parser::from_reader(reader); + let parser = Parser::from_reader(reader); let profile = parser .parse() @@ -311,7 +314,7 @@ impl<'a> Parser<'a> { } } - fn parse(&mut self) -> Result { + fn parse(mut self) -> Result { match self.next_event()? { Event::Start(bytes_start) if id_element(bytes_start.name()) == Some(MavXmlElement::Mavlink) => @@ -719,7 +722,12 @@ impl<'a> Parser<'a> { { extention = true; } - _ => return Err(MavXMLParseError::NoneSelfClosingExtTag), + ev => { + return Err(MavXMLParseError::UnexpectedEvent { + event: ev.into_owned(), + parent: MavXmlElement::Extensions, + }) + } }, Some(MavXmlElement::Deprecated) => { message.deprecated = Some( @@ -833,3 +841,153 @@ fn to_pascal_case(text: impl AsRef<[u8]>) -> String { } result } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn missing_mavlink_tag() { + let mut reader = Reader::from_str(""); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn invalid_xml() { + let mut reader = Reader::from_str(""); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn parse_int_error() { + let mut reader = Reader::from_str("7.5"); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn parse_float_error() { + let mut reader = Reader::from_str( + r#" + value + "#, + ); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn unexpected_tag() { + let mut reader = + Reader::from_str(r#""#); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn unexpected_xml_event() { + let mut reader = Reader::from_str(r#"text"#); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn expeted_text() { + let mut reader = Reader::from_str( + r#""#, + ); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn unexpected_end() { + let mut reader = Reader::from_str(r#""#); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn param_index_out_of_range() { + let mut reader = Reader::from_str( + r#" + value + "#, + ); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn invalid_type() { + let mut reader = Reader::from_str( + r#" + Hygrometer ID + "#, + ); + reader.config_mut().trim_text(true); + reader.config_mut().expand_empty_elements = true; + let p = Parser::from_reader(reader); + assert!(p.parse().is_err()); + } + + #[test] + fn invalid_enum_reference() { + let mut profile = MavProfile::default(); + profile.add_message(&MavMessage { + id: 69, + name: "MANUAL_CONTROL".to_string(), + fields: vec![MavField { + enumtype: Some("NONE_EXISTING_ENUM".to_string()), + ..Default::default() + }], + ..Default::default() + }); + assert!(profile.update_bitmask_enum_fields(Path::new("")).is_err()); + } + + #[test] + fn bitflag_enum_type_to_narrow() { + let mut profile = MavProfile::default(); + profile.add_message(&MavMessage { + id: 78, + name: "MANUAL_CONTROL".to_string(), + fields: vec![MavField { + enumtype: Some("ENUM_1K".to_string()), + mavtype: MavType::UInt8, + ..Default::default() + }], + ..Default::default() + }); + profile.add_enum(&MavEnum { + name: "ENUM_1K".to_string(), + entries: vec![MavEnumEntry { + value: Some(1024), + ..Default::default() + }], + bitmask: true, + ..Default::default() + }); + assert!(profile.update_bitmask_enum_fields(Path::new("")).is_err()); + } +} diff --git a/mavlink-bindgen/tests/definitions/includee.xml b/mavlink-bindgen/tests/definitions/includee.xml new file mode 100644 index 00000000000..0caa40bdfcc --- /dev/null +++ b/mavlink-bindgen/tests/definitions/includee.xml @@ -0,0 +1,18 @@ + + + + + + + + + + Custom mode + Type + Autopilot + Base mode + System status + Mavlink version + + + \ No newline at end of file diff --git a/mavlink-bindgen/tests/definitions/includer.xml b/mavlink-bindgen/tests/definitions/includer.xml new file mode 100644 index 00000000000..621dce8d69d --- /dev/null +++ b/mavlink-bindgen/tests/definitions/includer.xml @@ -0,0 +1,18 @@ + + + includee.xml + + + + + + + + + Custom 20 field + + + Custom 100 field + + + \ No newline at end of file diff --git a/mavlink-bindgen/tests/e2e_snapshots.rs b/mavlink-bindgen/tests/e2e_snapshots.rs index 986f23cb404..d78bcdefc40 100644 --- a/mavlink-bindgen/tests/e2e_snapshots.rs +++ b/mavlink-bindgen/tests/e2e_snapshots.rs @@ -61,3 +61,8 @@ fn snapshot_mav_bool() { fn snapshot_mav_cmd() { run_snapshot("mav_cmd.xml"); } + +#[test] +fn snapshot_include() { + run_snapshot("includer.xml"); +} diff --git a/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap new file mode 100644 index 00000000000..253c8d0d82c --- /dev/null +++ b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap @@ -0,0 +1,385 @@ +--- +source: mavlink-bindgen/tests/e2e_snapshots.rs +assertion_line: 26 +expression: contents +--- +#![doc = "MAVLink includer dialect."] +#![doc = ""] +#![doc = "This file was automatically generated, do not edit."] +#![allow(deprecated)] +#![allow(clippy::match_single_binding)] +#[cfg(feature = "arbitrary")] +use arbitrary::Arbitrary; +#[allow(unused_imports)] +use bitflags::{bitflags, Flags}; +#[allow(unused_imports)] +use mavlink_core::{ + bytes::Bytes, bytes_mut::BytesMut, types::CharArray, MavlinkVersion, Message, MessageData, +}; +#[allow(unused_imports)] +use num_derive::{FromPrimitive, ToPrimitive}; +#[allow(unused_imports)] +use num_traits::{FromPrimitive, ToPrimitive}; +#[cfg(feature = "serde")] +use serde::{Deserialize, Serialize}; +#[cfg(feature = "ts-rs")] +use ts_rs::TS; +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +#[derive(Debug, Copy, Clone, PartialEq, FromPrimitive, ToPrimitive)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[repr(u32)] +pub enum MavCmd { + MAV_CMD_0 = 0, + MAV_CMD_1 = 1, + MAV_CMD_10 = 10, +} +impl MavCmd { + pub const DEFAULT: Self = Self::MAV_CMD_0; +} +impl Default for MavCmd { + fn default() -> Self { + Self::DEFAULT + } +} +#[doc = "ID: 0"] +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +pub struct HEARTBEAT_DATA { + #[doc = "Custom mode"] + pub custom_mode: u32, + #[doc = "Type"] + pub mavtype: u8, + #[doc = "Autopilot"] + pub autopilot: u8, + #[doc = "Base mode"] + pub base_mode: u8, + #[doc = "System status"] + pub system_status: u8, + #[doc = "Mavlink version"] + pub mavlink_version: u8, +} +impl HEARTBEAT_DATA { + pub const ENCODED_LEN: usize = 9usize; + pub const DEFAULT: Self = Self { + custom_mode: 0_u32, + mavtype: 0_u8, + autopilot: 0_u8, + base_mode: 0_u8, + system_status: 0_u8, + mavlink_version: 0_u8, + }; + #[cfg(feature = "arbitrary")] + pub fn random(rng: &mut R) -> Self { + use arbitrary::{Arbitrary, Unstructured}; + let mut buf = [0u8; 1024]; + rng.fill_bytes(&mut buf); + let mut unstructured = Unstructured::new(&buf); + Self::arbitrary(&mut unstructured).unwrap_or_default() + } +} +impl Default for HEARTBEAT_DATA { + fn default() -> Self { + Self::DEFAULT.clone() + } +} +impl MessageData for HEARTBEAT_DATA { + type Message = MavMessage; + const ID: u32 = 0u32; + const NAME: &'static str = "HEARTBEAT"; + const EXTRA_CRC: u8 = 50u8; + const ENCODED_LEN: usize = 9usize; + fn deser( + _version: MavlinkVersion, + __input: &[u8], + ) -> Result { + let avail_len = __input.len(); + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + Bytes::new(__input) + }; + let mut __struct = Self::default(); + __struct.custom_mode = buf.get_u32_le()?; + __struct.mavtype = buf.get_u8()?; + __struct.autopilot = buf.get_u8()?; + __struct.base_mode = buf.get_u8()?; + __struct.system_status = buf.get_u8()?; + __struct.mavlink_version = buf.get_u8()?; + Ok(__struct) + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + let mut __tmp = BytesMut::new(bytes); + #[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(), + ) + } + __tmp.put_u32_le(self.custom_mode); + __tmp.put_u8(self.mavtype); + __tmp.put_u8(self.autopilot); + __tmp.put_u8(self.base_mode); + __tmp.put_u8(self.system_status); + __tmp.put_u8(self.mavlink_version); + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } +} +#[doc = "ID: 100"] +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +pub struct MSG_100_DATA { + #[doc = "Custom 100 field"] + pub some_field: u32, +} +impl MSG_100_DATA { + pub const ENCODED_LEN: usize = 4usize; + pub const DEFAULT: Self = Self { some_field: 0_u32 }; + #[cfg(feature = "arbitrary")] + pub fn random(rng: &mut R) -> Self { + use arbitrary::{Arbitrary, Unstructured}; + let mut buf = [0u8; 1024]; + rng.fill_bytes(&mut buf); + let mut unstructured = Unstructured::new(&buf); + Self::arbitrary(&mut unstructured).unwrap_or_default() + } +} +impl Default for MSG_100_DATA { + fn default() -> Self { + Self::DEFAULT.clone() + } +} +impl MessageData for MSG_100_DATA { + type Message = MavMessage; + const ID: u32 = 100u32; + const NAME: &'static str = "MSG_100"; + const EXTRA_CRC: u8 = 132u8; + const ENCODED_LEN: usize = 4usize; + fn deser( + _version: MavlinkVersion, + __input: &[u8], + ) -> Result { + let avail_len = __input.len(); + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + Bytes::new(__input) + }; + let mut __struct = Self::default(); + __struct.some_field = buf.get_u32_le()?; + Ok(__struct) + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + let mut __tmp = BytesMut::new(bytes); + #[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(), + ) + } + __tmp.put_u32_le(self.some_field); + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } +} +#[doc = "ID: 20"] +#[derive(Debug, Clone, PartialEq)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +pub struct MSG_20_DATA { + #[doc = "Custom 20 field"] + pub some_field: u32, +} +impl MSG_20_DATA { + pub const ENCODED_LEN: usize = 4usize; + pub const DEFAULT: Self = Self { some_field: 0_u32 }; + #[cfg(feature = "arbitrary")] + pub fn random(rng: &mut R) -> Self { + use arbitrary::{Arbitrary, Unstructured}; + let mut buf = [0u8; 1024]; + rng.fill_bytes(&mut buf); + let mut unstructured = Unstructured::new(&buf); + Self::arbitrary(&mut unstructured).unwrap_or_default() + } +} +impl Default for MSG_20_DATA { + fn default() -> Self { + Self::DEFAULT.clone() + } +} +impl MessageData for MSG_20_DATA { + type Message = MavMessage; + const ID: u32 = 20u32; + const NAME: &'static str = "MSG_20"; + const EXTRA_CRC: u8 = 54u8; + const ENCODED_LEN: usize = 4usize; + fn deser( + _version: MavlinkVersion, + __input: &[u8], + ) -> Result { + let avail_len = __input.len(); + let mut payload_buf = [0; Self::ENCODED_LEN]; + let mut buf = if avail_len < Self::ENCODED_LEN { + payload_buf[0..avail_len].copy_from_slice(__input); + Bytes::new(&payload_buf) + } else { + Bytes::new(__input) + }; + let mut __struct = Self::default(); + __struct.some_field = buf.get_u32_le()?; + Ok(__struct) + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + let mut __tmp = BytesMut::new(bytes); + #[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(), + ) + } + __tmp.put_u32_le(self.some_field); + if matches!(version, MavlinkVersion::V2) { + let len = __tmp.len(); + ::mavlink_core::utils::remove_trailing_zeroes(&bytes[..len]) + } else { + __tmp.len() + } + } +} +#[derive(Clone, PartialEq, Debug)] +#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] +#[cfg_attr(feature = "serde", serde(tag = "type"))] +#[cfg_attr(feature = "arbitrary", derive(Arbitrary))] +#[cfg_attr(feature = "ts-rs", derive(TS))] +#[cfg_attr(feature = "ts-rs", ts(export))] +#[repr(u32)] +pub enum MavMessage { + #[doc = "ID: 0"] + HEARTBEAT(HEARTBEAT_DATA), + #[doc = "ID: 100"] + MSG_100(MSG_100_DATA), + #[doc = "ID: 20"] + MSG_20(MSG_20_DATA), +} +impl MavMessage { + pub const fn all_ids() -> &'static [u32] { + &[0u32, 20u32, 100u32] + } + pub const fn all_messages() -> &'static [(&'static str, u32)] { + &[ + (HEARTBEAT_DATA::NAME, HEARTBEAT_DATA::ID), + (MSG_20_DATA::NAME, MSG_20_DATA::ID), + (MSG_100_DATA::NAME, MSG_100_DATA::ID), + ] + } +} +impl Message for MavMessage { + fn parse( + version: MavlinkVersion, + id: u32, + payload: &[u8], + ) -> Result { + match id { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::deser(version, payload).map(Self::HEARTBEAT), + MSG_100_DATA::ID => MSG_100_DATA::deser(version, payload).map(Self::MSG_100), + MSG_20_DATA::ID => MSG_20_DATA::deser(version, payload).map(Self::MSG_20), + _ => Err(::mavlink_core::error::ParserError::UnknownMessage { id }), + } + } + fn message_name(&self) -> &'static str { + match self { + Self::HEARTBEAT(..) => HEARTBEAT_DATA::NAME, + Self::MSG_100(..) => MSG_100_DATA::NAME, + Self::MSG_20(..) => MSG_20_DATA::NAME, + } + } + fn message_id(&self) -> u32 { + match self { + Self::HEARTBEAT(..) => HEARTBEAT_DATA::ID, + Self::MSG_100(..) => MSG_100_DATA::ID, + Self::MSG_20(..) => MSG_20_DATA::ID, + } + } + fn message_id_from_name(name: &str) -> Option { + match name { + HEARTBEAT_DATA::NAME => Some(HEARTBEAT_DATA::ID), + MSG_100_DATA::NAME => Some(MSG_100_DATA::ID), + MSG_20_DATA::NAME => Some(MSG_20_DATA::ID), + _ => None, + } + } + fn default_message_from_id(id: u32) -> Option { + match id { + HEARTBEAT_DATA::ID => Some(Self::HEARTBEAT(HEARTBEAT_DATA::default())), + MSG_100_DATA::ID => Some(Self::MSG_100(MSG_100_DATA::default())), + MSG_20_DATA::ID => Some(Self::MSG_20(MSG_20_DATA::default())), + _ => None, + } + } + #[cfg(feature = "arbitrary")] + fn random_message_from_id(id: u32, rng: &mut R) -> Option { + match id { + HEARTBEAT_DATA::ID => Some(Self::HEARTBEAT(HEARTBEAT_DATA::random(rng))), + MSG_100_DATA::ID => Some(Self::MSG_100(MSG_100_DATA::random(rng))), + MSG_20_DATA::ID => Some(Self::MSG_20(MSG_20_DATA::random(rng))), + _ => None, + } + } + fn ser(&self, version: MavlinkVersion, bytes: &mut [u8]) -> usize { + match self { + Self::HEARTBEAT(body) => body.ser(version, bytes), + Self::MSG_100(body) => body.ser(version, bytes), + Self::MSG_20(body) => body.ser(version, bytes), + } + } + fn extra_crc(id: u32) -> u8 { + match id { + HEARTBEAT_DATA::ID => HEARTBEAT_DATA::EXTRA_CRC, + MSG_100_DATA::ID => MSG_100_DATA::EXTRA_CRC, + MSG_20_DATA::ID => MSG_20_DATA::EXTRA_CRC, + _ => 0, + } + } + fn target_system_id(&self) -> Option { + match self { + _ => None, + } + } + fn target_component_id(&self) -> Option { + match self { + _ => None, + } + } +} diff --git a/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@mod.rs.snap b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@mod.rs.snap new file mode 100644 index 00000000000..559f4da7176 --- /dev/null +++ b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@mod.rs.snap @@ -0,0 +1,14 @@ +--- +source: mavlink-bindgen/tests/e2e_snapshots.rs +assertion_line: 26 +expression: contents +--- +#[allow(non_camel_case_types)] +#[allow(clippy::derive_partial_eq_without_eq)] +#[allow(clippy::field_reassign_with_default)] +#[allow(non_snake_case)] +#[allow(clippy::unnecessary_cast)] +#[allow(clippy::bad_bit_mask)] +#[allow(clippy::suspicious_else_formatting)] +#[cfg(feature = "dialect-includer")] +pub mod includer; From 99f0c71ccc61aeb54ee7d97329e43b19baba9f55 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 13 Mar 2026 15:09:12 +0100 Subject: [PATCH 4/5] disable superseded test with mav2-message-extensions --- mavlink-bindgen/tests/e2e_snapshots.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/mavlink-bindgen/tests/e2e_snapshots.rs b/mavlink-bindgen/tests/e2e_snapshots.rs index d78bcdefc40..4ed98e84a2c 100644 --- a/mavlink-bindgen/tests/e2e_snapshots.rs +++ b/mavlink-bindgen/tests/e2e_snapshots.rs @@ -42,6 +42,7 @@ fn snapshot_deprecated() { run_snapshot("deprecated.xml"); } +#[cfg(not(feature = "mav2-message-extensions"))] #[test] fn snapshot_superseded() { run_snapshot("superseded.xml"); From 77c555a4d0a778927eba32add463b678b57c9195 Mon Sep 17 00:00:00 2001 From: pv42 Date: Fri, 13 Mar 2026 15:55:07 +0100 Subject: [PATCH 5/5] add enum description inheritance --- mavlink-bindgen/src/codegen.rs | 3 +++ mavlink-bindgen/tests/definitions/includee.xml | 1 + .../snapshots/e2e_snapshots__includer.xml@includer.rs.snap | 1 + 3 files changed, 5 insertions(+) diff --git a/mavlink-bindgen/src/codegen.rs b/mavlink-bindgen/src/codegen.rs index 8de5c3444db..65eec284bf6 100644 --- a/mavlink-bindgen/src/codegen.rs +++ b/mavlink-bindgen/src/codegen.rs @@ -509,6 +509,9 @@ impl MavEnum { panic!("Enum entry {} already exists", enum_entry.name) } } + if enm.description.is_some() && self.description.is_none() { + self.description = enm.description.clone(); + } self.entries.append(&mut enm.entries.clone()); self.entries.sort_by_key(|entry| entry.value); } diff --git a/mavlink-bindgen/tests/definitions/includee.xml b/mavlink-bindgen/tests/definitions/includee.xml index 0caa40bdfcc..54289a68853 100644 --- a/mavlink-bindgen/tests/definitions/includee.xml +++ b/mavlink-bindgen/tests/definitions/includee.xml @@ -2,6 +2,7 @@ + Commands to be executed by the MAV. diff --git a/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap index 253c8d0d82c..b27cd8151c8 100644 --- a/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap +++ b/mavlink-bindgen/tests/snapshots/e2e_snapshots__includer.xml@includer.rs.snap @@ -31,6 +31,7 @@ use ts_rs::TS; #[cfg_attr(feature = "serde", serde(tag = "type"))] #[cfg_attr(feature = "arbitrary", derive(Arbitrary))] #[repr(u32)] +#[doc = "Commands to be executed by the MAV."] pub enum MavCmd { MAV_CMD_0 = 0, MAV_CMD_1 = 1,