IQ.Pilot/iqpilot/common/realtime.py

"""Utilities for reading real time clocks and keeping soft real time constraints."""
import gc
import os
import sys
import time

from setproctitle import getproctitle

from iqpilot.common.utils import MovingAverage
from iqpilot.system.hardware import PC


# time step for each process
DT_CTRL = 0.01  # controlsd
DT_MDL = 0.05  # model
DT_HW = 0.5  # hardwared and manager
DT_DMON = 0.05  # driver monitoring


class Priority:
  # CORE 2
  # - modeld = 55
  # - camerad = 54
  CTRL_LOW = 51 # plannerd & radard

  # CORE 3
  # - pandad = 55
  CTRL_HIGH = 53


def set_core_affinity(cores: list[int]) -> None:
  if sys.platform == 'linux' and not PC:
    os.sched_setaffinity(0, cores)


def config_realtime_process(cores: int | list[int], priority: int) -> None:
  gc.disable()
  if sys.platform == 'linux' and not PC:
    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
  c = cores if isinstance(cores, list) else [cores, ]
  set_core_affinity(c)


class CycleCollector:
  # config_realtime_process disables the collector; without this, reference cycles (failed requests, tracebacks) are never freed.
  def __init__(self, interval_s: float = 30.0):
    self._interval_s = interval_s
    gc.collect()
    gc.freeze()
    self._next = time.monotonic() + interval_s

  def tick(self) -> None:
    now = time.monotonic()
    if now >= self._next:
      gc.collect()
      self._next = now + self._interval_s


def config_background_thread() -> None:
  if sys.platform == 'linux' and not PC:
    os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
    set_core_affinity(list(range(os.cpu_count() or 1)))


def _swap_active() -> bool:
  try:
    with open("/proc/swaps") as f:
      return len(f.read().splitlines()) > 1
  except Exception:
    return False


def lock_memory() -> None:
  """mlockall this process so swap can't evict its pages and stall the RT loop.
  RT control procs only (locking ui/modeld would worsen pressure). Best-effort.

  No-op without active swap: with none, anonymous pages (thread stacks, heap)
  are already unreclaimable, so mlockall buys nothing — it only pins every 8 MB
  thread stack and MCL_FUTURE allocation in full (~400 MB/proc here) and, being
  non-migratable, starves CMA. The device runs swapless by design (iqos-zram)."""
  if sys.platform != 'linux' or PC or not _swap_active():
    return
  try:
    import ctypes
    import resource
    resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY))
    MCL_CURRENT, MCL_FUTURE = 0x1, 0x2
    libc = ctypes.CDLL("libc.so.6", use_errno=True)
    if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0:
      raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno()))
  except Exception as e:
    try:
      from iqpilot.common.swaglog import cloudlog
      cloudlog.warning(f"lock_memory (mlockall) failed: {e}")
    except Exception:
      pass


class Ratekeeper:
  def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
    """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
    self._interval = 1. / rate
    self._print_delay_threshold = print_delay_threshold
    self._frame = 0
    self._remaining = 0.0
    self._process_name = getproctitle()
    self._last_monitor_time = -1.
    self._next_frame_time = -1.

    self.avg_dt = MovingAverage(100)
    self.avg_dt.add_value(self._interval)

  def reset(self) -> None:
    self._remaining = 0.0
    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

  @property
  def remaining(self) -> float:
    return self._remaining

  @property
  def lag(self) -> float:
    return max(0., -self._remaining)

  @property
  def lagging(self) -> bool:
    expected_dt = self._interval * (1 / 0.9)
    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:
    lagged = self.monitor_time()
    if self._remaining > 0:
      time.sleep(self._remaining)
    return lagged

  # Monitors the cumulative lag, but does not enforce a rate
  def monitor_time(self) -> bool:
    if self._last_monitor_time < 0:
      self._next_frame_time = time.monotonic() + self._interval
      self._last_monitor_time = time.monotonic()

    prev = self._last_monitor_time
    self._last_monitor_time = time.monotonic()
    self.avg_dt.add_value(self._last_monitor_time - prev)

    lagged = False
    remaining = self._next_frame_time - time.monotonic()
    self._next_frame_time += self._interval
    if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
      print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
      lagged = True
    self._frame += 1
    self._remaining = remaining
    return lagged