From 41f69833e560d5885371f567f10aed8f68dd0c7e Mon Sep 17 00:00:00 2001 From: Victoria Fischer Date: Wed, 5 Nov 2025 11:56:10 +0100 Subject: [PATCH] tasks: simulation: move sim data into a separate partition for flashing --- build.rs | 61 ++++++++++++++++++++++++++++++++++-- src/bin/main.rs | 17 ++++++++-- src/tasks/simulation.rs | 69 ++++++++++++++++++++++++++++++++++++++--- 3 files changed, 138 insertions(+), 9 deletions(-) diff --git a/build.rs b/build.rs index b094135..ae9715e 100644 --- a/build.rs +++ b/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 { + 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::().unwrap() * 1024 * 1024) + } else { + Some(n.parse().unwrap()) + } } fn linker_be_nice() { diff --git a/src/bin/main.rs b/src/bin/main.rs index 0e2dc59..9f505ac 100644 --- a/src/bin/main.rs +++ b/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())); - spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(garage.motion.dyn_sender())); + use core::{cell::RefCell, ops::DerefMut}; + + 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"); diff --git a/src/tasks/simulation.rs b/src/tasks/simulation.rs index f39458c..bc6f4ad 100644 --- a/src/tasks/simulation.rs +++ b/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 { + storage: Rc>, + 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 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) { + Ok(_) => { + self.offset += buf.len(); + Ok(()) + }, + err => Err(RmpErr{}) + } + } +} + #[embassy_executor::task] -pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>) { - let mut rd = rmp::decode::Bytes::new(include_bytes!("../../target/motion_test_data.msgpack")); +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; + } + } + 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>) { - let mut rd = rmp::decode::Bytes::new(include_bytes!("../../target/gps_test_data.msgpack")); +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; + } + } + 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);