ulog_rs/
lib.rs

1#![doc = include_str!("../README.md")]
2
3pub mod data_message;
4pub mod dropout_message;
5pub mod format_message;
6pub mod info_message;
7pub mod logged_message;
8pub mod multi_message;
9pub mod parameter_message;
10pub mod subscription_message;
11pub mod tagged_logged_message;
12pub mod ulog;
13
14use byteorder::{LittleEndian, ReadBytesExt};
15use dropout_message::DropoutStats;
16use format_message::FormatMessage;
17use info_message::*;
18use logged_message::LoggedMessage;
19use multi_message::MultiMessage;
20use parameter_message::{DefaultParameterMessage, ParameterMessage};
21use serde_derive::Serialize;
22use std::collections::HashMap;
23use std::io::{self, Read};
24use subscription_message::SubscriptionMessage;
25use tagged_logged_message::*;
26use thiserror::Error;
27
28/// Maximum reasonable message size (64KB should be plenty)
29const MAX_MESSAGE_SIZE: u16 = 65535;
30
31#[derive(Debug, Clone, PartialEq)]
32/// Represents the type of a ULog value, which can be either a basic type or a nested message type.
33///
34/// The `ULogType` enum is used to represent the type of a ULog value. It can either be a `Basic` type,
35/// which corresponds to a primitive data type, or a `Message` type, which represents a nested message
36/// within the ULog file.
37pub enum ULogType {
38    Basic(ULogValueType),
39    Message(String), // For nested message types
40}
41
42// Define the possible C types that can appear in info messages
43#[derive(Debug, Clone, PartialEq, Serialize)]
44/// The possible C types that can appear in info messages.
45/// This enum represents the different data types that can be used in ULog messages.
46pub enum ULogValueType {
47    Int8,
48    UInt8,
49    Int16,
50    UInt16,
51    Int32,
52    UInt32,
53    Int64,
54    UInt64,
55    Float,
56    Double,
57    Bool,
58    Char,
59}
60
61#[derive(Debug, Clone, Serialize)]
62/// An enum representing the different data types that can be used in ULog messages.
63/// This enum provides variants for various primitive types as well as arrays and nested message types.
64pub enum ULogValue {
65    Int8(i8),
66    UInt8(u8),
67    Int16(i16),
68    UInt16(u16),
69    Int32(i32),
70    UInt32(u32),
71    Int64(i64),
72    UInt64(u64),
73    Float(f32),
74    Double(f64),
75    Bool(bool),
76    Char(char),
77    // Array variants
78    Int8Array(Vec<i8>),
79    UInt8Array(Vec<u8>),
80    Int16Array(Vec<i16>),
81    UInt16Array(Vec<u16>),
82    Int32Array(Vec<i32>),
83    UInt32Array(Vec<u32>),
84    Int64Array(Vec<i64>),
85    UInt64Array(Vec<u64>),
86    FloatArray(Vec<f32>),
87    DoubleArray(Vec<f64>),
88    BoolArray(Vec<bool>),
89    CharArray(String),                 // Special case: char arrays are strings
90    Message(Vec<ULogValue>),           // For nested message types
91    MessageArray(Vec<Vec<ULogValue>>), // For arrays of nested message types
92}
93
94impl ULogValue {
95    pub fn calculate_size(&self) -> usize {
96        match self {
97            ULogValue::BoolArray(v) => v.len(),
98            ULogValue::CharArray(s) => s.len(),
99            ULogValue::DoubleArray(v) => v.len() * 8,
100            ULogValue::FloatArray(v) => v.len() * 4,
101            ULogValue::Int16(_) | ULogValue::UInt16(_) => 2,
102            ULogValue::Int16Array(v) => v.len() * 2,
103            ULogValue::Int32(_) | ULogValue::UInt32(_) | ULogValue::Float(_) => 4,
104            ULogValue::Int32Array(v) => v.len() * 4,
105            ULogValue::Int64(_) | ULogValue::UInt64(_) | ULogValue::Double(_) => 8,
106            ULogValue::Int64Array(v) => v.len() * 8,
107            ULogValue::Int8(_) | ULogValue::UInt8(_) | ULogValue::Bool(_) | ULogValue::Char(_) => 1,
108            ULogValue::Int8Array(v) => v.len(),
109            ULogValue::UInt8Array(v) => v.len(),
110            ULogValue::Message(map) => map.iter().map(|v| v.calculate_size()).sum(),
111            ULogValue::MessageArray(arr) => arr
112                .iter()
113                .map(|v| v.iter().map(|inner| inner.calculate_size()).sum::<usize>())
114                .sum(),
115            ULogValue::UInt16Array(vec) => vec.len() * 2,
116            ULogValue::UInt32Array(vec) => vec.len() * 4,
117            ULogValue::UInt64Array(vec) => vec.len() * 8,
118        }
119    }
120}
121
122#[derive(Debug, Error)]
123pub enum ULogError {
124    #[error("IO error: {0}")]
125    Io(#[from] io::Error),
126    #[error("Invalid magic bytes")]
127    InvalidMagic,
128    #[error("Unsupported version: {0}")]
129    UnsupportedVersion(u8),
130    #[error("Invalid message type: {0}")]
131    InvalidMessageType(u8),
132    #[error("Invalid string data")]
133    InvalidString,
134    #[error("Invalid type name: {0}")]
135    InvalidTypeName(String),
136    #[error("Parse error: {0}")]
137    ParseError(String),
138    #[error("IncompatibleFlags: {:?}", .0)]
139    IncompatibleFlags(Vec<u8>),
140}
141// File header (16 bytes)
142#[derive(Debug)]
143/// A header for a ULog file, containing the version and timestamp.
144///
145/// The `ULogHeader` struct represents the header of a ULog file, which includes
146/// the version of the ULog format (`version`) and the timestamp of when the
147/// ULog file was created (`timestamp`). This header is used to parse the
148/// binary data of a ULog file.
149pub struct ULogHeader {
150    pub version: u8,
151    pub timestamp: u64,
152}
153
154// Message header (3 bytes)
155#[derive(Debug)]
156
157/// A header for a ULog message, containing the message size and type.
158///
159/// The `MessageHeader` struct represents the header of a ULog message, which includes
160/// the size of the message in bytes (`msg_size`) and the type of the message (`msg_type`).
161/// This header is used to parse the binary data of a ULog file.
162pub struct MessageHeader {
163    pub msg_size: u16,
164    pub msg_type: u8,
165}
166
167#[derive(Debug)]
168/// A message containing compatibility and incompatibility flags, as well as appended offsets.
169/// The `compat_flags` and `incompat_flags` fields are arrays of 8 bytes each, representing
170/// compatibility and incompatibility flags for the ULog file. The `appended_offsets` field
171/// is an array of 3 `u64` values representing offsets of appended data in the ULog file.
172pub struct FlagBitsMessage {
173    pub compat_flags: [u8; 8],
174    pub incompat_flags: [u8; 8],
175    pub appended_offsets: [u64; 3],
176}
177
178#[derive(Debug)]
179/// A parser for ULog binary data.
180///
181/// The `ULogParser` struct is responsible for parsing the binary data of a ULog file.
182/// It provides methods to access the various components of the ULog data, such as the
183/// header, format messages, subscriptions, logged messages, and parameter messages.
184///
185/// The parser is generic over the `Read` trait, allowing it to work with different
186/// types of input sources, such as files, network streams, or in-memory buffers.
187pub struct ULogParser<R: Read> {
188    reader: R,
189    _version: u8,
190    _current_timestamp: u64,
191    dropout_details: DropoutStats,
192    header: ULogHeader,
193    formats: HashMap<String, FormatMessage>,
194    subscriptions: HashMap<u16, SubscriptionMessage>,
195    logged_messages: Vec<LoggedMessage>,
196    logged_messages_tagged: HashMap<u16, Vec<TaggedLoggedMessage>>,
197    info_messages: HashMap<String, InfoMessage>,
198    initial_params: HashMap<String, ParameterMessage>,
199    multi_messages: HashMap<String, Vec<MultiMessage>>,
200    default_params: HashMap<String, DefaultParameterMessage>,
201    changed_params: HashMap<String, Vec<ParameterMessage>>,
202}
203
204impl<R: Read> ULogParser<R> {
205    pub fn new(mut reader: R) -> Result<Self, ULogError> {
206        // Read and verify magic bytes
207        let mut magic = [0u8; 7];
208        reader.read_exact(&mut magic)?;
209        if magic != [0x55, 0x4C, 0x6F, 0x67, 0x01, 0x12, 0x35] {
210            return Err(ULogError::InvalidMagic);
211        }
212        log::info!("Magic bytes: {:?}", magic);
213        // Read version
214        let version = reader.read_u8()?;
215        if version > 1 {
216            return Err(ULogError::UnsupportedVersion(version));
217        }
218        log::info!("ULog version: {}", version);
219        // Read timestamp
220        let timestamp = reader.read_u64::<LittleEndian>()?;
221
222        let header = ULogHeader { version, timestamp };
223
224        Ok(ULogParser {
225            reader,
226            _current_timestamp: timestamp,
227            _version: version,
228            dropout_details: DropoutStats {
229                total_drops: 0,
230                total_duration_ms: 0,
231                dropouts: Vec::new(),
232            },
233            header,
234            formats: HashMap::new(),
235            subscriptions: HashMap::new(),
236            logged_messages: Vec::new(),
237            logged_messages_tagged: HashMap::new(),
238            info_messages: HashMap::new(),
239            initial_params: HashMap::new(),
240            multi_messages: HashMap::new(),
241            default_params: HashMap::new(),
242            changed_params: HashMap::new(),
243        })
244    }
245
246    pub fn header(&self) -> &ULogHeader {
247        &self.header
248    }
249
250    fn _dump_next_bytes(&mut self, count: usize) -> Result<(), ULogError> {
251        let mut buf = vec![0u8; count];
252        self.reader.read_exact(&mut buf)?;
253        log::debug!("Next {} bytes: {:?}", count, buf);
254        Ok(())
255    }
256
257    pub fn read_message_header(&mut self) -> Result<MessageHeader, ULogError> {
258        let msg_size = self.reader.read_u16::<LittleEndian>()?;
259        let msg_type = self.reader.read_u8()?;
260        Ok(MessageHeader { msg_size, msg_type })
261    }
262
263    pub fn read_flag_bits(&mut self) -> Result<FlagBitsMessage, ULogError> {
264        let mut compat_flags = [0u8; 8];
265        let mut incompat_flags = [0u8; 8];
266        let mut appended_offsets = [0u64; 3];
267
268        self.reader.read_exact(&mut compat_flags)?;
269        self.reader.read_exact(&mut incompat_flags)?;
270
271        for offset in &mut appended_offsets {
272            *offset = self.reader.read_u64::<LittleEndian>()?;
273        }
274
275        // Check incompatible flags
276        if incompat_flags.iter().any(|&x| x != 0) {
277            return Err(ULogError::IncompatibleFlags(incompat_flags.to_vec()));
278        }
279
280        Ok(FlagBitsMessage {
281            compat_flags,
282            incompat_flags,
283            appended_offsets,
284        })
285    }
286
287    fn parse_type_string(type_str: &str) -> Result<(ULogType, Option<usize>), ULogError> {
288        let mut parts = type_str.split('[');
289        let base_type = parts.next().unwrap_or("");
290
291        let array_size = if let Some(size_str) = parts.next() {
292            // Remove the trailing ']' and parse the size
293            Some(
294                size_str
295                    .trim_end_matches(']')
296                    .parse::<usize>()
297                    .map_err(|_| ULogError::ParseError("Invalid array size".to_string()))?,
298            )
299        } else {
300            None
301        };
302
303        let value_type = match base_type {
304            // Basic types
305            "int8_t" => ULogType::Basic(ULogValueType::Int8),
306            "uint8_t" => ULogType::Basic(ULogValueType::UInt8),
307            "int16_t" => ULogType::Basic(ULogValueType::Int16),
308            "uint16_t" => ULogType::Basic(ULogValueType::UInt16),
309            "int32_t" => ULogType::Basic(ULogValueType::Int32),
310            "uint32_t" => ULogType::Basic(ULogValueType::UInt32),
311            "int64_t" => ULogType::Basic(ULogValueType::Int64),
312            "uint64_t" => ULogType::Basic(ULogValueType::UInt64),
313            "float" => ULogType::Basic(ULogValueType::Float),
314            "double" => ULogType::Basic(ULogValueType::Double),
315            "bool" => ULogType::Basic(ULogValueType::Bool),
316            "char" => ULogType::Basic(ULogValueType::Char),
317            // Any other type is treated as a message type
318            _ => ULogType::Message(base_type.to_string()),
319        };
320
321        Ok((value_type, array_size))
322    }
323
324    // Read a value of a given type from the reader
325    fn read_typed_value(
326        &mut self,
327        value_type: &ULogValueType,
328        array_size: Option<usize>,
329    ) -> Result<ULogValue, ULogError> {
330        match (value_type, array_size) {
331            // Single values
332            (ULogValueType::Int8, None) => Ok(ULogValue::Int8(self.reader.read_i8()?)),
333            (ULogValueType::UInt8, None) => Ok(ULogValue::UInt8(self.reader.read_u8()?)),
334            (ULogValueType::Int16, None) => {
335                Ok(ULogValue::Int16(self.reader.read_i16::<LittleEndian>()?))
336            }
337            (ULogValueType::UInt16, None) => {
338                Ok(ULogValue::UInt16(self.reader.read_u16::<LittleEndian>()?))
339            }
340            (ULogValueType::Int32, None) => {
341                Ok(ULogValue::Int32(self.reader.read_i32::<LittleEndian>()?))
342            }
343            (ULogValueType::UInt32, None) => {
344                Ok(ULogValue::UInt32(self.reader.read_u32::<LittleEndian>()?))
345            }
346            (ULogValueType::Int64, None) => {
347                Ok(ULogValue::Int64(self.reader.read_i64::<LittleEndian>()?))
348            }
349            (ULogValueType::UInt64, None) => {
350                Ok(ULogValue::UInt64(self.reader.read_u64::<LittleEndian>()?))
351            }
352            (ULogValueType::Float, None) => {
353                Ok(ULogValue::Float(self.reader.read_f32::<LittleEndian>()?))
354            }
355            (ULogValueType::Double, None) => {
356                Ok(ULogValue::Double(self.reader.read_f64::<LittleEndian>()?))
357            }
358            (ULogValueType::Bool, None) => Ok(ULogValue::Bool(self.reader.read_u8()? != 0)),
359            (ULogValueType::Char, None) => {
360                let c = self.reader.read_u8()? as char;
361                Ok(ULogValue::Char(c))
362            }
363
364            // Array values
365            (ULogValueType::Bool, Some(size)) => {
366                let mut values = vec![0u8; size];
367                self.reader.read_exact(&mut values)?;
368                Ok(ULogValue::BoolArray(
369                    values.iter().map(|&x| x != 0).collect(),
370                ))
371            }
372            (ULogValueType::UInt16, Some(size)) => {
373                let mut values = vec![0u16; size];
374                self.reader.read_u16_into::<LittleEndian>(&mut values)?;
375                Ok(ULogValue::UInt16Array(values))
376            }
377            (ULogValueType::UInt32, Some(size)) => {
378                let mut values = vec![0u32; size];
379                self.reader.read_u32_into::<LittleEndian>(&mut values)?;
380                Ok(ULogValue::UInt32Array(values))
381            }
382            (ULogValueType::Int8, Some(size)) => {
383                let mut values = vec![0i8; size];
384                self.reader.read_i8_into(&mut values)?;
385                Ok(ULogValue::Int8Array(values))
386            }
387            (ULogValueType::UInt8, Some(size)) => {
388                let mut values = vec![0u8; size];
389                self.reader.read_exact(&mut values)?;
390                Ok(ULogValue::UInt8Array(values))
391            }
392            (ULogValueType::Float, Some(size)) => {
393                let mut values = vec![0.0f32; size];
394                self.reader.read_f32_into::<LittleEndian>(&mut values)?;
395                Ok(ULogValue::FloatArray(values))
396            }
397            // Special case for char arrays - treat as strings
398            (ULogValueType::Char, Some(size)) => {
399                let mut bytes = vec![0u8; size];
400                self.reader.read_exact(&mut bytes)?;
401                // Convert to string, trimming any null terminators
402                let s = String::from_utf8_lossy(&bytes)
403                    .trim_matches('\0')
404                    .to_string();
405                Ok(ULogValue::CharArray(s))
406            }
407            ulog_value_type => {
408                log::error!("Unsupported type/size combination");
409                Err(ULogError::ParseError(format!(
410                    "Invalid type/size combination: {:?}",
411                    ulog_value_type
412                )))
413            }
414        }
415    }
416    fn read_string(&mut self, len: usize) -> Result<String, ULogError> {
417        let mut buf = vec![0u8; len];
418        self.reader.read_exact(&mut buf)?;
419        String::from_utf8(buf).map_err(|_| ULogError::InvalidString)
420    }
421
422    /// Check if a byte represents a valid ULog message type
423    fn is_valid_message_type(msg_type: u8) -> bool {
424        let is_valid = matches!(
425            msg_type,
426            b'A' | // Add message
427            b'R' | // Remove message
428            b'D' | // Data message
429            b'I' | // Info message
430            b'M' | // Multi info message
431            b'P' | // Parameter message
432            b'Q' | // Parameter default message
433            b'L' | // Logged string
434            b'C' | // Tagged logged string
435            b'S' | // Synchronization
436            b'O' // Dropout
437        );
438        if !is_valid {
439            log::warn!("Invalid message type: {}", msg_type);
440        }
441        is_valid
442    }
443
444    /// Parses the definitions section of the ULog file until the data section is reached.
445    /// This method reads the flag bits message first, then iterates through the definition
446    /// section, handling various message types such as info messages, format messages,
447    /// initial parameters, and multi messages. Once the first subscription message is
448    /// encountered, the method breaks out of the loop to continue parsing the data section.
449    pub fn parse_definitions(&mut self) -> Result<(), ULogError> {
450        log::info!("Parsing definitions section");
451
452        // Only read flag bits message for version 1
453        if self._version > 0 {
454            let header = self.read_message_header()?;
455            log::debug!(
456                "Flag bits header: msg_size={}, msg_type={}",
457                header.msg_size,
458                header.msg_type as char
459            );
460            if header.msg_type != b'B' {
461                return Err(ULogError::InvalidMessageType(header.msg_type));
462            }
463            let _flag_bits = self.read_flag_bits()?;
464        }
465
466        // Parse definition section until we hit data section
467        loop {
468            let header = self.read_message_header()?;
469            // Debug print the raw bytes
470            log::debug!(
471                "Message header: size={}, type={}({:#x})",
472                header.msg_size,
473                header.msg_type as char,
474                header.msg_type
475            );
476
477            match header.msg_type {
478                b'I' => {
479                    log::debug!("Handling Info message");
480                    self.handle_info_message(&header)?
481                }
482                b'F' => {
483                    log::debug!("Handling Format message");
484                    self.handle_format_message(&header)?
485                }
486                b'P' => {
487                    log::debug!("Handling Parameter message");
488                    self.handle_initial_param(&header)?
489                }
490                b'A' => {
491                    log::debug!("Handling Subscription message");
492                    self.handle_subscription_message(&header)?;
493                    break;
494                }
495                b'Q' => {
496                    self.handle_default_parameter()?;
497                }
498                _ => {
499                    log::debug!("Unknown message type: {}", header.msg_type as char);
500                    // Before skipping, let's dump the next few bytes to see what's going on
501                    let mut peek_buf = vec![0u8; std::cmp::min(16, header.msg_size as usize)];
502                    self.reader.read_exact(&mut peek_buf)?;
503                    log::debug!("Next {} bytes: {:?}", peek_buf.len(), peek_buf);
504
505                    if header.msg_size > 16 {
506                        let mut remainder = vec![0u8; header.msg_size as usize - 16];
507                        self.reader.read_exact(&mut remainder)?;
508                    }
509                }
510            }
511        }
512        Ok(())
513    }
514
515    /// Parses the data section of the ULog file, handling various message types such as
516    /// subscription messages, info messages, multi messages, logged messages, data messages,
517    /// parameter changes, and dropouts. The method reads the message headers and dispatches
518    /// to the appropriate handler for each message type. If an unknown message type is
519    /// encountered, the message is skipped. The method continues parsing the data section
520    /// until the end of the file is reached.
521    pub fn parse_data(&mut self) -> Result<(), ULogError> {
522        loop {
523            match self.read_message_header() {
524                Ok(header) => {
525                    if !Self::is_valid_message_type(header.msg_type) {
526                        return Ok(());
527                    }
528                    if header.msg_size > MAX_MESSAGE_SIZE {
529                        return Ok(());
530                    }
531
532                    match header.msg_type {
533                        b'A' => self.handle_subscription_message(&header)?,
534                        b'I' => self.handle_info_message(&header)?,
535                        b'M' => self.handle_multi_message(&header)?,
536                        b'L' => self.handle_logged_message(&header)?,
537                        b'C' => self.handle_tagged_logged_message(&header)?,
538                        b'D' => self.handle_data_message(&header)?,
539                        b'O' => self.handle_dropout(&header)?,
540                        b'P' => self.handle_parameter_change(&header)?,
541                        // Skip unsubscription messages for now since they're unused
542                        b'R' => self.skip_message(&header)?,
543                        // Skipping synchronization messages for now
544                        b'S' => self.skip_message(&header)?,
545                        b'Q' => {
546                            self.handle_default_parameter()?;
547                        }
548                        _ => self.skip_message(&header)?,
549                    }
550                }
551                Err(ULogError::Io(e)) if e.kind() == io::ErrorKind::UnexpectedEof => {
552                    log::info!("Reached end of file");
553                    break;
554                }
555                Err(e) => return Err(e),
556            }
557        }
558        Ok(())
559    }
560
561    fn skip_message(&mut self, header: &MessageHeader) -> Result<(), ULogError> {
562        let mut buf = vec![0u8; header.msg_size as usize];
563        self.reader.read_exact(&mut buf)?;
564        Ok(())
565    }
566
567    #[allow(dead_code)]
568    /// Main method to create a new `ULogParser` instance.
569    ///
570    /// This function creates a new `ULogParser` instance, parses the definitions,
571    /// and then parses the data from the reader. If any errors occur during the
572    /// parsing process, they are returned as a `ULogError`.
573    pub fn parse_reader(reader: R) -> Result<ULogParser<R>, ULogError> {
574        let mut parser = ULogParser::new(reader)?;
575        parser.parse_definitions()?;
576        parser.parse_data()?;
577        Ok(parser)
578    }
579
580    pub fn last_timestamp(&self) -> u64 {
581        self._current_timestamp
582    }
583}
584
585#[cfg(test)]
586mod tests {
587    use super::*;
588    use std::io::Cursor;
589
590    #[test]
591    fn test_parse_header() {
592        let mut data = vec![];
593        // Magic bytes
594        data.extend_from_slice(&[0x55, 0x4C, 0x6F, 0x67, 0x01, 0x12, 0x35]);
595        // Version
596        data.push(1);
597        // Timestamp
598        data.extend_from_slice(&[0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77]);
599
600        let parser = ULogParser::new(Cursor::new(data)).unwrap();
601        assert_eq!(parser.header.version, 1);
602        assert_eq!(parser.header.timestamp, 0x7766554433221100);
603    }
604}
ulog_rs/lib.rs

ulog_rs/
lib.rs
