ardupilot_binlog/
format.rs

1use std::sync::Arc;
2
3use crate::error::BinlogError;
4use crate::value::FieldValue;
5
6/// Schema definition for a DataFlash message type, parsed from a FMT message.
7#[derive(Debug, Clone)]
8pub struct MessageFormat {
9    /// Message type ID (0–255)
10    pub msg_type: u8,
11    /// Total message length in bytes (including 3-byte header)
12    pub msg_len: u8,
13    /// Message name (e.g. "ATT", "GPS", "IMU")
14    pub name: String,
15    /// Raw format string (e.g. "QccccCCCC")
16    pub format: String,
17    /// Field labels in order (e.g. ["TimeUS", "Roll", "Pitch", ...]).
18    /// Shared with parsed entries via `Arc` to avoid per-entry string copies.
19    pub labels: Arc<[String]>,
20}
21
22/// Return the byte size of a single format character.
23fn field_size(c: char) -> Result<usize, BinlogError> {
24    match c {
25        'b' | 'B' | 'M' => Ok(1),
26        'h' | 'H' | 'c' | 'C' => Ok(2),
27        'i' | 'I' | 'e' | 'E' | 'f' | 'L' | 'n' => Ok(4),
28        'q' | 'Q' | 'd' => Ok(8),
29        'N' => Ok(16),
30        'a' | 'Z' => Ok(64),
31        _ => Err(BinlogError::InvalidFormat(c)),
32    }
33}
34
35/// Decode a fixed-size null-padded byte slice into a trimmed String.
36fn decode_string(bytes: &[u8]) -> String {
37    let end = bytes.iter().position(|&b| b == 0).unwrap_or(bytes.len());
38    String::from_utf8_lossy(&bytes[..end])
39        .trim_end()
40        .to_string()
41}
42
43impl MessageFormat {
44    /// Return the computed payload size from the format string (sum of field sizes).
45    #[must_use]
46    pub fn payload_size(&self) -> usize {
47        self.format.chars().filter_map(|c| field_size(c).ok()).sum()
48    }
49
50    /// Extract a microsecond timestamp from the raw payload bytes, if present.
51    ///
52    /// - Format char `'Q'`: first 8 bytes as `u64` (already microseconds).
53    /// - Format char `'I'` with first label `"TimeMS"` or `"TimeUS"`: first 4 bytes
54    ///   as `u32`, multiplied by 1000 to convert milliseconds → microseconds.
55    /// - Otherwise returns `None`.
56    pub(crate) fn extract_timestamp(&self, payload: &[u8]) -> Option<u64> {
57        match self.format.as_bytes().first().copied() {
58            Some(b'Q') if payload.len() >= 8 => {
59                let bytes: [u8; 8] = payload[..8].try_into().ok()?;
60                Some(u64::from_le_bytes(bytes))
61            }
62            Some(b'I') if payload.len() >= 4 => {
63                let is_time_label = self
64                    .labels
65                    .first()
66                    .map(|l| l == "TimeMS" || l == "TimeUS")
67                    .unwrap_or(false);
68                if is_time_label {
69                    let bytes: [u8; 4] = payload[..4].try_into().ok()?;
70                    Some(u32::from_le_bytes(bytes) as u64 * 1000)
71                } else {
72                    None
73                }
74            }
75            _ => None,
76        }
77    }
78
79    /// Decode a raw payload buffer into field values using this format's type string.
80    /// Labels are shared separately via `Arc<[String]>`.
81    pub fn decode_fields(&self, payload: &[u8]) -> Result<Vec<FieldValue>, BinlogError> {
82        let mut values = Vec::new();
83        let mut offset = 0;
84
85        for c in self.format.chars() {
86            let size = field_size(c)?;
87            if offset + size > payload.len() {
88                return Err(BinlogError::UnexpectedEof);
89            }
90            let bytes = &payload[offset..offset + size];
91            values.push(decode_field(c, bytes)?);
92            offset += size;
93        }
94
95        Ok(values)
96    }
97}
98
99/// Convert a byte slice prefix to a fixed-size array, returning an error if too short.
100fn to_array<const N: usize>(bytes: &[u8]) -> Result<[u8; N], BinlogError> {
101    bytes
102        .get(..N)
103        .and_then(|s| s.try_into().ok())
104        .ok_or(BinlogError::PayloadTooShort)
105}
106
107/// Decode a single field from its raw bytes given the format character.
108fn decode_field(c: char, bytes: &[u8]) -> Result<FieldValue, BinlogError> {
109    let scaled = |raw: f64| FieldValue::Float(raw / 100.0);
110
111    match c {
112        'b' => Ok(FieldValue::Int(bytes[0] as i8 as i64)),
113        'B' | 'M' => Ok(FieldValue::Int(bytes[0] as i64)),
114        'h' | 'H' => {
115            let pair = [bytes[0], bytes[1]];
116            Ok(FieldValue::Int(if c == 'h' {
117                i16::from_le_bytes(pair) as i64
118            } else {
119                u16::from_le_bytes(pair) as i64
120            }))
121        }
122        'i' | 'I' | 'L' => Ok(FieldValue::Int(if c == 'I' {
123            u32::from_le_bytes(to_array(bytes)?) as i64
124        } else {
125            i32::from_le_bytes(to_array(bytes)?) as i64
126        })),
127        'q' => Ok(FieldValue::Int(i64::from_le_bytes(to_array(bytes)?))),
128        'Q' => Ok(FieldValue::Uint(u64::from_le_bytes(to_array(bytes)?))),
129        'f' => Ok(FieldValue::Float(
130            f32::from_le_bytes(to_array(bytes)?) as f64
131        )),
132        'd' => Ok(FieldValue::Float(f64::from_le_bytes(to_array(bytes)?))),
133        'c' | 'e' => Ok(scaled(if c == 'c' {
134            i16::from_le_bytes([bytes[0], bytes[1]]) as f64
135        } else {
136            i32::from_le_bytes(to_array(bytes)?) as f64
137        })),
138        'C' | 'E' => Ok(scaled(if c == 'C' {
139            u16::from_le_bytes([bytes[0], bytes[1]]) as f64
140        } else {
141            u32::from_le_bytes(to_array(bytes)?) as f64
142        })),
143        'n' | 'N' | 'Z' => Ok(FieldValue::String(decode_string(bytes))),
144        'a' => {
145            let arr = bytes
146                .chunks_exact(2)
147                .take(32)
148                .map(|chunk| i16::from_le_bytes([chunk[0], chunk[1]]))
149                .collect();
150            Ok(FieldValue::Array(arr))
151        }
152        _ => Err(BinlogError::InvalidFormat(c)),
153    }
154}
155
156/// Parse an 86-byte FMT payload into a MessageFormat.
157pub(crate) fn parse_fmt_payload(payload: &[u8]) -> Result<MessageFormat, BinlogError> {
158    if payload.len() < 86 {
159        return Err(BinlogError::UnexpectedEof);
160    }
161
162    let msg_type = payload[0];
163    let msg_len = payload[1];
164    let name = decode_string(&payload[2..6]);
165    let format = decode_string(&payload[6..22]);
166    let labels_raw = decode_string(&payload[22..86]);
167    let mut labels: Vec<String> = labels_raw.split(',').map(|s| s.to_string()).collect();
168
169    // Pad with synthetic labels if format has more fields than labels
170    let format_len = format.chars().count();
171    while labels.len() < format_len {
172        labels.push(format!("field_{}", labels.len()));
173    }
174
175    Ok(MessageFormat {
176        msg_type,
177        msg_len,
178        name,
179        format,
180        labels: labels.into(),
181    })
182}
183
184#[cfg(test)]
185mod tests {
186    use super::*;
187
188    #[test]
189    fn field_size_all_types() {
190        assert_eq!(field_size('b').unwrap(), 1);
191        assert_eq!(field_size('B').unwrap(), 1);
192        assert_eq!(field_size('M').unwrap(), 1);
193        assert_eq!(field_size('h').unwrap(), 2);
194        assert_eq!(field_size('H').unwrap(), 2);
195        assert_eq!(field_size('c').unwrap(), 2);
196        assert_eq!(field_size('C').unwrap(), 2);
197        assert_eq!(field_size('i').unwrap(), 4);
198        assert_eq!(field_size('I').unwrap(), 4);
199        assert_eq!(field_size('e').unwrap(), 4);
200        assert_eq!(field_size('E').unwrap(), 4);
201        assert_eq!(field_size('f').unwrap(), 4);
202        assert_eq!(field_size('L').unwrap(), 4);
203        assert_eq!(field_size('n').unwrap(), 4);
204        assert_eq!(field_size('q').unwrap(), 8);
205        assert_eq!(field_size('Q').unwrap(), 8);
206        assert_eq!(field_size('d').unwrap(), 8);
207        assert_eq!(field_size('N').unwrap(), 16);
208        assert_eq!(field_size('a').unwrap(), 64);
209        assert_eq!(field_size('Z').unwrap(), 64);
210    }
211
212    #[test]
213    fn field_size_invalid() {
214        assert!(field_size('x').is_err());
215    }
216
217    #[test]
218    fn payload_size_known_format() {
219        let fmt = MessageFormat {
220            msg_type: 0,
221            msg_len: 0,
222            name: String::new(),
223            format: "QccccCCCC".into(),
224            labels: Arc::from([]),
225        };
226        // Q=8, c=2*4, C=2*4 = 8+8+8 = 24
227        assert_eq!(fmt.payload_size(), 24);
228    }
229
230    #[test]
231    fn decode_integer_types() {
232        // b: signed i8
233        assert_eq!(decode_field('b', &[0xFF]).unwrap(), FieldValue::Int(-1));
234        // B: unsigned u8
235        assert_eq!(decode_field('B', &[0xFF]).unwrap(), FieldValue::Int(255));
236        // h: i16 LE
237        assert_eq!(
238            decode_field('h', &[0x00, 0x80]).unwrap(),
239            FieldValue::Int(-32768)
240        );
241        // H: u16 LE
242        assert_eq!(
243            decode_field('H', &[0xFF, 0xFF]).unwrap(),
244            FieldValue::Int(65535)
245        );
246        // i: i32 LE
247        assert_eq!(
248            decode_field('i', &[0x01, 0x00, 0x00, 0x00]).unwrap(),
249            FieldValue::Int(1)
250        );
251        // I: u32 LE
252        assert_eq!(
253            decode_field('I', &[0xFF, 0xFF, 0xFF, 0xFF]).unwrap(),
254            FieldValue::Int(u32::MAX as i64)
255        );
256        // q: i64 LE
257        let bytes = (-42i64).to_le_bytes();
258        assert_eq!(decode_field('q', &bytes).unwrap(), FieldValue::Int(-42));
259        // Q: u64 LE
260        let bytes = u64::MAX.to_le_bytes();
261        assert_eq!(
262            decode_field('Q', &bytes).unwrap(),
263            FieldValue::Uint(u64::MAX)
264        );
265        // M: flight mode u8
266        assert_eq!(decode_field('M', &[5]).unwrap(), FieldValue::Int(5));
267        // L: lat/lon degE7 i32
268        let val: i32 = 473_977_000; // ~47.3977 degrees
269        assert_eq!(
270            decode_field('L', &val.to_le_bytes()).unwrap(),
271            FieldValue::Int(val as i64)
272        );
273    }
274
275    #[test]
276    fn decode_float_types() {
277        // f: f32
278        let v = 1.5f32;
279        assert_eq!(
280            decode_field('f', &v.to_le_bytes()).unwrap(),
281            FieldValue::Float(v as f64)
282        );
283        // d: f64
284        let v = 123.456789f64;
285        assert_eq!(
286            decode_field('d', &v.to_le_bytes()).unwrap(),
287            FieldValue::Float(v)
288        );
289    }
290
291    #[test]
292    fn decode_scaled_types() {
293        // c: i16 / 100
294        let v: i16 = 4500; // 45.00
295        assert_eq!(
296            decode_field('c', &v.to_le_bytes()).unwrap(),
297            FieldValue::Float(45.0)
298        );
299        // C: u16 / 100
300        let v: u16 = 1234; // 12.34
301        assert_eq!(
302            decode_field('C', &v.to_le_bytes()).unwrap(),
303            FieldValue::Float(12.34)
304        );
305        // e: i32 / 100
306        let v: i32 = -5000; // -50.00
307        assert_eq!(
308            decode_field('e', &v.to_le_bytes()).unwrap(),
309            FieldValue::Float(-50.0)
310        );
311        // E: u32 / 100
312        let v: u32 = 100_000; // 1000.00
313        assert_eq!(
314            decode_field('E', &v.to_le_bytes()).unwrap(),
315            FieldValue::Float(1000.0)
316        );
317    }
318
319    #[test]
320    fn decode_string_types() {
321        // n: 4-byte null-padded
322        assert_eq!(
323            decode_field('n', b"ATT\0").unwrap(),
324            FieldValue::String("ATT".into())
325        );
326        // N: 16-byte null-padded
327        let mut buf = [0u8; 16];
328        buf[..5].copy_from_slice(b"Hello");
329        assert_eq!(
330            decode_field('N', &buf).unwrap(),
331            FieldValue::String("Hello".into())
332        );
333        // Z: 64-byte null-padded
334        let mut buf = [0u8; 64];
335        buf[..11].copy_from_slice(b"Test string");
336        assert_eq!(
337            decode_field('Z', &buf).unwrap(),
338            FieldValue::String("Test string".into())
339        );
340    }
341
342    #[test]
343    fn decode_array_type() {
344        let mut buf = [0u8; 64];
345        for i in 0..32i16 {
346            let bytes = i.to_le_bytes();
347            buf[i as usize * 2] = bytes[0];
348            buf[i as usize * 2 + 1] = bytes[1];
349        }
350        let expected: Vec<i16> = (0..32).collect();
351        assert_eq!(
352            decode_field('a', &buf).unwrap(),
353            FieldValue::Array(expected)
354        );
355    }
356
357    #[test]
358    fn decode_fields_values() {
359        let fmt = MessageFormat {
360            msg_type: 0x81,
361            msg_len: 27,
362            name: "TEST".into(),
363            format: "Qh".into(),
364            labels: vec!["TimeUS".into(), "Val".into()].into(),
365        };
366        let mut payload = Vec::new();
367        payload.extend_from_slice(&1000u64.to_le_bytes()); // Q
368        payload.extend_from_slice(&(-42i16).to_le_bytes()); // h
369        let values = fmt.decode_fields(&payload).unwrap();
370        assert_eq!(values.len(), 2);
371        assert_eq!(values[0], FieldValue::Uint(1000));
372        assert_eq!(values[1], FieldValue::Int(-42));
373    }
374
375    #[test]
376    fn parse_fmt_payload_pads_labels() {
377        let mut payload = [0u8; 86];
378        payload[0] = 0x82;
379        payload[1] = 5;
380        payload[2..6].copy_from_slice(b"X\0\0\0");
381        payload[6..8].copy_from_slice(b"BB");
382        payload[22..27].copy_from_slice(b"First");
383        let mf = parse_fmt_payload(&payload).unwrap();
384        assert_eq!(mf.labels.len(), 2);
385        assert_eq!(mf.labels[0], "First");
386        assert_eq!(mf.labels[1], "field_1");
387    }
388
389    #[test]
390    fn parse_fmt_payload_roundtrip() {
391        let mut payload = [0u8; 86];
392        payload[0] = 0x81; // type
393        payload[1] = 27; // length
394        payload[2..6].copy_from_slice(b"ATT\0"); // name
395        let fmt_str = b"QccccCCCC";
396        payload[6..6 + fmt_str.len()].copy_from_slice(fmt_str);
397        let labels = b"TimeUS,Roll,Pitch,Yaw,DesRoll,DesPitch,DesYaw,ErrRP,ErrYaw";
398        payload[22..22 + labels.len()].copy_from_slice(labels);
399
400        let mf = parse_fmt_payload(&payload).unwrap();
401        assert_eq!(mf.msg_type, 0x81);
402        assert_eq!(mf.msg_len, 27);
403        assert_eq!(mf.name, "ATT");
404        assert_eq!(mf.format, "QccccCCCC");
405        assert_eq!(mf.labels.len(), 9);
406        assert_eq!(mf.labels[0], "TimeUS");
407        assert_eq!(mf.labels[8], "ErrYaw");
408    }
409
410    #[test]
411    fn parse_fmt_payload_too_short() {
412        assert!(parse_fmt_payload(&[0u8; 10]).is_err());
413    }
414
415    #[test]
416    fn string_with_no_null() {
417        // Full 4 bytes, no null terminator
418        assert_eq!(
419            decode_field('n', b"ABCD").unwrap(),
420            FieldValue::String("ABCD".into())
421        );
422    }
423}
ardupilot_binlog/format.rs

ardupilot_binlog/
format.rs
