From 7fd36a91fd0460c4efed1e072fc34d79e72c5a50 Mon Sep 17 00:00:00 2001
From: Naxdy <naxdy@polskalemon.com>
Date: Wed, 27 Mar 2024 20:11:05 +0100
Subject: [PATCH] feat(input): use loop logic closer to phob

---
 src/input.rs | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/src/input.rs b/src/input.rs
index ee29a14..fd02b8d 100644
--- a/src/input.rs
+++ b/src/input.rs
@@ -13,7 +13,7 @@ use embassy_rp::{
     spi::Spi,
 };
 use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
-use embassy_time::{Instant, Timer};
+use embassy_time::{Duration, Instant, Timer};
 use libm::{fmaxf, fminf};
 
 use crate::{
@@ -139,7 +139,11 @@ async fn update_stick_states<
     let end_time = Instant::now() + embassy_time::Duration::from_millis(1);
     let timer = Timer::at(end_time);
 
-    while Instant::now() < end_time {
+    let mut loop_time = Duration::from_millis(0);
+
+    while Instant::now() < end_time - loop_time {
+        let loop_start = Instant::now();
+
         adc_count += 1;
         ax_sum += read_ext_adc(
             Stick::ControlStick,
@@ -170,6 +174,8 @@ async fn update_stick_states<
             &mut spi_ccs,
         ) as u32;
 
+        loop_time = Instant::now() - loop_start;
+
         // with this, we can poll the sticks at 1000Hz (ish), while updating
         // the rest of the controller (the buttons) much faster, to ensure
         // better input integrity for button inputs.