tasks: simulation: move sim data into a separate partition for flashing

build.rs

@@ -1,5 +1,5 @@
 use std::fs;
-use std::io::Write;
+use std::io::{Read, Seek, Write};
 use std::path::Path;
 use std::fs::File;
 use image::GenericImageView;
@@ -75,6 +75,7 @@ fn write_sim_data() {
 
     let gps_output = output_path.join("gps_test_data.msgpack");
     let motion_output = output_path.join("motion_test_data.msgpack");
+    let unified_output = output_path.join("unified.msgpack");
 
     println!("cargo::rerun-if-changed={}", gps_input.to_str().unwrap());
     if !gps_output.exists() || gps_output.metadata().unwrap().modified().unwrap() < gps_input.metadata().unwrap().modified().unwrap() {
@@ -85,7 +86,7 @@ fn write_sim_data() {
             headers.iter().position(|x| { x == "longitude" }).unwrap(),
             headers.iter().position(|x| { x == "latitude" }).unwrap(),
         );
-        let mut gps_output = File::create(gps_output).unwrap();
+        let mut gps_output = File::create(gps_output.clone()).unwrap();
         let mut last_stamp = 0.0;
         for record in gps_data.records().flatten() {
             let (timestamp, lat, lon) = (
@@ -130,7 +131,7 @@ fn write_sim_data() {
             headers.iter().position(|x| { x == "z" }).unwrap(),
         );
 
-        let mut motion_output = File::create(motion_output).unwrap();
+        let mut motion_output = File::create(motion_output.clone()).unwrap();
         let mut last_stamp = 0.0;
         for (accel_record, gyro_record) in accel_data.records().flatten().zip(gyro_data.records().flatten()) {
             let (timestamp, accel_x, accel_y, accel_z) = (
@@ -158,6 +159,60 @@ fn write_sim_data() {
             }
         }
     }
+
+    // GPS data = 2, motion data = 1
+    
+    let mut unified_fd = File::create(unified_output.clone()).unwrap();
+    let mut motion_output = File::open(motion_output).unwrap();
+    let mut gps_output = File::open(gps_output).unwrap();
+    rmp::encode::write_ext_meta(&mut unified_fd, motion_output.metadata().unwrap().len() as u32, 1).unwrap();
+    let mut buf = Vec::new();
+    motion_output.read_to_end(&mut buf).unwrap();
+    unified_fd.write_all(buf.as_slice()).unwrap();
+
+    rmp::encode::write_ext_meta(&mut unified_fd, gps_output.metadata().unwrap().len() as u32, 2).unwrap();
+    let mut buf = Vec::new();
+    gps_output.read_to_end(&mut buf).unwrap();
+    unified_fd.write_all(buf.as_slice()).unwrap();
+
+    let mut partitions = Reader::from_reader(File::open("partitions.csv").unwrap());
+    let mut data_offset = 0x9000; // Assumes default bootloader size (0x7000) plus partition table (0x2000)
+    let mut data_size = 0;
+    let mut found_sim_partition = false;
+    for p_desc in partitions.records().flatten() {
+        let offset = parse_partition_number(p_desc.get(3).unwrap().trim());
+        data_size = parse_partition_number(p_desc.get(4).unwrap().trim()).unwrap();
+        data_offset = offset.unwrap_or(data_offset);
+        if let Some("sim") = p_desc.get(0) {
+            found_sim_partition = true;
+            break;
+        } else {
+            data_offset += data_size;
+        }
+    }
+
+    if !found_sim_partition {
+        panic!("Could not find a 'sim' partition in partitions.csv!");
+    }
+
+    if buf.len() >= data_size {
+        panic!("Simulation data is too big! Cannot fit {:#x} bytes into a partition with a size of {data_size:#x} bytes.", buf.len());
+    }
+
+    let mut data_flash_script = File::create(output_path.join("flash-sim-data.sh")).unwrap();
+    data_flash_script.write_all(format!("espflash write-bin {data_offset:#x?} {}", unified_output.to_str().unwrap()).as_bytes()).unwrap();
+}
+
+fn parse_partition_number(n: &str) -> Option<usize> {
+    if n.is_empty() {
+        None
+    } else if let Some(hex_offset) = n.strip_prefix("0x") {
+        Some(usize::from_str_radix(hex_offset, 16).unwrap())
+    } else if let Some(mb_offset) = n.strip_suffix("M") {
+        Some(mb_offset.parse::<usize>().unwrap() * 1024 * 1024)
+    } else {
+        Some(n.parse().unwrap())
+    }
 }
 
 fn linker_be_nice() {

src/bin/main.rs

@@ -123,8 +123,21 @@ async fn main(spawner: Spawner) {
 
     #[cfg(feature="simulation")]
     {
-        spawner.must_spawn(renderbug_embassy::tasks::simulation::motion_simulation_task(garage.motion.dyn_sender()));
+        use core::{cell::RefCell, ops::DerefMut};
-        spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(garage.motion.dyn_sender()));
+
+        use alloc::rc::Rc;
+        use esp_storage::FlashStorage;
+        let storage = Rc::new(RefCell::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 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()));
     }
 
     info!("Launching motion engine");

src/tasks/simulation.rs

@@ -1,17 +1,68 @@
+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 esp_storage::FlashStorage;
 use nalgebra::{Vector2, Vector3};
 use log::*;
+use rmp::decode::{RmpRead, RmpReadErr};
 
 use crate::events::{Measurement, SensorSource};
 use crate::Breaker;
 
+struct OffsetReader<S> {
+    storage: Rc<RefCell<S>>,
+    offset: usize
+}
+
+#[derive(Debug)]
+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")
+    }
+}
+
+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) {
+            Ok(_) => {
+                self.offset += buf.len();
+                Ok(())
+            },
+            err => Err(RmpErr{})
+        }
+    }
+}
+
 #[embassy_executor::task]
-pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>) {
+pub async fn motion_simulation_task(storage: Rc<RefCell<FlashStorage>>, offset: usize, events: DynamicSender<'static, Measurement>) {
-    let mut rd = rmp::decode::Bytes::new(include_bytes!("../../target/motion_test_data.msgpack"));
+    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;
+        }
+    }
+    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);
@@ -38,11 +89,21 @@ pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>)
 }
 
 #[embassy_executor::task]
-pub async fn location_simulation_task(events: DynamicSender<'static, Measurement>) {
+pub async fn location_simulation_task(storage: Rc<RefCell<FlashStorage>>, offset: usize, events: DynamicSender<'static, Measurement>) {
-    let mut rd = rmp::decode::Bytes::new(include_bytes!("../../target/gps_test_data.msgpack"));
+    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;
+        }
+    }
+    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);