diff --git a/Software/CAN_Sensor/src/main.rs b/Software/CAN_Sensor/src/main.rs
index 537f141..dd4ddcc 100644
--- a/Software/CAN_Sensor/src/main.rs
+++ b/Software/CAN_Sensor/src/main.rs
@@ -466,59 +466,73 @@ async fn worker(
     let low_th: u16 = (ADC_MAX as u16) / 3; // ~1/3 Vref
     let high_th: u16 = ((ADC_MAX as u32 * 2) / 3) as u16; // ~2/3 Vref
 
+    const AVG_WINDOWS: u32 = 4;
+    const YIELD_EVERY: u32 = 64;
+    let probe_duration = Duration::from_millis(100);
+
     loop {
-        // Count rising edges of Q in a 100 ms window
-        let start = Instant::now();
-        let mut pulses: u32 = 0;
-        let mut last_q = q_high;
+        let mut total_pulses: u32 = 0;
 
-        probe_gnd.set_as_output(Speed::Low);
-        probe_gnd.set_low();
-        let probe_duration = Duration::from_millis(100);
-        while Instant::now()
-            .checked_duration_since(start)
-            .unwrap_or(Duration::from_millis(0))
-            < probe_duration
-        {
-            // Sample the analog input (Threshold/Trigger on A1)
-            let val: u16 = adc.convert(&mut ain, SampleTime::CYCLES28_5);
+        for _ in 0..AVG_WINDOWS {
+            // Count rising edges of Q in a 100 ms window
+            let start = Instant::now();
+            let mut pulses: u32 = 0;
+            let mut last_q = q_high;
+            let mut iter_count: u32 = 0;
 
-            // 555 core behavior:
-            // - If input <= 1/3 Vref => set Q high (trigger)
-            // - If input >= 2/3 Vref => set Q low (threshold)
-            // - Otherwise keep previous Q state (hysteresis)
-            if val <= low_th {
-                q_high = true;
-            } else if val >= high_th {
-                q_high = false;
+            probe_gnd.set_as_output(Speed::Low);
+            probe_gnd.set_low();
+            while Instant::now()
+                .checked_duration_since(start)
+                .unwrap_or(Duration::from_millis(0))
+                < probe_duration
+            {
+                // Sample the analog input (Threshold/Trigger on A1)
+                let val: u16 = adc.convert(&mut ain, SampleTime::CYCLES28_5);
+
+                // 555 core behavior:
+                // - If input <= 1/3 Vref => set Q high (trigger)
+                // - If input >= 2/3 Vref => set Q low (threshold)
+                // - Otherwise keep previous Q state (hysteresis)
+                if val <= low_th {
+                    q_high = true;
+                } else if val >= high_th {
+                    q_high = false;
+                }
+
+                // Drive output pin accordingly
+                if q_high {
+                    q.set_high();
+                } else {
+                    q.set_low();
+                }
+
+                // Count rising edges
+                if !last_q && q_high {
+                    pulses = pulses.saturating_add(1);
+                }
+                last_q = q_high;
+
+                // Yield every YIELD_EVERY samples to keep USB alive without
+                // disrupting per-sample timing
+                iter_count += 1;
+                if iter_count % YIELD_EVERY == 0 {
+                    yield_now().await;
+                }
             }
-
-            // Drive output pin accordingly
-            if q_high {
-                q.set_high();
-            } else {
-                q.set_low();
-            }
-
-            // Count rising edges
-            if !last_q && q_high {
-                pulses = pulses.saturating_add(1);
-            }
-            last_q = q_high;
-
-            // Yield to allow USB and other tasks to run
-            yield_now().await;
+            probe_gnd.set_as_input(Pull::None);
+            total_pulses = total_pulses.saturating_add(pulses);
         }
-        probe_gnd.set_as_input(Pull::None);
 
-        let freq_hz: u32 = pulses * (1000 / probe_duration.as_millis()) as u32; // pulses per 0.1s => Hz
+        let avg_pulses = total_pulses / AVG_WINDOWS;
+        let freq_hz: u32 = avg_pulses * (1000 / probe_duration.as_millis()) as u32;
 
         let mut msg: heapless::String<128> = heapless::String::new();
         let _ = write!(
             &mut msg,
-            "555 window={}ms pulses={} freq={} Hz (A1->Q on PB0) id={:?}\r\n",
-            probe_duration.as_millis(),
-            pulses,
+            "555 window={}ms avg_pulses={} freq={} Hz (A1->Q on PB0) id={:?}\r\n",
+            probe_duration.as_millis() * AVG_WINDOWS as u64,
+            avg_pulses,
             freq_hz,
             identify_id.as_raw()
         );