mavlink/dialects/
dynamic.rs

1//! Dynamically loaded MAVLink dialects.
2//!
3//! [`DynamicDialect`] loads MAVLink XML definitions at runtime and
4//! implements [`Dialect`] for encoding and decoding [`DynamicMessage`].
5
6use std::{
7    borrow::Cow,
8    collections::{BTreeMap, HashMap, HashSet},
9    error::Error,
10    fmt::{Display, Formatter},
11    path::{Path, PathBuf},
12    sync::Arc,
13};
14
15use mavlink_bindgen::{BindGenError, parser};
16use mavlink_core::error::{EncoderError, ParserError};
17
18use crate::{Dialect, MavlinkVersion};
19
20pub use super::dynamic_parser::DynamicValue;
21use super::dynamic_parser::{write_default, write_value};
22
23/// An error returned while constructing a runtime message.
24#[derive(Debug, Clone, PartialEq, Eq)]
25pub enum DynamicMessageError {
26    /// Dialect has no message with the requested name.
27    UnknownMessage { name: String },
28    /// Message has no field with the requested name.
29    UnknownField { message: String, field: String },
30    /// Field value does not match its dialect definition.
31    InvalidValue {
32        message: String,
33        field: String,
34        reason: String,
35    },
36}
37
38impl Display for DynamicMessageError {
39    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
40        match self {
41            Self::UnknownMessage { name } => write!(f, "unknown runtime dialect message {name}"),
42            Self::UnknownField { message, field } => {
43                write!(f, "runtime dialect message {message} has no field {field}")
44            }
45            Self::InvalidValue {
46                message,
47                field,
48                reason,
49            } => write!(f, "invalid value for {message}.{field}: {reason}"),
50        }
51    }
52}
53
54impl Error for DynamicMessageError {}
55
56/// An error returned while loading a runtime MAVLink dialect.
57#[derive(Debug)]
58pub enum DynamicDialectError {
59    /// The XML dialect file could not be resolved.
60    Io {
61        /// Path supplied by the caller.
62        path: PathBuf,
63        /// Underlying I/O error.
64        source: std::io::Error,
65    },
66    /// The MAVLink XML definition could not be loaded.
67    Xml(BindGenError),
68    /// Two different messages use the same MAVLink message ID.
69    DuplicateMessageId {
70        /// Duplicate ID.
71        id: u32,
72        /// First message name.
73        first: String,
74        /// Conflicting message name.
75        second: String,
76    },
77    /// A message ID cannot be represented in a MAVLink 2 frame.
78    MessageIdOutOfRange {
79        /// Invalid message ID.
80        id: u32,
81        /// Message name.
82        name: String,
83    },
84    /// A message payload does not fit in a MAVLink frame.
85    PayloadTooLong {
86        /// Message name.
87        name: String,
88        /// Calculated encoded payload length.
89        length: usize,
90    },
91}
92
93impl Display for DynamicDialectError {
94    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
95        match self {
96            Self::Io { path, source } => {
97                write!(
98                    f,
99                    "failed to resolve runtime dialect {}: {source}",
100                    path.display()
101                )
102            }
103            Self::Xml(error) => write!(f, "failed to load runtime dialect XML: {error}"),
104            Self::DuplicateMessageId { id, first, second } => write!(
105                f,
106                "runtime dialect defines message ID {id} for both {first} and {second}"
107            ),
108            Self::MessageIdOutOfRange { id, name } => write!(
109                f,
110                "runtime dialect message {name} has ID {id}, which exceeds the MAVLink 2 limit"
111            ),
112            Self::PayloadTooLong { name, length } => write!(
113                f,
114                "runtime dialect message {name} has a {length}-byte payload, exceeding MAVLink's 255-byte limit"
115            ),
116        }
117    }
118}
119
120impl Error for DynamicDialectError {
121    fn source(&self) -> Option<&(dyn Error + 'static)> {
122        match self {
123            Self::Io { source, .. } => Some(source),
124            Self::Xml(error) => Some(error),
125            Self::DuplicateMessageId { .. }
126            | Self::MessageIdOutOfRange { .. }
127            | Self::PayloadTooLong { .. } => None,
128        }
129    }
130}
131
132/// Metadata for a field in a runtime-loaded message definition.
133#[derive(Debug, Clone, PartialEq, Eq)]
134pub struct DynamicField {
135    name: String,
136    primitive_type: String,
137    pub(super) mavtype: parser::MavType,
138    pub(super) enumeration: Option<Arc<DynamicEnumDefinition>>,
139    offset: usize,
140    encoded_size: usize,
141    is_extension: bool,
142}
143
144impl DynamicField {
145    /// XML field name.
146    #[must_use]
147    pub fn name(&self) -> &str {
148        &self.name
149    }
150
151    /// The MAVLink primitive type, such as `uint32_t` or `float`.
152    #[must_use]
153    pub fn primitive_type(&self) -> &str {
154        &self.primitive_type
155    }
156
157    /// Byte offset in the serialized message payload.
158    #[must_use]
159    pub fn offset(&self) -> usize {
160        self.offset
161    }
162
163    /// Number of bytes occupied by this field.
164    #[must_use]
165    pub fn encoded_size(&self) -> usize {
166        self.encoded_size
167    }
168
169    /// Whether this is a MAVLink 2 extension field.
170    #[must_use]
171    pub fn is_extension(&self) -> bool {
172        self.is_extension
173    }
174}
175
176#[derive(Debug, Clone, PartialEq, Eq)]
177pub(super) struct DynamicEnumDefinition {
178    pub(super) name: String,
179    pub(super) primitive: Option<parser::MavType>,
180    pub(super) entries: Vec<(String, u64)>,
181}
182
183/// Metadata for one runtime-loaded MAVLink message.
184#[derive(Debug, Clone, PartialEq, Eq)]
185pub struct DynamicMessageDefinition {
186    id: u32,
187    name: String,
188    extra_crc: u8,
189    base_payload_len: usize,
190    encoded_size: usize,
191    fields: Vec<DynamicField>,
192}
193
194impl DynamicMessageDefinition {
195    /// MAVLink message ID.
196    #[must_use]
197    pub fn id(&self) -> u32 {
198        self.id
199    }
200
201    /// MAVLink message name.
202    #[must_use]
203    pub fn name(&self) -> &str {
204        &self.name
205    }
206
207    /// MAVLink CRC extra byte.
208    #[must_use]
209    pub fn extra_crc(&self) -> u8 {
210        self.extra_crc
211    }
212
213    /// Serialized length before MAVLink 2 extension fields.
214    #[must_use]
215    pub fn base_payload_len(&self) -> usize {
216        self.base_payload_len
217    }
218
219    /// Maximum serialized payload size including extension fields.
220    #[must_use]
221    pub fn encoded_size(&self) -> usize {
222        self.encoded_size
223    }
224
225    /// Fields in MAVLink wire order.
226    #[must_use]
227    pub fn fields(&self) -> &[DynamicField] {
228        &self.fields
229    }
230}
231
232/// A message decoded or constructed through a [`DynamicDialect`].
233#[derive(Debug, Clone, PartialEq, Eq)]
234pub struct DynamicMessage {
235    definition: Arc<DynamicMessageDefinition>,
236    version: MavlinkVersion,
237    payload: Vec<u8>,
238}
239
240impl DynamicMessage {
241    /// Loaded definition used to decode this message.
242    #[must_use]
243    pub fn definition(&self) -> &DynamicMessageDefinition {
244        &self.definition
245    }
246
247    /// MAVLink framing version for this message.
248    #[must_use]
249    pub fn version(&self) -> MavlinkVersion {
250        self.version
251    }
252
253    /// Raw serialized MAVLink payload, without the frame header or checksum.
254    ///
255    /// For MAVLink 2 this can include extension bytes not yet described by
256    /// the loaded dialect.
257    #[must_use]
258    pub fn payload(&self) -> &[u8] {
259        &self.payload
260    }
261
262    /// Serialized bytes for the named field, or `None` if the field is unknown.
263    ///
264    /// Missing payload bytes are zero-filled.
265    #[must_use]
266    pub fn field_bytes(&self, name: &str) -> Option<Cow<'_, [u8]>> {
267        let field = self
268            .definition
269            .fields
270            .iter()
271            .find(|field| field.name == name)?;
272        let end = field.offset.checked_add(field.encoded_size)?;
273        if let Some(bytes) = self.payload.get(field.offset..end) {
274            return Some(Cow::Borrowed(bytes));
275        }
276
277        let mut bytes = vec![0; field.encoded_size];
278        if let Some(available) = self.payload.get(field.offset..) {
279            bytes[..available.len()].copy_from_slice(available);
280        }
281        Some(Cow::Owned(bytes))
282    }
283}
284
285/// An immutable MAVLink dialect loaded from XML at runtime.
286#[derive(Debug, Clone)]
287pub struct DynamicDialect {
288    messages: Arc<BTreeMap<u32, Arc<DynamicMessageDefinition>>>,
289    version: Option<u8>,
290}
291
292impl DynamicDialect {
293    /// Load a dialect XML file and all of its relative `<include>` definitions.
294    ///
295    /// The XML is parsed once. The resulting dialect is immutable, cheap to
296    /// clone, and can be shared safely between readers.
297    pub fn from_xml_file(path: impl AsRef<Path>) -> Result<Self, DynamicDialectError> {
298        let requested_path = path.as_ref();
299        let source_file =
300            requested_path
301                .canonicalize()
302                .map_err(|source| DynamicDialectError::Io {
303                    path: requested_path.to_owned(),
304                    source,
305                })?;
306        let definitions_dir = source_file.parent().unwrap_or_else(|| Path::new(""));
307        let mut parsed_files = HashSet::new();
308        let profile = parser::parse_profile(definitions_dir, &source_file, &mut parsed_files)
309            .map_err(DynamicDialectError::Xml)?;
310
311        Self::from_profile(profile)
312    }
313
314    /// Return the loaded definition for `message_id`.
315    #[must_use]
316    pub fn message_definition(&self, message_id: u32) -> Option<&DynamicMessageDefinition> {
317        self.messages.get(&message_id).map(Arc::as_ref)
318    }
319
320    /// Return all loaded message definitions, ordered by message ID.
321    pub fn message_definitions(&self) -> impl Iterator<Item = &DynamicMessageDefinition> {
322        self.messages.values().map(Arc::as_ref)
323    }
324
325    /// Construct an encodable runtime message from a known definition and its
326    /// wire payload. This is the dynamic counterpart to constructing a
327    /// generated `MavMessage` value before calling `send`.
328    pub fn message_from_payload(
329        &self,
330        version: MavlinkVersion,
331        message_id: u32,
332        payload: impl Into<Vec<u8>>,
333    ) -> Result<DynamicMessage, ParserError> {
334        self.decode(version, message_id, &payload.into())
335    }
336
337    /// Construct a runtime message from its name and named field values.
338    ///
339    /// Numeric values must match the field's primitive type.
340    pub fn message_from_fields<K>(
341        &self,
342        version: MavlinkVersion,
343        message_name: &str,
344        fields: impl IntoIterator<Item = (K, DynamicValue)>,
345    ) -> Result<DynamicMessage, DynamicMessageError>
346    where
347        K: AsRef<str>,
348    {
349        let definition = self
350            .messages
351            .values()
352            .find(|definition| definition.name == message_name)
353            .ok_or_else(|| DynamicMessageError::UnknownMessage {
354                name: message_name.to_owned(),
355            })?;
356        let mut payload = vec![0; definition.encoded_size];
357        // MAVLink messages have at most 64 fields.
358        let mut supplied_fields = [false; 64];
359
360        for (field_name, value) in fields {
361            let field_name = field_name.as_ref();
362            let (index, field) = definition
363                .fields
364                .iter()
365                .enumerate()
366                .find(|(_, field)| field.name == field_name)
367                .ok_or_else(|| DynamicMessageError::UnknownField {
368                    message: message_name.to_owned(),
369                    field: field_name.to_owned(),
370                })?;
371            write_value(
372                &field.mavtype,
373                field.enumeration.as_deref(),
374                &value,
375                &mut payload[field.offset..field.offset + field.encoded_size],
376            )
377            .map_err(|reason| DynamicMessageError::InvalidValue {
378                message: message_name.to_owned(),
379                field: field_name.to_owned(),
380                reason,
381            })?;
382            supplied_fields[index] = true;
383        }
384
385        for (field, supplied) in definition.fields.iter().zip(supplied_fields) {
386            if !supplied {
387                write_default(
388                    &field.mavtype,
389                    field.enumeration.as_deref(),
390                    self.version,
391                    &mut payload[field.offset..field.offset + field.encoded_size],
392                );
393            }
394        }
395
396        payload.truncate(match version {
397            MavlinkVersion::V1 => definition.base_payload_len,
398            MavlinkVersion::V2 => mavlink_core::utils::remove_trailing_zeroes(&payload),
399        });
400
401        Ok(DynamicMessage {
402            definition: Arc::clone(definition),
403            version,
404            payload,
405        })
406    }
407
408    fn from_profile(profile: parser::MavProfile) -> Result<Self, DynamicDialectError> {
409        let version = profile.version;
410        let mut enums = HashMap::new();
411        for (name, enumeration) in profile.enums {
412            for restriction in
413                std::iter::once(None).chain(enumeration.additional_primitives.iter().map(Some))
414            {
415                let entries = enumeration
416                    .entries_with_values(restriction)
417                    .map(|(entry, value)| (entry.name.clone(), value))
418                    .collect();
419                let mut primitive = restriction.or(enumeration.primitive.as_ref());
420                while let Some(parser::MavType::Array(element, _)) = primitive {
421                    primitive = Some(element);
422                }
423                enums.insert(
424                    (name.clone(), restriction.cloned()),
425                    Arc::new(DynamicEnumDefinition {
426                        name: enumeration.name.clone(),
427                        primitive: primitive.cloned(),
428                        entries,
429                    }),
430                );
431            }
432        }
433
434        let mut messages = BTreeMap::new();
435        for message in profile.messages.into_values() {
436            if message.id > 0x00ff_ffff {
437                return Err(DynamicDialectError::MessageIdOutOfRange {
438                    id: message.id,
439                    name: message.name,
440                });
441            }
442
443            let definition = Arc::new(runtime_message_definition(&message, &enums)?);
444            if let Some(existing) = messages.insert(definition.id, Arc::clone(&definition)) {
445                return Err(DynamicDialectError::DuplicateMessageId {
446                    id: definition.id,
447                    first: existing.name.clone(),
448                    second: definition.name.clone(),
449                });
450            }
451        }
452
453        Ok(Self {
454            messages: Arc::new(messages),
455            version,
456        })
457    }
458}
459
460impl Dialect for DynamicDialect {
461    type Message = DynamicMessage;
462    type EncodeError = EncoderError;
463
464    fn message_id(&self, message: &Self::Message) -> u32 {
465        message.definition.id
466    }
467
468    fn extra_crc(&self, message_id: u32) -> Option<u8> {
469        self.messages
470            .get(&message_id)
471            .map(|definition| definition.extra_crc)
472    }
473
474    fn decode(
475        &self,
476        version: MavlinkVersion,
477        message_id: u32,
478        payload: &[u8],
479    ) -> Result<Self::Message, ParserError> {
480        let definition = self
481            .messages
482            .get(&message_id)
483            .ok_or(ParserError::UnknownMessage { id: message_id })?;
484        // MAVLink 2 extension fields may have been added by a newer version
485        // of this dialect.
486        if version == MavlinkVersion::V1 && payload.len() > definition.encoded_size {
487            return Err(ParserError::InvalidPayloadSize {
488                id: message_id,
489                maximum: definition.encoded_size,
490                actual: payload.len(),
491            });
492        }
493
494        Ok(DynamicMessage {
495            definition: Arc::clone(definition),
496            version,
497            payload: payload.to_vec(),
498        })
499    }
500
501    fn encode(
502        &self,
503        version: MavlinkVersion,
504        message: &Self::Message,
505        payload: &mut [u8],
506    ) -> Result<usize, Self::EncodeError> {
507        let definition =
508            self.messages
509                .get(&message.definition.id)
510                .ok_or(EncoderError::UnknownMessage {
511                    id: message.definition.id,
512                })?;
513        if !Arc::ptr_eq(definition, &message.definition) {
514            return Err(EncoderError::DialectMismatch {
515                id: message.definition.id,
516            });
517        }
518        let encoded_size = match version {
519            MavlinkVersion::V1 => definition.base_payload_len,
520            MavlinkVersion::V2 => message.payload.len(),
521        };
522        if encoded_size > payload.len() {
523            return Err(EncoderError::InvalidPayloadSize {
524                id: message.definition.id,
525                maximum: payload.len(),
526                actual: encoded_size,
527            });
528        }
529        if version == MavlinkVersion::V1 {
530            payload[..encoded_size].fill(0);
531        }
532        payload[..message.payload.len().min(encoded_size)]
533            .copy_from_slice(&message.payload[..message.payload.len().min(encoded_size)]);
534        Ok(encoded_size)
535    }
536}
537
538fn runtime_message_definition(
539    message: &parser::MavMessage,
540    enums: &HashMap<(String, Option<parser::MavType>), Arc<DynamicEnumDefinition>>,
541) -> Result<DynamicMessageDefinition, DynamicDialectError> {
542    let mut fields = Vec::with_capacity(message.fields.len());
543    let mut offset = 0;
544    let mut base_payload_len = 0;
545
546    for field in &message.fields {
547        let encoded_size = field.mavtype.size();
548        fields.push(DynamicField {
549            name: field.name.clone(),
550            primitive_type: field.mavtype.primitive_type(),
551            mavtype: field.mavtype.clone(),
552            enumeration: field
553                .enumtype
554                .as_ref()
555                .and_then(|name| {
556                    enums.get(&(
557                        name.clone(),
558                        field.is_undersized.then(|| field.mavtype.clone()),
559                    ))
560                })
561                .cloned(),
562            offset,
563            encoded_size,
564            is_extension: field.is_extension,
565        });
566        offset += encoded_size;
567        if !field.is_extension {
568            base_payload_len = offset;
569        }
570    }
571
572    if offset > u8::MAX.into() {
573        return Err(DynamicDialectError::PayloadTooLong {
574            name: message.name.clone(),
575            length: offset,
576        });
577    }
578
579    Ok(DynamicMessageDefinition {
580        id: message.id,
581        name: message.name.clone(),
582        extra_crc: parser::extra_crc(message),
583        base_payload_len,
584        encoded_size: offset,
585        fields,
586    })
587}
588
589#[cfg(all(test, feature = "dialect-common"))]
590mod tests {
591    use std::io::Cursor;
592
593    use crate::{
594        MAVLinkV2MessageRaw, MavHeader, MavlinkReader, MavlinkVersion, Message,
595        dialects::common::{
596            COMMAND_LONG_DATA, HEARTBEAT_DATA, MavAutopilot, MavCmd, MavMessage, MavModeFlag,
597            MavState, MavType,
598        },
599    };
600
601    use super::DynamicDialect;
602
603    fn common_dialect() -> DynamicDialect {
604        DynamicDialect::from_xml_file(concat!(
605            env!("CARGO_MANIFEST_DIR"),
606            "/mavlink/message_definitions/v1.0/common.xml"
607        ))
608        .unwrap()
609    }
610
611    #[test]
612    fn runtime_loaded_dialect_byte_rep_check() {
613        let dialect = common_dialect();
614        let header = MavHeader {
615            sequence: 34,
616            system_id: 1,
617            component_id: 2,
618        };
619        let messages = [
620            MavMessage::HEARTBEAT(HEARTBEAT_DATA {
621                custom_mode: 42,
622                mavtype: MavType::MAV_TYPE_QUADROTOR,
623                autopilot: MavAutopilot::MAV_AUTOPILOT_PX4,
624                base_mode: MavModeFlag::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
625                system_status: MavState::MAV_STATE_ACTIVE,
626                mavlink_version: 3,
627            }),
628            MavMessage::COMMAND_LONG(COMMAND_LONG_DATA {
629                param1: 1.0,
630                param2: 2.0,
631                param3: 3.0,
632                param4: 4.0,
633                param5: 5.0,
634                param6: 6.0,
635                param7: 7.0,
636                command: MavCmd::MAV_CMD_NAV_WAYPOINT,
637                target_system: 0,
638                target_component: 0,
639                confirmation: 0,
640            }),
641        ];
642
643        for message in messages {
644            let mut common_frame = MAVLinkV2MessageRaw::new();
645            common_frame.serialize_message(header, &message);
646
647            let mut reader = MavlinkReader::new(Cursor::new(common_frame.raw_bytes().to_vec()));
648            let (decoded_header, dynamic_message) = reader
649                .read_message_with_dialect(&dialect, MavlinkVersion::V2)
650                .unwrap();
651
652            if let MavMessage::COMMAND_LONG(expected) = &message {
653                assert_eq!(
654                    dynamic_message.field_bytes("command").unwrap().as_ref(),
655                    (expected.command as u16).to_le_bytes()
656                );
657                assert_eq!(
658                    dynamic_message.field_bytes("confirmation").unwrap(),
659                    &[0][..]
660                );
661            }
662
663            let mut dynamic_frame = MAVLinkV2MessageRaw::new();
664            dynamic_frame
665                .serialize_message_with_dialect(decoded_header, &dialect, &dynamic_message)
666                .unwrap();
667
668            assert_eq!(dynamic_frame.raw_bytes(), common_frame.raw_bytes());
669        }
670    }
671
672    #[test]
673    fn construct_message() {
674        let dialect = common_dialect();
675        let expected = MavMessage::HEARTBEAT(HEARTBEAT_DATA {
676            custom_mode: 42,
677            mavtype: MavType::MAV_TYPE_QUADROTOR,
678            ..Default::default()
679        });
680
681        for version in [MavlinkVersion::V1, MavlinkVersion::V2] {
682            let message = dialect
683                .message_from_fields(
684                    version,
685                    "HEARTBEAT",
686                    [
687                        ("custom_mode", 42u32.into()),
688                        ("mavtype", "MAV_TYPE_QUADROTOR".into()),
689                    ],
690                )
691                .unwrap();
692            let mut payload = [0; 255];
693            let length = expected.ser(version, &mut payload);
694            assert_eq!(message.payload(), &payload[..length]);
695        }
696
697        for (field, value) in [
698            ("custom_mode", 42u8.into()),
699            ("mavtype", 2u32.into()),
700            ("base_mode", 1i8.into()),
701        ] {
702            assert!(
703                dialect
704                    .message_from_fields(MavlinkVersion::V2, "HEARTBEAT", [(field, value)])
705                    .is_err()
706            );
707        }
708    }
709
710    #[test]
711    #[cfg(feature = "serde")]
712    fn serialize_message_with_unknown_bits() {
713        use crate::dialects::common::{CAMERA_IMAGE_CAPTURED_DATA, MavBool};
714
715        let dialect = common_dialect();
716        let expected = MavMessage::CAMERA_IMAGE_CAPTURED(CAMERA_IMAGE_CAPTURED_DATA {
717            capture_result: MavBool::from_bits_retain(u8::MAX as _),
718            file_url: "image.jpg".into(),
719            q: [1.0, 0.0, 0.0, 0.0],
720            ..Default::default()
721        });
722        let expected = serde_json::to_value(expected).unwrap();
723
724        for flags in [
725            (-1i8).into(),
726            expected["capture_result"].as_str().unwrap().into(),
727        ] {
728            let message = dialect
729                .message_from_fields(
730                    MavlinkVersion::V2,
731                    "CAMERA_IMAGE_CAPTURED",
732                    [
733                        ("capture_result", flags),
734                        ("file_url", "image.jpg".into()),
735                        ("q", [1.0f32, 0.0, 0.0, 0.0].into()),
736                    ],
737                )
738                .unwrap();
739            assert_eq!(serde_json::to_value(message).unwrap(), expected);
740        }
741    }
742}
mavlink/dialects/dynamic.rs

mavlink/dialects/
dynamic.rs
