diff --git a/src/bin/main.rs b/src/bin/main.rs index 9f505ac..f2a8471 100644 --- a/src/bin/main.rs +++ b/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())); - spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(storage, data_offset, garage.motion.dyn_sender())); + for sim_data in SimDataTable::open(storage.clone(), data_offset) { + 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"); diff --git a/src/events.rs b/src/events.rs index d095d4c..65a2596 100644 --- a/src/events.rs +++ b/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, - GPS, + IMU = 1, + GPS = 2, // Fusion outputs GravityReference, @@ -96,6 +98,16 @@ pub enum SensorSource { Simulation } +impl From 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, diff --git a/src/tasks/simulation.rs b/src/tasks/simulation.rs index bc6f4ad..efe61cd 100644 --- a/src/tasks/simulation.rs +++ b/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::Breaker; +use crate::events::{Measurement, SensorSource, SensorState}; + +pub struct SharedFlash { + storage: Rc> +} + +impl Clone for SharedFlash { + fn clone(&self) -> Self { + Self { + storage: Rc::clone(&self.storage) + } + } +} + +impl SharedFlash { + pub fn new(storage: S) -> Self { + Self { + storage: Rc::new(RefCell::new(storage)) + } + } +} + +impl Storage for SharedFlash { + fn write(&mut self, offset: u32, bytes: &[u8]) -> Result<(), Self::Error> { + self.storage.borrow_mut().write(offset, bytes) + } +} + +impl ReadStorage for SharedFlash { + 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 { + storage: S, + reader: OffsetReader, +} + +impl SimDataTable { + pub fn open(storage: S, offset: usize) -> Self { + debug!("Opening simulation data at {offset:#02x}"); + Self { + reader: OffsetReader::new(storage.clone(), offset), + storage + } + } +} + +impl Iterator for SimDataTable { + type Item = SimDataReader; + + fn next(&mut self) -> Option { + 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 { - storage: Rc>, + storage: S, offset: usize } -#[derive(Debug)] -struct RmpErr{} -impl RmpReadErr for RmpErr { +impl OffsetReader { + pub const fn new(storage: S, offset: usize) -> Self { + // TODO: Should add bounds checking since we will know the size of the chunk already + Self { + storage, + offset + } + } -} - -impl core::fmt::Display for RmpErr { - fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { - f.write_str("RmpErr") + pub const fn pos(&self) -> usize { + self.offset } } @@ -38,87 +106,109 @@ impl RmpRead for OffsetReader { 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>, offset: usize, events: DynamicSender<'static, Measurement>) { - let mut rd = OffsetReader { storage, offset }; - let mut runtime_secs = Breaker::default(); - let mut runtime = Duration::default(); - events.send(Measurement::SensorOnline(SensorSource::IMU)).await; - loop { - let this_type = rmp::decode::read_ext_meta(&mut rd).unwrap(); - if this_type.typeid != 1 { - rd.offset += this_type.size as usize; - } else { - break; + +pub struct SimDataReader { + reader: OffsetReader, + srcid: SensorSource, + runtime: Duration +} + +impl SimDataReader { + pub fn open(storage: S, offset: usize, srcid: SensorSource) -> Self { + debug!("Opening {srcid:?} sim data chunk at {offset:#02x}"); + Self { + 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) { - assert_eq!(size, 7, "Expected 7 fields, but only found {size}"); - let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64); + + pub fn srcid(&self) -> SensorSource { + self.srcid + } + + pub async fn read_next(&mut self) -> Result { + 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 { + 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 rd).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 self.reader).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 rd).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 self.reader).unwrap() as f32, + rmp::decode::read_f64(&mut self.reader).unwrap() as f32, ); - runtime += delay; - runtime_secs.set(runtime.as_secs()); - if runtime_secs.read_tripped().is_some() { - events.send(Measurement::SimulationProgress(SensorSource::IMU, runtime, 0.0)).await; - } - Timer::after(delay).await; - events.send(Measurement::IMU{ accel, gyro }).await; + + Ok(Measurement::IMU { accel, gyro }) + } + + async fn read_gps(&mut self) -> Result { + self.verify_chunk_len(3); + 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] -pub async fn location_simulation_task(storage: Rc>, offset: usize, events: DynamicSender<'static, Measurement>) { - let mut rd = OffsetReader { storage, offset }; - let mut runtime_secs = Breaker::default(); - let mut runtime = Duration::default(); - events.send(Measurement::SensorOnline(SensorSource::GPS)).await; - loop { - 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; - } +#[derive(Debug)] +pub struct RmpErr{} +impl RmpReadErr for RmpErr {} + +impl core::fmt::Display for RmpErr { + fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { + f.write_str("RmpErr") } - 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}"); - let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64); - let coords = Vector2::new( - rmp::decode::read_f64(&mut rd).unwrap(), - rmp::decode::read_f64(&mut rd).unwrap() - ); - runtime += delay; - runtime_secs.set(runtime.as_secs()); - if runtime_secs.read_tripped().is_some() { - events.send(Measurement::SimulationProgress(SensorSource::GPS, runtime, 0.0)).await; - } - Timer::after(delay).await; - events.send(Measurement::GPS(Some(coords))).await; +} + +#[embassy_executor::task(pool_size = 2)] +pub async fn simulation_task(mut reader: SimDataReader>, events: DynamicSender<'static, Measurement>) { + warn!("Starting simulation for {:?}", reader.srcid()); + + events.send(Measurement::SensorHardwareStatus(SensorSource::Simulation, SensorState::AcquiringFix)).await; + events.send(Measurement::SensorHardwareStatus(reader.srcid(), SensorState::Online)).await; + + // TODO: SimulationProgress updates + while let Ok(next_evt) = reader.read_next().await { + events.send(next_evt).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()); } \ No newline at end of file