controlsd: add lagging alert (#24360)

* controlsd: add lagging alert

* move those

* just this for now

* no camera malfunction
old-commit-hash: 3abbe827deac4954a2af97a191171c8b7d08d0c1

common/realtime.py

@@ -2,6 +2,7 @@
 import gc
 import os
 import time
+from collections import deque
 from typing import Optional, List, Union
 
 from setproctitle import getproctitle  # pylint: disable=no-name-in-module
@@ -59,6 +60,8 @@ class Ratekeeper:
     self._frame = 0
     self._remaining = 0.0
     self._process_name = getproctitle()
+    self._dts = deque([self._interval], maxlen=100)
+    self._last_monitor_time = sec_since_boot()
 
   @property
   def frame(self) -> int:
@@ -68,6 +71,12 @@ class Ratekeeper:
   def remaining(self) -> float:
     return self._remaining
 
+  @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
+
   # Maintain loop rate by calling this at the end of each loop
   def keep_time(self) -> bool:
     lagged = self.monitor_time()
@@ -77,6 +86,10 @@ class Ratekeeper:
 
   # this only monitor the cumulative lag, but does not enforce a rate
   def monitor_time(self) -> bool:
+    prev = self._last_monitor_time
+    self._last_monitor_time = sec_since_boot()
+    self._dts.append(self._last_monitor_time - prev)
+
     lagged = False
     remaining = self._next_frame_time - sec_since_boot()
     self._next_frame_time += self._interval