common: add MovingAverage class for real-time windowed average calculation (#34569)

* add MovingAverage class for real-time windowed average calculation

* move to util.py

common/realtime.py

@@ -2,10 +2,10 @@
 import gc
 import os
 import time
-from collections import deque
 
 from setproctitle import getproctitle
 
+from openpilot.common.util import MovingAverage
 from openpilot.system.hardware import PC
 
 
@@ -48,10 +48,12 @@ class Ratekeeper:
     self._frame = 0
     self._remaining = 0.0
     self._process_name = getproctitle()
-    self._dts = deque([self._interval], maxlen=100)
     self._last_monitor_time = -1.
     self._next_frame_time = -1.
 
+    self.avg_dt = MovingAverage(100)
+    self.avg_dt.add_value(self._interval)
+
   @property
   def frame(self) -> int:
     return self._frame
@@ -62,9 +64,8 @@ class Ratekeeper:
 
   @property
   def lagging(self) -> bool:
-    avg_dt = sum(self._dts) / len(self._dts)
     expected_dt = self._interval * (1 / 0.9)
-    return avg_dt > expected_dt
+    return self.avg_dt.get_average() > expected_dt
 
   # Maintain loop rate by calling this at the end of each loop
   def keep_time(self) -> bool:
@@ -81,7 +82,7 @@ class Ratekeeper:
 
     prev = self._last_monitor_time
     self._last_monitor_time = time.monotonic()
-    self._dts.append(self._last_monitor_time - prev)
+    self.avg_dt.add_value(self._last_monitor_time - prev)
 
     lagged = False
     remaining = self._next_frame_time - time.monotonic()

common/util.py

@@ -0,0 +1,24 @@
+class MovingAverage:
+  def __init__(self, window_size: int):
+    self.window_size: int = window_size
+    self.buffer: list[float] = [0.0] * window_size
+    self.index: int = 0
+    self.count: int = 0
+    self.sum: float = 0.0
+
+  def add_value(self, new_value: float):
+    # Update the sum: subtract the value being replaced and add the new value
+    self.sum -= self.buffer[self.index]
+    self.buffer[self.index] = new_value
+    self.sum += new_value
+
+    # Update the index in a circular manner
+    self.index = (self.index + 1) % self.window_size
+
+    # Track the number of added values (for partial windows)
+    self.count = min(self.count + 1, self.window_size)
+
+  def get_average(self) -> float:
+    if self.count == 0:
+      return float('nan')
+    return self.sum / self.count