Rework timeout

src/dht.rs

@@ -1,7 +1,7 @@
 use embassy_rp::pio::{
     Common, Config, Instance, PioPin, ShiftConfig, ShiftDirection, StateMachine,
 };
-use embassy_time::Timer;
+use embassy_time::{with_timeout, Duration};
 use fixed::traits::ToFixed;
 use fixed_macro::types::U56F8;
 
@@ -45,35 +45,17 @@ impl<'d, PIO: Instance, const SM: usize> DhtPio<'d, PIO, SM> {
     }
 
     pub async fn read_data(&mut self) -> Result<(u16, u16), DhtError> {
-        let mut timeout = 2000;
-        let mut raw: [Option<u32>; 2] = [None; 2];
-
         self.sm.tx().wait_push(1).await;
 
-        while timeout > 0 && raw[1].is_none() {
-            match self.sm.rx().try_pull() {
-                Some(d) => {
-                    if raw[0].is_none() {
-                        raw[0] = Some(d);
-                    } else {
-                        raw[1] = Some(d);
-                    }
-                }
-                None => (),
+        let (data, crc) = match with_timeout(Duration::from_secs(2), self.read_rx()).await {
+            Ok(r) => r,
+            Err(_) => {
+                self.sm.clear_fifos();
+                self.sm.restart();
+
+                return Err(DhtError::Timeout);
             }
-
-            Timer::after_millis(1).await;
-            timeout -= 1;
-        }
-
-        if timeout <= 0 {
-            self.sm.restart();
-            self.sm.clear_fifos();
-            return Err(DhtError::Timeout);
-        }
-
-        let data = raw[0].unwrap();
-        let crc = raw[1].unwrap();
+        };
 
         if Self::compute_crc(data) != crc {
             return Err(DhtError::CrcMismatch(data, crc));
@@ -85,6 +67,13 @@ impl<'d, PIO: Instance, const SM: usize> DhtPio<'d, PIO, SM> {
         ))
     }
 
+    async fn read_rx(&mut self) -> (u32, u32) {
+        (
+            self.sm.rx().wait_pull().await,
+            self.sm.rx().wait_pull().await,
+        )
+    }
+
     fn compute_crc(data: u32) -> u32 {
         let mut crc: u32 = 0;
         crc += data & 0x000000FF;