diff --git a/src/tasks/motion.rs b/src/tasks/motion.rs index cef64ce..6224546 100644 --- a/src/tasks/motion.rs +++ b/src/tasks/motion.rs @@ -1,8 +1,11 @@ use embassy_sync::{channel::DynamicReceiver, pubsub::DynPublisher}; use log::*; +use embassy_time::{Duration, WithTimeout}; use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}}; +const TIMEOUT: Duration = Duration::from_millis(3); + #[embassy_executor::task] pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>) { let mut states = BikeStates::default(); @@ -14,11 +17,11 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_ match next_measurement { Measurement::IMU { accel, gyro } => { states.insert_imu(accel, gyro); - states.commit(&prediction_sink).await; + states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit IMU data in time"); }, Measurement::GPS(Some(gps_pos)) => { states.insert_gps(gps_pos); - states.commit(&prediction_sink).await; + states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit GPS data in time"); }, Measurement::GPS(None) => { states.has_gps_fix.set(false); @@ -26,7 +29,7 @@ pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_ // FIXME: This needs harmonized with the automatic data timeout from above, somehow? Measurement::SensorHardwareStatus(source, state) => { warn!("Sensor {source:?} reports {state:?}!"); - prediction_sink.publish(Prediction::SensorStatus(source, state)).await; + prediction_sink.publish(Prediction::SensorStatus(source, state)).with_timeout(TIMEOUT).await.expect("Could not update sensor status in time"); }, Measurement::SimulationProgress(source, duration, pct) => debug!("{source:?} simulation time: {} {} / 255", duration.as_secs(), pct), Measurement::Annotation => ()