mavlink/dialects/
dynamic_parser.rs

1//! Field value encoding for runtime loaded dialects.
2
3use mavlink_bindgen::parser::MavType;
4use mavlink_core::bytes_mut::BytesMut;
5
6use super::dynamic::DynamicEnumDefinition;
7
8/// A field value used to construct a runtime loaded MAVLink message.
9///
10/// Numeric values must match the field's primitive type, including array elements.
11#[derive(Debug, Clone, PartialEq)]
12pub enum DynamicValue {
13    U8(u8),
14    U16(u16),
15    U32(u32),
16    U64(u64),
17    I8(i8),
18    I16(i16),
19    I32(i32),
20    I64(i64),
21    F32(f32),
22    F64(f64),
23    /// Character array, enum name or `|` separated bitmask names.
24    String(String),
25    /// Array with the field's declared length.
26    Array(Vec<Self>),
27}
28
29impl From<u8> for DynamicValue {
30    fn from(value: u8) -> Self {
31        Self::U8(value)
32    }
33}
34
35impl From<u16> for DynamicValue {
36    fn from(value: u16) -> Self {
37        Self::U16(value)
38    }
39}
40
41impl From<u32> for DynamicValue {
42    fn from(value: u32) -> Self {
43        Self::U32(value)
44    }
45}
46
47impl From<u64> for DynamicValue {
48    fn from(value: u64) -> Self {
49        Self::U64(value)
50    }
51}
52
53impl From<i8> for DynamicValue {
54    fn from(value: i8) -> Self {
55        Self::I8(value)
56    }
57}
58
59impl From<i16> for DynamicValue {
60    fn from(value: i16) -> Self {
61        Self::I16(value)
62    }
63}
64
65impl From<i32> for DynamicValue {
66    fn from(value: i32) -> Self {
67        Self::I32(value)
68    }
69}
70
71impl From<i64> for DynamicValue {
72    fn from(value: i64) -> Self {
73        Self::I64(value)
74    }
75}
76
77impl From<f32> for DynamicValue {
78    fn from(value: f32) -> Self {
79        Self::F32(value)
80    }
81}
82
83impl From<f64> for DynamicValue {
84    fn from(value: f64) -> Self {
85        Self::F64(value)
86    }
87}
88
89impl From<String> for DynamicValue {
90    fn from(value: String) -> Self {
91        Self::String(value)
92    }
93}
94
95impl From<&str> for DynamicValue {
96    fn from(value: &str) -> Self {
97        Self::String(value.into())
98    }
99}
100
101impl<T: Into<Self>> From<Vec<T>> for DynamicValue {
102    fn from(value: Vec<T>) -> Self {
103        Self::Array(value.into_iter().map(Into::into).collect())
104    }
105}
106
107impl<T: Into<Self>, const N: usize> From<[T; N]> for DynamicValue {
108    fn from(value: [T; N]) -> Self {
109        Self::Array(value.into_iter().map(Into::into).collect())
110    }
111}
112
113pub(super) fn write_value(
114    mavtype: &MavType,
115    enumeration: Option<&DynamicEnumDefinition>,
116    value: &DynamicValue,
117    output: &mut [u8],
118) -> Result<(), String> {
119    if let MavType::Array(element_type, length) = mavtype {
120        let DynamicValue::Array(values) = value else {
121            return Err(format!("expected an array of {length} values"));
122        };
123        if values.len() != *length {
124            return Err(format!(
125                "expected an array of {length} values, got {}",
126                values.len()
127            ));
128        }
129
130        let element_size = element_type.size();
131        for (element, output) in values.iter().zip(output.chunks_exact_mut(element_size)) {
132            // Array elements stay primitive, as in generated dialects.
133            write_value(element_type, None, element, output)?;
134        }
135        return Ok(());
136    }
137
138    if let MavType::CharArray(length) = mavtype {
139        return write_char_array(*length, value, output);
140    }
141
142    if let DynamicValue::String(name) = value {
143        let enumeration = enumeration.ok_or_else(|| "field has no associated enum".to_owned())?;
144        let value = parse_enum_value(name, enumeration)?;
145        let primitive = enumeration.primitive.as_ref().unwrap_or(mavtype);
146        let fits = if enumeration.primitive.is_some() {
147            value <= u64::MAX >> (64 - primitive.size() * 8)
148        } else {
149            enum_value_fits(mavtype, value)
150        };
151        if !fits {
152            return Err(format!(
153                "enum value does not fit in {}",
154                primitive.rust_type()
155            ));
156        }
157        let value = cast_integer(primitive, value)
158            .ok_or_else(|| "enums require an integer type".to_owned())?;
159        output.copy_from_slice(&value.to_le_bytes()[..output.len()]);
160        return Ok(());
161    }
162
163    if let Some(enumeration) = enumeration {
164        let value = integer_value(value)?;
165        if let Some(primitive) = &enumeration.primitive {
166            let encoded =
167                cast_integer(primitive, value).and_then(|bits| cast_integer(mavtype, bits));
168            if encoded != Some(value) {
169                return Err(format!(
170                    "bitmask value cannot be represented by {}",
171                    primitive.rust_type()
172                ));
173            }
174        } else if !enum_value_fits(mavtype, value)
175            || !enumeration.entries.iter().any(|(_, entry)| *entry == value)
176        {
177            return Err(format!("{value} is not a valid enum value"));
178        }
179    }
180
181    let mut output = BytesMut::new(output);
182    match (mavtype, value) {
183        (
184            MavType::UInt8 | MavType::UInt8MavlinkVersion | MavType::Char,
185            DynamicValue::U8(value),
186        ) => {
187            output.put_u8(*value);
188        }
189        (MavType::UInt16, DynamicValue::U16(value)) => output.put_u16_le(*value),
190        (MavType::UInt32, DynamicValue::U32(value)) => output.put_u32_le(*value),
191        (MavType::UInt64, DynamicValue::U64(value)) => output.put_u64_le(*value),
192        (MavType::Int8, DynamicValue::I8(value)) => output.put_i8(*value),
193        (MavType::Int16, DynamicValue::I16(value)) => output.put_i16_le(*value),
194        (MavType::Int32, DynamicValue::I32(value)) => output.put_i32_le(*value),
195        (MavType::Int64, DynamicValue::I64(value)) => output.put_i64_le(*value),
196        (MavType::Float, DynamicValue::F32(value)) => output.put_f32_le(*value),
197        (MavType::Double, DynamicValue::F64(value)) => output.put_f64_le(*value),
198        _ => return Err(format!("expected {}", mavtype.rust_type())),
199    }
200    Ok(())
201}
202
203pub(super) fn write_default(
204    mavtype: &MavType,
205    enumeration: Option<&DynamicEnumDefinition>,
206    dialect_version: Option<u8>,
207    output: &mut [u8],
208) {
209    if let MavType::Array(element_type, _) = mavtype {
210        let element_size = element_type.size();
211        for output in output.chunks_exact_mut(element_size) {
212            write_default(element_type, None, dialect_version, output);
213        }
214    } else if let Some((_, value)) = enumeration.and_then(|enumeration| enumeration.entries.first())
215    {
216        for (output, value) in output.iter_mut().zip(value.to_le_bytes()) {
217            *output = value;
218        }
219    } else if matches!(mavtype, MavType::UInt8MavlinkVersion) {
220        output[0] = dialect_version.unwrap_or(0);
221    }
222}
223
224pub(super) fn cast_integer(mavtype: &MavType, value: u64) -> Option<u64> {
225    Some(match mavtype {
226        MavType::UInt8 | MavType::UInt8MavlinkVersion | MavType::Char => u64::from(value as u8),
227        MavType::UInt16 => u64::from(value as u16),
228        MavType::UInt32 => u64::from(value as u32),
229        MavType::UInt64 | MavType::Int64 => value,
230        MavType::Int8 => value as i8 as u64,
231        MavType::Int16 => value as i16 as u64,
232        MavType::Int32 => value as i32 as u64,
233        _ => return None,
234    })
235}
236
237pub(super) fn enum_value_fits(mavtype: &MavType, value: u64) -> bool {
238    match mavtype {
239        MavType::Int8 => i8::try_from(value).is_ok(),
240        MavType::Int16 => i16::try_from(value).is_ok(),
241        MavType::Int32 => i32::try_from(value).is_ok(),
242        MavType::Int64 => i64::try_from(value).is_ok(),
243        _ => cast_integer(mavtype, value) == Some(value),
244    }
245}
246
247fn write_char_array(length: usize, value: &DynamicValue, output: &mut [u8]) -> Result<(), String> {
248    match value {
249        DynamicValue::String(value) => {
250            let value = value.as_bytes();
251            let copied = value.len().min(length);
252            output.fill(0);
253            output[..copied].copy_from_slice(&value[..copied]);
254            Ok(())
255        }
256        DynamicValue::Array(values) if values.len() == length => {
257            for (value, byte) in values.iter().zip(output) {
258                let DynamicValue::U8(value) = value else {
259                    return Err("expected u8 character array elements".to_owned());
260                };
261                *byte = *value;
262            }
263            Ok(())
264        }
265        DynamicValue::Array(values) => Err(format!(
266            "expected a character array of {length} values, got {}",
267            values.len()
268        )),
269        _ => Err(format!("expected a string or array of {length} bytes")),
270    }
271}
272
273fn integer_value(value: &DynamicValue) -> Result<u64, String> {
274    Ok(match value {
275        DynamicValue::U8(value) => u64::from(*value),
276        DynamicValue::U16(value) => u64::from(*value),
277        DynamicValue::U32(value) => u64::from(*value),
278        DynamicValue::U64(value) => *value,
279        DynamicValue::I8(value) => *value as u64,
280        DynamicValue::I16(value) => *value as u64,
281        DynamicValue::I32(value) => *value as u64,
282        DynamicValue::I64(value) => *value as u64,
283        _ => return Err("expected an integer or enum name".to_owned()),
284    })
285}
286
287fn parse_enum_value(value: &str, enumeration: &DynamicEnumDefinition) -> Result<u64, String> {
288    if enumeration.primitive.is_some() {
289        if value.trim().is_empty() {
290            return Ok(0);
291        }
292        value.split('|').try_fold(0, |bits, name| {
293            let name = name.trim();
294            let value = if let Some(hex) = name.strip_prefix("0x") {
295                u64::from_str_radix(hex, 16).map_err(|_| format!("invalid bitmask value {name}"))?
296            } else {
297                enum_entry_value(enumeration, name)?
298            };
299            Ok(bits | value)
300        })
301    } else {
302        enum_entry_value(enumeration, value)
303    }
304}
305
306fn enum_entry_value(enumeration: &DynamicEnumDefinition, name: &str) -> Result<u64, String> {
307    enumeration
308        .entries
309        .iter()
310        .find_map(|(entry, value)| (entry == name).then_some(*value))
311        .ok_or_else(|| format!("unknown entry {name} in enum {}", enumeration.name))
312}
313
314#[cfg(test)]
315mod tests {
316    use super::{MavType, write_value};
317
318    #[test]
319    fn numeric_types_must_match() {
320        let values = [
321            (MavType::UInt8, 1u8.into()),
322            (MavType::UInt16, 1u16.into()),
323            (MavType::UInt32, 1u32.into()),
324            (MavType::UInt64, 1u64.into()),
325            (MavType::Int8, 1i8.into()),
326            (MavType::Int16, 1i16.into()),
327            (MavType::Int32, 1i32.into()),
328            (MavType::Int64, 1i64.into()),
329            (MavType::Float, 1.0f32.into()),
330            (MavType::Double, 1.0f64.into()),
331        ];
332        for (field_type, _) in &values {
333            for (value_type, value) in &values {
334                let result = write_value(field_type, None, value, &mut [0; 8][..field_type.size()]);
335                assert_eq!(result.is_ok(), field_type == value_type);
336            }
337        }
338    }
339
340    #[test]
341    fn array_types_must_match() {
342        let mavtype = MavType::Array(Box::new(MavType::UInt8), 1);
343        for mavtype in [mavtype, MavType::CharArray(1)] {
344            assert!(write_value(&mavtype, None, &[1u8].into(), &mut [0]).is_ok());
345            assert!(write_value(&mavtype, None, &[1u16].into(), &mut [0]).is_err());
346        }
347    }
348}
mavlink/dialects/dynamic_parser.rs

mavlink/dialects/
dynamic_parser.rs
