tasks: simulation: refactor the simulation tasks to common types that can simulate any sensor

src/bin/main.rs

@@ -123,21 +123,23 @@ async fn main(spawner: Spawner) {
 
     #[cfg(feature="simulation")]
     {
-        use core::{cell::RefCell, ops::DerefMut};
-
-        use alloc::rc::Rc;
         use esp_storage::FlashStorage;
-        let storage = Rc::new(RefCell::new(FlashStorage::new()));
+        use renderbug_embassy::tasks::simulation::{SharedFlash, SimDataTable};
+        let mut storage = SharedFlash::new(FlashStorage::new());
         let mut buf = [8; 1024];
-        let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(storage.borrow_mut().deref_mut(), &mut buf).unwrap();
+        let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut buf).unwrap();
         let data_partition = partitions.find_partition(
             esp_bootloader_esp_idf::partitions::PartitionType::Data(
                 esp_bootloader_esp_idf::partitions::DataPartitionSubType::Undefined
         )).expect("Unable to read partition table").expect("Could not find data partition!");
         let data_offset = data_partition.offset() as usize;
         info!("Loading simulation data starting at {data_offset:#02x}");
-        spawner.must_spawn(renderbug_embassy::tasks::simulation::motion_simulation_task(Rc::clone(&storage), data_offset, garage.motion.dyn_sender()));
+        for sim_data in SimDataTable::open(storage.clone(), data_offset) {
-        spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(storage, data_offset, garage.motion.dyn_sender()));
+            info!("Found simulation data for {:?}", sim_data.srcid());
+            if spawner.spawn(renderbug_embassy::tasks::simulation::simulation_task(sim_data, garage.motion.dyn_sender())).is_err() {
+                error!("Unable to spawn simulation task! Increase the task pool size.");
+            }
+        }
     }
 
     info!("Launching motion engine");

src/events.rs

@@ -80,11 +80,13 @@ pub enum Telemetry {
     Prediction(Prediction),
 }
 
+// GPS data = 2, motion data = 1
 #[derive(Debug, EnumSetType, Enum)]
 pub enum SensorSource {
+    Unknown = 0,
     // Real hardware
-    IMU,
+    IMU = 1,
-    GPS,
+    GPS = 2,
 
     // Fusion outputs
     GravityReference,
@@ -96,6 +98,16 @@ pub enum SensorSource {
     Simulation
 }
 
+impl From<i8> for SensorSource {
+    fn from(value: i8) -> Self {
+        match value {
+            1 => SensorSource::IMU,
+            2 => SensorSource::GPS,
+            _ => SensorSource::Unknown
+        }
+    }
+}
+
 #[derive(Debug)]
 pub struct BusGarage {
     pub motion: Channel<NoopRawMutex, Measurement, 5>,

src/tasks/simulation.rs

@@ -2,35 +2,103 @@ use core::cell::RefCell;
 use core::fmt::Formatter;
 
 use alloc::rc::Rc;
-use alloc::sync::Arc;
-use embassy_embedded_hal::flash;
-use embassy_sync::blocking_mutex::raw::NoopRawMutex;
 use embassy_sync::channel::DynamicSender;
-use embassy_sync::mutex::Mutex;
 use embassy_time::{Duration, Timer};
-use embedded_storage::ReadStorage;
+use embedded_storage::{ReadStorage, Storage};
 use esp_storage::FlashStorage;
 use nalgebra::{Vector2, Vector3};
 use log::*;
 use rmp::decode::{RmpRead, RmpReadErr};
 
-use crate::events::{Measurement, SensorSource};
+use crate::events::{Measurement, SensorSource, SensorState};
-use crate::Breaker;
+
+pub struct SharedFlash<S> {
+    storage: Rc<RefCell<S>>
+}
+
+impl<S> Clone for SharedFlash<S> {
+    fn clone(&self) -> Self {
+        Self {
+            storage: Rc::clone(&self.storage)
+        }
+    }
+}
+
+impl<S> SharedFlash<S> {
+    pub fn new(storage: S) -> Self {
+        Self {
+            storage: Rc::new(RefCell::new(storage))
+        }
+    }
+}
+
+impl<S: Storage> Storage for SharedFlash<S> {
+    fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> {
+        self.storage.borrow_mut().write(offset, bytes)
+    }
+}
+
+impl<S: ReadStorage> ReadStorage for SharedFlash<S> {
+    type Error = S::Error;
+
+    fn read(&mut self, offset: u32, bytes: &mut [u8]) -> Result<(), Self::Error> {
+        self.storage.borrow_mut().read(offset, bytes)
+    }
+
+    fn capacity(&self) -> usize {
+        self.storage.borrow().capacity()
+    }
+}
+
+pub struct SimDataTable<S> {
+    storage: S,
+    reader: OffsetReader<S>,
+}
+
+impl<S: ReadStorage + Clone> SimDataTable<S> {
+    pub fn open(storage: S, offset: usize) -> Self {
+        debug!("Opening simulation data at {offset:#02x}");
+        Self {
+            reader: OffsetReader::new(storage.clone(), offset),
+            storage
+        }
+    }
+}
+
+impl<S: ReadStorage + Clone> Iterator for SimDataTable<S> {
+    type Item = SimDataReader<S>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if let Ok(this_type) = rmp::decode::read_ext_meta(&mut self.reader) {
+            debug!("Found type={this_type:?} offset={:#02x}", self.reader.pos());
+            let start_pos = self.reader.pos();
+            self.reader.offset += this_type.size as usize;
+            match this_type.typeid.into() {
+                SensorSource::Unknown => panic!("Unsupported sensor type"),
+                srcid => Some(SimDataReader::open(self.storage.clone(), start_pos, srcid))
+            }
+        } else {
+            None
+        }
+    }
+}
 
 struct OffsetReader<S> {
-    storage: Rc<RefCell<S>>,
+    storage: S,
     offset: usize
 }
 
-#[derive(Debug)]
+impl<S: ReadStorage> OffsetReader<S> {
-struct RmpErr{}
+    pub const fn new(storage: S, offset: usize) -> Self {
-impl RmpReadErr for RmpErr {
+        // TODO: Should add bounds checking since we will know the size of the chunk already
+        Self {
+            storage,
+            offset
+        }
+    }
 
-}
+    pub const fn pos(&self) -> usize {
-
+        self.offset
-impl core::fmt::Display for RmpErr {
-    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-        f.write_str("RmpErr")
     }
 }
 
@@ -38,87 +106,109 @@ impl<S: ReadStorage> RmpRead for OffsetReader<S> {
     type Error = RmpErr;
 
     fn read_exact_buf(&mut self, buf: &mut [u8]) -> Result<(), Self::Error> {
-        match self.storage.borrow_mut().read(self.offset as u32, buf) {
+        match self.storage.read(self.offset as u32, buf) {
             Ok(_) => {
                 self.offset += buf.len();
                 Ok(())
             },
-            err => Err(RmpErr{})
+            _ => Err(RmpErr{})
         }
     }
 }
 
-#[embassy_executor::task]
+
-pub async fn motion_simulation_task(storage: Rc<RefCell<FlashStorage>>, offset: usize, events: DynamicSender<'static, Measurement>) {
+pub struct SimDataReader<S> {
-    let mut rd = OffsetReader { storage, offset };
+    reader: OffsetReader<S>,
-    let mut runtime_secs = Breaker::default();
+    srcid: SensorSource,
-    let mut runtime = Duration::default();
+    runtime: Duration
-    events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
+}
-    loop {
+
-        let this_type = rmp::decode::read_ext_meta(&mut rd).unwrap();
+impl<S: ReadStorage> SimDataReader<S> {
-        if this_type.typeid != 1 {
+    pub fn open(storage: S, offset: usize, srcid: SensorSource) -> Self {
-            rd.offset += this_type.size as usize;
+        debug!("Opening {srcid:?} sim data chunk at {offset:#02x}");
-        } else {
+        Self {
-            break;
+            reader: OffsetReader::new(storage, offset),
+            srcid,
+            runtime: Default::default()
         }
     }
-    info!("Found motion data at {:#02x}", rd.offset);
+
-    while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
+    pub fn srcid(&self) -> SensorSource {
-        assert_eq!(size, 7, "Expected 7 fields, but only found {size}");
+        self.srcid
-        let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
+    }
+
+    pub async fn read_next(&mut self) -> Result<Measurement, ()> {
+        match self.srcid {
+            SensorSource::IMU => self.read_motion().await,
+            SensorSource::GPS => self.read_gps().await,
+            srcid => unimplemented!("{srcid:?} is not a simulatable sensor!")
+        }
+    }
+
+    fn verify_chunk_len(&mut self, length: u32) {
+        let chunk_len = rmp::decode::read_array_len(&mut self.reader).expect("Could not find the chunk length!");
+        assert_eq!(chunk_len, length, "Expected {length} fields but instead found {chunk_len}");
+    }
+
+    async fn read_delay_field(&mut self) {
+        let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut self.reader).expect("Expected to find timestamp") * 1000.0) as u64);
+        self.runtime += delay;
+        Timer::after(delay).await
+    }
+
+    async fn read_motion(&mut self) -> Result<Measurement, ()> {
+        self.verify_chunk_len(7);
+        self.read_delay_field().await;
         let accel = Vector3::new(
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
         );
         let gyro = Vector3::new(
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
-            rmp::decode::read_f64(&mut rd).unwrap() as f32,
+            rmp::decode::read_f64(&mut self.reader).unwrap() as f32,
         );
-        runtime += delay;
+
-        runtime_secs.set(runtime.as_secs());
+        Ok(Measurement::IMU { accel, gyro })
-        if runtime_secs.read_tripped().is_some() {
+    }
-            events.send(Measurement::SimulationProgress(SensorSource::IMU, runtime, 0.0)).await;
+
-        }
+    async fn read_gps(&mut self) -> Result<Measurement, ()> {
-        Timer::after(delay).await;
+        self.verify_chunk_len(3);
-        events.send(Measurement::IMU{ accel, gyro }).await;
+        self.read_delay_field().await;
+        let coords = Vector2::new(
+            rmp::decode::read_f64(&mut self.reader).unwrap(),
+            rmp::decode::read_f64(&mut self.reader).unwrap()
+        );
+        
+        Ok(Measurement::GPS(Some(coords)))
     }
-    events.send(Measurement::SensorOffline(SensorSource::IMU)).await;
-    warn!("End of motion recording");
 }
 
-#[embassy_executor::task]
+#[derive(Debug)]
-pub async fn location_simulation_task(storage: Rc<RefCell<FlashStorage>>, offset: usize, events: DynamicSender<'static, Measurement>) {
+pub struct RmpErr{}
-    let mut rd = OffsetReader { storage, offset };
+impl RmpReadErr for RmpErr {}
-    let mut runtime_secs = Breaker::default();
+
-    let mut runtime = Duration::default();
+impl core::fmt::Display for RmpErr {
-    events.send(Measurement::SensorOnline(SensorSource::GPS)).await;
+    fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result {
-    loop {
+        f.write_str("RmpErr")
-        let this_type = rmp::decode::read_ext_meta(&mut rd).unwrap();
-        info!("Found type={this_type:?}");
-        if this_type.typeid != 2 {
-            rd.offset += this_type.size as usize;
-        } else {
-            break;
-        }
     }
-    info!("Found GPS data at {:#02x}", rd.offset);
+}
-    while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
+
-        assert_eq!(size, 3, "Expected 3 fields, but only found {size}");
+#[embassy_executor::task(pool_size = 2)]
-        let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
+pub async fn simulation_task(mut reader: SimDataReader<SharedFlash<FlashStorage>>, events: DynamicSender<'static, Measurement>) {
-        let coords = Vector2::new(
+    warn!("Starting simulation for {:?}", reader.srcid());
-            rmp::decode::read_f64(&mut rd).unwrap(),
+
-            rmp::decode::read_f64(&mut rd).unwrap()
+    events.send(Measurement::SensorHardwareStatus(SensorSource::Simulation, SensorState::AcquiringFix)).await;
-        );
+    events.send(Measurement::SensorHardwareStatus(reader.srcid(), SensorState::Online)).await;
-        runtime += delay;
+
-        runtime_secs.set(runtime.as_secs());
+    // TODO: SimulationProgress updates
-        if runtime_secs.read_tripped().is_some() {
+    while let Ok(next_evt) = reader.read_next().await {
-            events.send(Measurement::SimulationProgress(SensorSource::GPS, runtime, 0.0)).await;
+        events.send(next_evt).await;
-        }
-        Timer::after(delay).await;
-        events.send(Measurement::GPS(Some(coords))).await;
     }
-    events.send(Measurement::SensorOffline(SensorSource::GPS)).await;
+
-    warn!("End of GPS recording");
+    events.send(Measurement::SensorHardwareStatus(reader.srcid(), SensorState::Offline)).await;
+    events.send(Measurement::SensorHardwareStatus(SensorSource::Simulation, SensorState::Degraded)).await;
+
+    warn!("End of simulation for {:?}", reader.srcid());
 }