update: use Instant to dynamically adjust loop timing and maintain consistent 50ms interval

Software/MainBoard/rust/src/main.rs

@@ -39,7 +39,7 @@ use embassy_net::Stack;
 use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
 use embassy_sync::mutex::{Mutex, MutexGuard};
 use embassy_sync::once_lock::OnceLock;
-use embassy_time::{Duration, Timer, WithTimeout};
+use embassy_time::{Duration, Instant, Timer, WithTimeout};
 use esp_hal::rom::ets_delay_us;
 use esp_hal::system::software_reset;
 use esp_println::{logger, println};
@@ -658,6 +658,7 @@ pub async fn do_secure_pump(
         board.board_hal.pump(plant_id, true).await?;
 
         for step in 0..steps_in_50ms {
+            let step_start = Instant::now();
             let flow_value = board.board_hal.get_tank_sensor()?.get_flow_meter_value();
             flow_collector[step] = flow_value;
             let flow_value_ml = flow_value as f32 * board.board_hal.get_config().tank.ml_per_pulse;
@@ -745,8 +746,21 @@ pub async fn do_secure_pump(
                 }
             }
 
-            //todo calcualte dynamically to offset time for stuff
+            // Keep the loop period as close to 50ms (including sensor read + processing time) as possible.
-            Timer::after_millis(50).await;
+            // Always sleep at least 1ms to avoid a fully busy loop when we overrun.
+            let elapsed = step_start.elapsed();
+            let target = Duration::from_millis(50);
+            let sleep_time = match target.checked_sub(elapsed) {
+                Some(remaining) => {
+                    if remaining < Duration::from_millis(1) {
+                        Duration::from_millis(1)
+                    } else {
+                        remaining
+                    }
+                }
+                None => Duration::from_millis(1),
+            };
+            Timer::after(sleep_time).await;
             pump_time_ms += 50;
         }
     }