simdata: implement bundle streams

src/events.rs

@@ -96,7 +96,8 @@ impl From<StreamType> for SensorSource {
         match value {
             StreamType::Annotations => Self::Annotations,
             StreamType::GPS => Self::GPS,
-            StreamType::IMU => Self::IMU
+            StreamType::IMU => Self::IMU,
+            StreamType::Bundle => unimplemented!()
         }
     }
 }

src/lib.rs

@@ -8,11 +8,8 @@ pub mod animation;
 pub mod idle;
 pub mod logging;
 pub mod graphics;
-
-#[cfg(feature="simulation")]
+pub mod tracing;
 pub mod storage;
-
-#[cfg(feature="simulation")]
 pub mod simdata;
 
 extern crate alloc;

src/simdata.rs

@@ -6,6 +6,7 @@ pub trait RmpData: Sized {
 }
 
 pub trait EventRecord: RmpData {
+    fn stream_id() -> StreamType;
     fn field_count() -> usize;
 }
 
@@ -28,14 +29,15 @@ impl<E> From<E> for SimDataError<E> {
     }
 }
 
-#[derive(Debug)]
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
 pub enum StreamType {
     IMU,
     GPS,
-    Annotations
+    Annotations,
+    Bundle
 }
 
-impl TryFrom<i8> for StreamType {    
+impl TryFrom<i8> for StreamType {
     type Error = ();
     
     fn try_from(value: i8) -> Result<Self, Self::Error> {
@@ -43,6 +45,7 @@ impl TryFrom<i8> for StreamType {
             1 => Ok(StreamType::IMU),
             2 => Ok(StreamType::GPS),
             3 => Ok(StreamType::Annotations),
+            4 => Ok(StreamType::Bundle),
             _ => Err(())
         }
     }
@@ -53,7 +56,8 @@ impl From<StreamType> for i8 {
         match value {
             StreamType::IMU => 1,
             StreamType::GPS => 2,
-            StreamType::Annotations => 3
+            StreamType::Annotations => 3,
+            StreamType::Bundle => 4
         }
     }
 }
@@ -65,19 +69,48 @@ pub struct StreamIndex {
 
 impl RmpData for StreamIndex {
     fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
-        rmp::decode::read_array_len(reader).map(|count| {
-            Self {
-                count: count as usize
-            }
-        }).map_err(|_| { SimDataError::StreamIndexMissing })
+        if rmp::decode::read_u16(reader)? != 0xDA1A {
+            Err(SimDataError::StreamIndexMissing)
+        } else {
+            rmp::decode::read_array_len(reader).map(|count| {
+                Self {
+                    count: count as usize
+                }
+            }).map_err(|err| { SimDataError::DecodeError(err) })
+        }
     }
 
     fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_u16(writer, 0xDA1A)?;
         rmp::encode::write_array_len(writer, self.count as u32)?;
         Ok(())
     }
 }
 
+
+
+#[derive(Debug)]
+pub struct BundleEventHeader {
+    pub id: StreamType,
+}
+
+impl RmpData for BundleEventHeader {
+    fn from_rmp<Reader: RmpRead>(reader: &mut Reader) -> Result<Self, SimDataError<ValueReadError<Reader::Error>>> {
+        let meta = rmp::decode::read_i8(reader)?;
+        if let Ok(id) = meta.try_into() {
+            Ok(Self {
+                id
+            })
+        } else {
+            Err(SimDataError::EventHeaderMissing)
+        }
+    }
+
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_i8(writer, self.id.into())
+    }
+}
+
 #[derive(Debug)]
 pub struct StreamHeader {
     pub id: StreamType,
@@ -97,8 +130,9 @@ impl RmpData for StreamHeader {
         }
     }
 
-    fn write_rmp<Writer: RmpWrite>(&self, _writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
-        todo!()
+    fn write_rmp<Writer: RmpWrite>(&self, writer: &mut Writer) -> Result<(), ValueWriteError<Writer::Error>> {
+        rmp::encode::write_ext_meta(writer, self.size as u32, self.id.into())?;
+        Ok(())
     }
 }
 
@@ -158,12 +192,20 @@ impl EventRecord for GPSReading {
     fn field_count() -> usize {
         2
     }
+
+    fn stream_id() -> StreamType {
+        StreamType::GPS
+    }
 }
 
 impl EventRecord for IMUReading {
     fn field_count() -> usize {
         6
     }
+
+    fn stream_id() -> StreamType {
+        StreamType::IMU
+    }
 }
 
 impl RmpData for GPSReading {
@@ -239,4 +281,8 @@ impl EventRecord for AnnotationReading {
     fn field_count() -> usize {
         1
     }
+
+    fn stream_id() -> StreamType {
+        StreamType::Annotations
+    }
 }