diff --git a/controller/src/sensor_manager.rs b/controller/src/sensor_manager.rs index 0b82a19..2909ede 100644 --- a/controller/src/sensor_manager.rs +++ b/controller/src/sensor_manager.rs @@ -76,25 +76,33 @@ async fn init_sensors(mut hardware: SensorHardware) { }; loop { + let mut updated = false; POLL_STATE.store(PollState::Tof1 as u8, Ordering::SeqCst); if let Ok(dist) = tof.read_range_single_millimeters_blocking() { info!("dist1: {dist}mm"); data.tof1 = Some(dist); + updated = true; } Timer::after_millis(3).await; POLL_STATE.store(PollState::Tof2 as u8, Ordering::SeqCst); if let Ok(dist) = tof2.read_range_single_millimeters_blocking() { info!("dist2: {dist}mm"); data.tof2 = Some(dist); + updated = true; } Timer::after_millis(3).await; POLL_STATE.store(PollState::Gyro as u8, Ordering::SeqCst); if let Ok(gyro) = gyro.get_gyro() { info!("rotation: {gyro}"); data.gyro = Some(gyro); + updated = true; } if let Ok(accel) = gyro.get_acc() { data.accel = Some(accel); + updated = true; + } + if updated { + data.updated = Instant::now(); } POLL_STATE.store(PollState::None as u8, Ordering::SeqCst);