removed time drift - now wait exact time (doesn't sleep but wait the precise instant)

src/main.rs

@@ -72,33 +72,44 @@ fn main() {
 struct PacketSequence {
     packets: Vec<MavlinkPayloadFlightTM>,
     index: usize,
-    last_timestamp: u64,
+    delta_from_start: u64,
+    start_instant: Option<Instant>,
 }
 
 impl From<Vec<MavlinkPayloadFlightTM>> for PacketSequence {
     fn from(value: Vec<MavlinkPayloadFlightTM>) -> Self {
-        let last_timestamp = value[0].timestamp;
+        let delta_from_start = value[0].timestamp;
         Self {
             packets: value,
             index: 0,
-            last_timestamp,
+            delta_from_start,
+            start_instant: None,
         }
     }
 }
 
 impl PacketSequence {
     fn wait_next(&mut self) -> Option<&MavlinkPayloadFlightTM> {
-        let last = self.last_timestamp;
-        let to_sleep = self.packets[self.index].timestamp - last;
         // return None if we reached the end of the sequence
         if self.index == self.packets.len() {
             return None;
         }
 
-        // sleep for the difference between the last timestamp and the current one
-        std::thread::sleep(Duration::from_millis(to_sleep / 1000));
+        // get time delta from the start
+        let next_timestamp = self.packets[self.index].timestamp;
+        let time_delta = next_timestamp - self.delta_from_start;
+
+        // get the instant the first packet was sent or set it if it's the first packet
+        if let Some(start_inst) = self.start_instant {
+            // get the instant to wait until
+            let till = start_inst + Duration::from_micros(time_delta);
 
-        self.last_timestamp = self.packets[self.index].timestamp;
+            // sleep for the difference between the last timestamp and the current one
+            std::thread::sleep(till - Instant::now());
+        } else {
+            // set the start instant
+            self.start_instant = Some(Instant::now());
+        }
         self.index += 1;
         self.packets.get(self.index - 1)
     }