tdfischer/renderbug-bike
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bump a lot of big changes I dont want to break down into individual commits

Browse Source
This commit is contained in:
Victoria Fischer
2025-10-11 16:34:09 +02:00
parent 0e9e0c1b13
commit 8280f38185
48 changed files with 1275467 additions and 394056 deletions
Download Patch File Download Diff File Expand all files Collapse all files

191
src/tasks/simulation.rs
View File

@@ -1,160 +1,63 @@
use embassy_sync::channel::DynamicSender;
use embassy_time::{Duration, Timer};
use nalgebra::{Vector2, Vector3};
use csv_core::{ReadFieldResult, Reader};
use log::*;
use crate::events::Measurement;
const ACCELEROMETER_DATA: &str = include_str!("../../test-data/accelerometer.csv");
//const GYRO_DATA: &str = include_str!("../../test-data/gyro.csv");
const GPS_DATA: &str = include_str!("../../test-data/gps.csv");
struct SimDataReader<'a> {
reader: Reader,
buf: &'a str,
read_offset: usize
}
impl<'a> SimDataReader<'a> {
pub fn new(buf: &'a str) -> Self {
Self {
buf,
read_offset: 0,
reader: Reader::new()
}
}
}
impl<'a> Iterator for SimDataReader<'a> {
type Item = &'a str;
fn next(&mut self) -> Option<Self::Item> {
let mut out_buf = [0u8; 128];
match self.reader.read_field(&self.buf.as_bytes()[self.read_offset..(self.read_offset + 128).min(self.buf.len())], &mut out_buf) {
(ReadFieldResult::Field { record_end: _ }, read_size, write_size) => {
let start = self.read_offset;
self.read_offset += read_size;
Some(&self.buf[start..(start + write_size)])
},
(ReadFieldResult::End, _, _) => None,
err => panic!("{:?}", err)
}
}
}
#[derive(Default, Debug, Clone, Copy)]
enum MotionFields {
#[default]
Time,
SecondsElapsed,
Z,
Y,
X,
End
}
impl MotionFields {
pub fn next(self) -> Self {
match self {
Self::Time => Self::SecondsElapsed,
Self::SecondsElapsed => Self::Z,
Self::Z => Self::Y,
Self::Y => Self::X,
Self::X => Self::End,
Self::End => panic!("No more fields")
}
}
}
#[derive(Default, Debug, Clone, Copy)]
enum GpsFields {
#[default]
Time,
SecondsElapsed,
BearingAcc,
SpeedAcc,
VertAcc,
HorizAcc,
Speed,
Bearing,
Altitude,
Longitude,
Latitude,
End
}
impl GpsFields {
pub fn next(self) -> Self {
match self {
Self::Time => Self::SecondsElapsed,
Self::SecondsElapsed => Self::BearingAcc,
Self::BearingAcc => Self::SpeedAcc,
Self::SpeedAcc => Self::VertAcc,
Self::VertAcc => Self::HorizAcc,
Self::HorizAcc => Self::Speed,
Self::Speed => Self::Bearing,
Self::Bearing => Self::Altitude,
Self::Altitude => Self::Longitude,
Self::Longitude => Self::Latitude,
Self::Latitude => Self::End,
Self::End => panic!("No more fields")
}
}
}
use crate::events::{Measurement, SensorSource};
use crate::Breaker;
#[embassy_executor::task]
pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>) {
let mut accel = Vector3::default();
let mut cur_field = MotionFields::default();
let mut last_stamp = 0.0;
let mut cur_delta = Duration::from_ticks(0);
let reader = SimDataReader::new(ACCELEROMETER_DATA);
for field in reader {
match cur_field {
MotionFields::SecondsElapsed => {
let secs = field.parse::<f64>().unwrap();
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
last_stamp = secs;
}
MotionFields::X => accel.x = field.parse::<f32>().unwrap(),
MotionFields::Y => accel.y = field.parse().unwrap(),
MotionFields::Z => accel.z = field.parse().unwrap(),
_ => ()
}
cur_field = cur_field.next();
if let MotionFields::End = cur_field {
cur_field = MotionFields::default();
events.send(Measurement::IMU{ accel, gyro: Vector3::default() }).await;
Timer::after(cur_delta).await;
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/motion.msgpack"));
let mut runtime_secs = Breaker::default();
let mut runtime = Duration::default();
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
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);
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,
);
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,
);
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;
}
events.send(Measurement::SensorOffline(SensorSource::IMU)).await;
warn!("End of motion recording");
}
#[embassy_executor::task]
pub async fn location_simulation_task(events: DynamicSender<'static, Measurement>) {
let mut coords = Vector2::default();
let mut cur_field = GpsFields::default();
let mut last_stamp = 0.0;
let mut cur_delta = Duration::from_ticks(0);
let reader = SimDataReader::new(GPS_DATA);
for field in reader {
match cur_field {
GpsFields::SecondsElapsed => {
let secs = field.parse::<f64>().unwrap();
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
last_stamp = secs;
}
GpsFields::Latitude => coords.x = field.parse::<f64>().unwrap(),
GpsFields::Longitude => coords.y = field.parse().unwrap(),
_ => ()
}
cur_field = cur_field.next();
if let GpsFields::End = cur_field {
cur_field = GpsFields::default();
Timer::after(cur_delta).await;
events.send(Measurement::GPS(Some(coords))).await;
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/gps.msgpack"));
let mut runtime_secs = Breaker::default();
let mut runtime = Duration::default();
events.send(Measurement::SensorOnline(SensorSource::GPS)).await;
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;
}
events.send(Measurement::SensorOffline(SensorSource::GPS)).await;
warn!("End of GPS recording");
}