IQ.Pilot Prebuilt Release @ 7a91404

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/basedir.py

@@ -0,0 +1,4 @@
+import os
+
+
+BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../.."))

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/constants.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+# conversions
+class CV:
+  # Speed
+  MPH_TO_KPH = 1.609344
+  KPH_TO_MPH = 1. / MPH_TO_KPH
+  MS_TO_KPH = 3.6
+  KPH_TO_MS = 1. / MS_TO_KPH
+  MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
+  MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
+  MS_TO_KNOTS = 1.9438
+  KNOTS_TO_MS = 1. / MS_TO_KNOTS
+
+  # Angle
+  DEG_TO_RAD = np.pi / 180.
+  RAD_TO_DEG = 1. / DEG_TO_RAD
+
+  # Mass
+  LB_TO_KG = 0.453592
+
+
+ACCELERATION_DUE_TO_GRAVITY = 9.81  # m/s^2

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/gpio.py

@@ -0,0 +1,89 @@
+import os
+import fcntl
+import ctypes
+from functools import cache
+
+def gpio_init(pin: int, output: bool) -> None:
+  try:
+    with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
+      f.write(b"out" if output else b"in")
+  except Exception as e:
+    print(f"Failed to set gpio {pin} direction: {e}")
+
+def gpio_set(pin: int, high: bool) -> None:
+  try:
+    with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
+      f.write(b"1" if high else b"0")
+  except Exception as e:
+    print(f"Failed to set gpio {pin} value: {e}")
+
+def gpio_read(pin: int) -> bool | None:
+  val = None
+  try:
+    with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
+      val = bool(int(f.read().strip()))
+  except Exception as e:
+    print(f"Failed to set gpio {pin} value: {e}")
+
+  return val
+
+def gpio_export(pin: int) -> None:
+  if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
+    return
+
+  try:
+    with open("/sys/class/gpio/export", 'w') as f:
+      f.write(str(pin))
+  except Exception:
+    print(f"Failed to export gpio {pin}")
+
+@cache
+def get_irq_action(irq: int) -> list[str]:
+  try:
+    with open(f"/sys/kernel/irq/{irq}/actions") as f:
+      actions = f.read().strip().split(',')
+      return actions
+  except FileNotFoundError:
+    return []
+
+def get_irqs_for_action(action: str) -> list[str]:
+  ret = []
+  with open("/proc/interrupts") as f:
+    for l in f.readlines():
+      irq = l.split(':')[0].strip()
+      if irq.isdigit() and action in get_irq_action(irq):
+        ret.append(irq)
+  return ret
+
+# *** gpiochip ***
+
+class gpioevent_data(ctypes.Structure):
+  _fields_ = [
+    ("timestamp", ctypes.c_uint64),
+    ("id", ctypes.c_uint32),
+  ]
+
+class gpioevent_request(ctypes.Structure):
+  _fields_ = [
+    ("lineoffset", ctypes.c_uint32),
+    ("handleflags", ctypes.c_uint32),
+    ("eventflags", ctypes.c_uint32),
+    ("label", ctypes.c_char * 32),
+    ("fd", ctypes.c_int)
+  ]
+
+def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int:
+  GPIOEVENT_REQUEST_BOTH_EDGES = 0x3
+  GPIOHANDLE_REQUEST_INPUT = 0x1
+  GPIO_GET_LINEEVENT_IOCTL = 0xc030b404
+
+  rq = gpioevent_request()
+  rq.lineoffset = pin
+  rq.handleflags = GPIOHANDLE_REQUEST_INPUT
+  rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES
+  rq.label = label.encode('utf-8')[:31] + b'\0'
+
+  fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY)
+  fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq)
+  os.close(fd)
+  return int(rq.fd)

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/gps.py

@@ -0,0 +1,8 @@
+from iqpilot.common.params import Params
+
+
+def get_gps_location_service(params: Params) -> str:
+  if params.get_bool("UbloxAvailable"):
+    return "gpsLocationExternal"
+  else:
+    return "gpsLocation"

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/i2c.py

@@ -0,0 +1,81 @@
+import os
+import fcntl
+import ctypes
+
+# I2C constants from /usr/include/linux/i2c-dev.h
+I2C_SLAVE = 0x0703
+I2C_SLAVE_FORCE = 0x0706
+I2C_SMBUS = 0x0720
+
+# SMBus transfer types
+I2C_SMBUS_READ = 1
+I2C_SMBUS_WRITE = 0
+I2C_SMBUS_BYTE_DATA = 2
+I2C_SMBUS_I2C_BLOCK_DATA = 8
+
+I2C_SMBUS_BLOCK_MAX = 32
+
+
+class _I2cSmbusData(ctypes.Union):
+  _fields_ = [
+    ("byte", ctypes.c_uint8),
+    ("word", ctypes.c_uint16),
+    ("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)),
+  ]
+
+
+class _I2cSmbusIoctlData(ctypes.Structure):
+  _fields_ = [
+    ("read_write", ctypes.c_uint8),
+    ("command", ctypes.c_uint8),
+    ("size", ctypes.c_uint32),
+    ("data", ctypes.POINTER(_I2cSmbusData)),
+  ]
+
+
+class SMBus:
+  def __init__(self, bus: int):
+    self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR)
+
+  def __enter__(self) -> 'SMBus':
+    return self
+
+  def __exit__(self, *args) -> None:
+    self.close()
+
+  def close(self) -> None:
+    if hasattr(self, '_fd') and self._fd >= 0:
+      os.close(self._fd)
+      self._fd = -1
+
+  def _set_address(self, addr: int, force: bool = False) -> None:
+    ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE
+    fcntl.ioctl(self._fd, ioctl_arg, addr)
+
+  def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None:
+    ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data))
+    fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data)
+
+  def read_byte_data(self, addr: int, register: int, force: bool = False) -> int:
+    self._set_address(addr, force)
+    data = _I2cSmbusData()
+    self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data)
+    return int(data.byte)
+
+  def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None:
+    self._set_address(addr, force)
+    data = _I2cSmbusData()
+    data.byte = value & 0xFF
+    self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data)
+
+  def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]:
+    self._set_address(addr, force)
+    if not (0 <= length <= I2C_SMBUS_BLOCK_MAX):
+      raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}")
+
+    data = _I2cSmbusData()
+    data.block[0] = length
+    self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data)
+    read_len = int(data.block[0]) or length
+    read_len = min(read_len, length)
+    return [int(b) for b in data.block[1 : read_len + 1]]

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/issue_debug.py

@@ -0,0 +1,44 @@
+import os
+import threading
+import time
+from datetime import datetime
+from pathlib import Path
+
+from iqpilot.system.hardware import PC
+from iqpilot.system.hardware.hw import Paths
+
+
+DEBUG_FILENAME = "iqpilot_issue_debug.txt"
+DEBUG_PATH = Path(Paths.comma_home()) / "community" / DEBUG_FILENAME if PC else Path("/data/community") / DEBUG_FILENAME
+
+_lock = threading.Lock()
+_last_log_times: dict[str, float] = {}
+
+
+def log_issue(tag: str, message: str) -> None:
+  try:
+    DEBUG_PATH.parent.mkdir(parents=True, exist_ok=True)
+    with _lock:
+      with open(DEBUG_PATH, "a", encoding="utf-8") as f:
+        timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+        f.write(f"[{timestamp}] [{tag}] {message}\n")
+  except OSError:
+    pass
+
+
+def log_issue_limited(key: str, tag: str, message: str, interval_sec: float = 1.0) -> None:
+  now = time.monotonic()
+  with _lock:
+    last = _last_log_times.get(key, 0.0)
+    if now - last < interval_sec:
+      return
+    _last_log_times[key] = now
+
+  log_issue(tag, message)
+
+
+def clear_issue_debug_log() -> None:
+  try:
+    os.remove(DEBUG_PATH)
+  except OSError:
+    pass

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/logging_extra.py

@@ -0,0 +1,249 @@
+import io
+import os
+import sys
+import copy
+import json
+import time
+import uuid
+import socket
+import logging
+import traceback
+import numpy as np
+from threading import local
+from collections import OrderedDict
+from contextlib import contextmanager
+
+LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ
+
+def json_handler(obj):
+  if isinstance(obj, np.bool_):
+    return bool(obj)
+  # if isinstance(obj, (datetime.date, datetime.time)):
+  #   return obj.isoformat()
+  return repr(obj)
+
+def json_robust_dumps(obj):
+  return json.dumps(obj, default=json_handler)
+
+class NiceOrderedDict(OrderedDict):
+  def __str__(self):
+    return json_robust_dumps(self)
+
+class SwagFormatter(logging.Formatter):
+  def __init__(self, swaglogger):
+    logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
+
+    self.swaglogger = swaglogger
+    self.host = socket.gethostname()
+
+  def format_dict(self, record):
+    record_dict = NiceOrderedDict()
+
+    if isinstance(record.msg, dict):
+      record_dict['msg'] = record.msg
+    else:
+      try:
+        record_dict['msg'] = record.getMessage()
+      except (ValueError, TypeError):
+        record_dict['msg'] = [record.msg]+record.args
+
+    record_dict['ctx'] = self.swaglogger.get_ctx()
+
+    if record.exc_info:
+      record_dict['exc_info'] = self.formatException(record.exc_info)
+
+    record_dict['level'] = record.levelname
+    record_dict['levelnum'] = record.levelno
+    record_dict['name'] = record.name
+    record_dict['filename'] = record.filename
+    record_dict['lineno'] = record.lineno
+    record_dict['pathname'] = record.pathname
+    record_dict['module'] = record.module
+    record_dict['funcName'] = record.funcName
+    record_dict['host'] = self.host
+    record_dict['process'] = record.process
+    record_dict['thread'] = record.thread
+    record_dict['threadName'] = record.threadName
+    record_dict['created'] = record.created
+
+    return record_dict
+
+  def format(self, record):
+    if self.swaglogger is None:
+      raise Exception("must set swaglogger before calling format()")
+    return json_robust_dumps(self.format_dict(record))
+
+class SwagLogFileFormatter(SwagFormatter):
+  def fix_kv(self, k, v):
+    # append type to names to preserve legacy naming in logs
+    # avoids overlapping key namespaces with different types
+    # e.g. log.info() creates 'msg' -> 'msg$s'
+    #      log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i'
+    #      because overlapping namespace 'msg' caused problems
+    if isinstance(v, (str, bytes)):
+      k += "$s"
+    elif isinstance(v, float):
+      k += "$f"
+    elif isinstance(v, bool):
+      k += "$b"
+    elif isinstance(v, int):
+      k += "$i"
+    elif isinstance(v, dict):
+      nv = {}
+      for ik, iv in v.items():
+        ik, iv = self.fix_kv(ik, iv)
+        nv[ik] = iv
+      v = nv
+    elif isinstance(v, list):
+      k += "$a"
+    return k, v
+
+  def format(self, record):
+    if isinstance(record, str):
+      v = json.loads(record)
+    else:
+      v = self.format_dict(record)
+
+    mk, mv = self.fix_kv('msg', v['msg'])
+    del v['msg']
+    v[mk] = mv
+    v['id'] = uuid.uuid4().hex
+
+    return json_robust_dumps(v)
+
+class SwagErrorFilter(logging.Filter):
+  def filter(self, record):
+    return record.levelno < logging.ERROR
+
+def _tmpfunc():
+  return 0
+
+def _srcfile():
+  return os.path.normcase(_tmpfunc.__code__.co_filename)
+
+class SwagLogger(logging.Logger):
+  def __init__(self):
+    logging.Logger.__init__(self, "swaglog")
+
+    self.global_ctx = {}
+
+    self.log_local = local()
+    self.log_local.ctx = {}
+
+  def local_ctx(self):
+    try:
+      return self.log_local.ctx
+    except AttributeError:
+      self.log_local.ctx = {}
+      return self.log_local.ctx
+
+  def get_ctx(self):
+    return dict(self.local_ctx(), **self.global_ctx)
+
+  @contextmanager
+  def ctx(self, **kwargs):
+    old_ctx = self.local_ctx()
+    self.log_local.ctx = copy.copy(old_ctx) or {}
+    self.log_local.ctx.update(kwargs)
+    try:
+      yield
+    finally:
+      self.log_local.ctx = old_ctx
+
+  def bind(self, **kwargs):
+    self.local_ctx().update(kwargs)
+
+  def bind_global(self, **kwargs):
+    self.global_ctx.update(kwargs)
+
+  def event(self, event, *args, **kwargs):
+    evt = NiceOrderedDict()
+    evt['event'] = event
+    if args:
+      evt['args'] = args
+    evt.update(kwargs)
+    if 'error' in kwargs:
+      self.error(evt)
+    elif 'debug' in kwargs:
+      self.debug(evt)
+    else:
+      self.info(evt)
+
+  def timestamp(self, event_name):
+    if LOG_TIMESTAMPS:
+      t = time.monotonic()
+      tstp = NiceOrderedDict()
+      tstp['timestamp'] = NiceOrderedDict()
+      tstp['timestamp']["event"] = event_name
+      tstp['timestamp']["time"] = t*1e9
+      self.debug(tstp)
+
+  def findCaller(self, stack_info=False, stacklevel=1):
+    """
+    Find the stack frame of the caller so that we can note the source
+    file name, line number and function name.
+    """
+    f = sys._getframe(3)
+    #On some versions of IronPython, currentframe() returns None if
+    #IronPython isn't run with -X:Frames.
+    if f is not None:
+      f = f.f_back
+    orig_f = f
+    while f and stacklevel > 1:
+      f = f.f_back
+      stacklevel -= 1
+    if not f:
+      f = orig_f
+    rv = "(unknown file)", 0, "(unknown function)", None
+    while hasattr(f, "f_code"):
+      co = f.f_code
+      filename = os.path.normcase(co.co_filename)
+
+      if filename == _srcfile:
+        f = f.f_back
+        continue
+      sinfo = None
+      if stack_info:
+        sio = io.StringIO()
+        sio.write('Stack (most recent call last):\n')
+        traceback.print_stack(f, file=sio)
+        sinfo = sio.getvalue()
+        if sinfo[-1] == '\n':
+          sinfo = sinfo[:-1]
+        sio.close()
+      rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
+      break
+    return rv
+
+if __name__ == "__main__":
+  log = SwagLogger()
+
+  stdout_handler = logging.StreamHandler(sys.stdout)
+  stdout_handler.setLevel(logging.INFO)
+  stdout_handler.addFilter(SwagErrorFilter())
+  log.addHandler(stdout_handler)
+
+  stderr_handler = logging.StreamHandler(sys.stderr)
+  stderr_handler.setLevel(logging.ERROR)
+  log.addHandler(stderr_handler)
+
+  log.info("asdasd %s", "a")
+  log.info({'wut': 1})
+  log.warning("warning")
+  log.error("error")
+  log.critical("critical")
+  log.event("test", x="y")
+
+  with log.ctx():
+    stdout_handler.setFormatter(SwagFormatter(log))
+    stderr_handler.setFormatter(SwagFormatter(log))
+    log.bind(user="some user")
+    log.info("in req")
+    print("")
+    log.warning("warning")
+    print("")
+    log.error("error")
+    print("")
+    log.critical("critical")
+    print("")
+    log.event("do_req", a=1, b="c")

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/realtime.py

@@ -0,0 +1,134 @@
+"""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)
+
+
+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 lock_memory() -> None:
+  """mlockall this process so memory reclaim/compaction can't stall it. RT control
+  procs only (locking ui/modeld would worsen pressure). Best-effort."""
+  if sys.platform != 'linux' or PC:
+    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

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/slc_utilities.py

@@ -0,0 +1,126 @@
+import math
+import numpy as np
+
+try:
+  import requests
+except ImportError:
+  requests = None
+
+from iqpilot.common.slc_variables import EARTH_RADIUS
+
+
+def calculate_bearing_offset(latitude, longitude, current_bearing, distance):
+  """
+  Calculate new GPS coordinates given a starting point, bearing, and distance.
+  Used for Mapbox API lookahead calculations.
+
+  Args:
+    latitude: Starting latitude in degrees
+    longitude: Starting longitude in degrees
+    current_bearing: Bearing in degrees (0-360)
+    distance: Distance to project in meters
+
+  Returns:
+    Tuple of (new_latitude, new_longitude) in degrees
+  """
+  bearing = math.radians(current_bearing)
+  lat_rad = math.radians(latitude)
+  lon_rad = math.radians(longitude)
+
+  delta = distance / EARTH_RADIUS
+
+  new_lat = math.asin(math.sin(lat_rad) * math.cos(delta) + math.cos(lat_rad) * math.sin(delta) * math.cos(bearing))
+  new_lon = lon_rad + math.atan2(math.sin(bearing) * math.sin(delta) * math.cos(lat_rad), math.cos(delta) - math.sin(lat_rad) * math.sin(new_lat))
+  return math.degrees(new_lat), math.degrees(new_lon)
+
+
+def calculate_distance_to_point(lat1, lon1, lat2, lon2):
+  """
+  Calculate the great circle distance between two GPS points using the Haversine formula.
+
+  Args:
+    lat1, lon1: First point coordinates in degrees
+    lat2, lon2: Second point coordinates in degrees
+
+  Returns:
+    Distance in meters
+  """
+  lat1_rad = math.radians(lat1)
+  lon1_rad = math.radians(lon1)
+  lat2_rad = math.radians(lat2)
+  lon2_rad = math.radians(lon2)
+
+  delta_lat = lat2_rad - lat1_rad
+  delta_lon = lon2_rad - lon1_rad
+
+  a = (math.sin(delta_lat / 2) ** 2) + math.cos(lat1_rad) * math.cos(lat2_rad) * (math.sin(delta_lon / 2) ** 2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+
+  return EARTH_RADIUS * c
+
+
+def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
+  """
+  Calculate the width of a lane based on lane line positions.
+  Used for speed limit filler to determine road width.
+
+  Args:
+    lane_line1: First lane line object with x, y coordinates
+    lane_line2: Second lane line object with x, y coordinates
+    road_edge: Optional road edge object with x, y coordinates
+
+  Returns:
+    Lane width in meters
+  """
+  lane_line1_x = np.asarray(lane_line1.x)
+  lane_line1_y = np.asarray(lane_line1.y)
+
+  lane_line2_x = np.asarray(lane_line2.x)
+  lane_line2_y = np.asarray(lane_line2.y)
+
+  lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y)
+  distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp))
+
+  if road_edge is None:
+    return distance_to_lane
+
+  road_edge_x = np.asarray(road_edge.x)
+  road_edge_y = np.asarray(road_edge.y)
+
+  edge_y_interp = np.interp(lane_line2_x, road_edge_x, road_edge_y)
+  distance_to_edge = np.median(np.abs(lane_line2_y - edge_y_interp))
+
+  return max(distance_to_lane, distance_to_edge)
+
+
+def is_url_pingable(url):
+  """
+  Check if a URL is accessible and responding.
+  Used to verify Mapbox/Overpass API availability before making requests.
+
+  Args:
+    url: URL to ping
+
+  Returns:
+    Boolean indicating if URL is accessible
+  """
+  if not url:
+    return False
+
+  if requests is None:
+    return False
+
+  if not hasattr(is_url_pingable, "session"):
+    is_url_pingable.session = requests.Session()
+    is_url_pingable.session.headers.update({"User-Agent": "iqpilot-ping-test/1.0"})
+
+  try:
+    response = is_url_pingable.session.head(url, timeout=10, allow_redirects=True)
+    if response.status_code in (405, 501):
+      response = is_url_pingable.session.get(url, timeout=10, allow_redirects=True, stream=True)
+
+    is_accessible = response.ok
+    response.close()
+    return is_accessible
+  except Exception:
+    return False

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/slc_variables.py

@@ -0,0 +1,35 @@
+# Earth radius in meters (for GPS calculations)
+EARTH_RADIUS = 6378137
+
+# Mapbox API limits
+FREE_MAPBOX_REQUESTS = 100_000
+
+# Speed limit offset zones for different unit systems
+# Each entry is (min_speed_ms, max_speed_ms, param_name); the param value is a
+# percent offset applied to the resolved limit (e.g. 10 -> +10%), lower bound inclusive
+
+OFFSET_PERCENT_MAX = 50.0
+
+OFFSET_MAP_IMPERIAL = [
+  (0, 8.94, "speed_limit_offset1"),               # 0-20 mph
+  (8.94, 17.88, "speed_limit_offset2"),           # 20-40 mph
+  (17.88, float("inf"), "speed_limit_offset3"),   # 40+ mph
+]
+
+OFFSET_MAP_METRIC = [
+  (0, 8.33, "speed_limit_offset1"),               # 0-30 km/h
+  (8.33, 16.67, "speed_limit_offset2"),           # 30-60 km/h
+  (16.67, float("inf"), "speed_limit_offset3"),   # 60+ km/h
+]
+
+# Speed limit filler constants
+BOUNDING_BOX_RADIUS_DEGREE = 0.1
+MAX_ENTRIES = 1_000_000
+MAX_OVERPASS_DATA_BYTES = 1_073_741_824
+MAX_OVERPASS_REQUESTS = 10_000
+METERS_PER_DEG_LAT = 111_320
+VETTING_INTERVAL_DAYS = 7
+
+# Overpass API URLs
+OVERPASS_API_URL = "https://overpass-api.de/api/interpreter"
+OVERPASS_STATUS_URL = "https://overpass-api.de/api/status"

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/speed_assist_tiers.py

@@ -0,0 +1,20 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Engagement tiers for the speed-assist feature. A tier is persisted as an integer
+under the "IQSpeedAssistMode" param; the ordinal IS the stored value and must remain
+stable (0..3), ordered by how much the tier is allowed to intervene.
+"""
+from enum import IntEnum
+
+STORE_KEY = "IQSpeedAssistMode"
+
+# none -> just display the limit -> highlight overspeed -> move the set speed
+SpeedAssistTier = IntEnum("SpeedAssistTier", "DISABLED ADVISORY ALERTING ACTUATING", start=0)
+
+DEFAULT_TIER = SpeedAssistTier.ADVISORY
+
+
+def actuates_speed(tier) -> bool:
+  """Only the top tier is permitted to drive the cruise set speed."""
+  return int(tier) == SpeedAssistTier.ACTUATING

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/swaglog.py

@@ -0,0 +1,165 @@
+import logging
+import os
+import sys
+import time
+import warnings
+from pathlib import Path
+from logging.handlers import BaseRotatingHandler
+
+import zmq
+
+from iqpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
+from iqpilot.system.hardware.hw import Paths
+
+
+def get_file_handler():
+  Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
+  base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
+  handler = SwaglogRotatingFileHandler(base_filename)
+  return handler
+
+class SwaglogRotatingFileHandler(BaseRotatingHandler):
+  def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
+    super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
+    self.base_filename = base_filename
+    self.interval = interval # seconds
+    self.max_bytes = max_bytes
+    self.backup_count = backup_count
+    self.log_files = self.get_existing_logfiles()
+    log_indexes = [f.split(".")[-1] for f in self.log_files]
+    self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
+    self.last_rollover = None
+    self.doRollover()
+
+  def _open(self):
+    self.last_rollover = time.monotonic()
+    self.last_file_idx += 1
+    next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
+    stream = open(next_filename, self.mode, encoding=self.encoding)
+    self.log_files.insert(0, next_filename)
+    return stream
+
+  def get_existing_logfiles(self):
+    log_files = list()
+    base_dir = os.path.dirname(self.base_filename)
+    for fn in os.listdir(base_dir):
+      fp = os.path.join(base_dir, fn)
+      if fp.startswith(self.base_filename) and os.path.isfile(fp):
+        log_files.append(fp)
+    return sorted(log_files)
+
+  def shouldRollover(self, record):
+    size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
+    time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
+    return size_exceeded or time_exceeded
+
+  def doRollover(self):
+    if self.stream:
+      self.stream.close()
+    self.stream = self._open()
+
+    if self.backup_count > 0:
+      while len(self.log_files) > self.backup_count:
+        to_delete = self.log_files.pop()
+        if os.path.exists(to_delete): # just being safe, should always exist
+          os.remove(to_delete)
+
+class UnixDomainSocketHandler(logging.Handler):
+  def __init__(self, formatter):
+    logging.Handler.__init__(self)
+    self.setFormatter(formatter)
+    self.pid = None
+
+    self.zctx = None
+    self.sock = None
+
+  def __del__(self):
+    self.close()
+
+  def close(self):
+    if self.sock is not None:
+      self.sock.close()
+    if self.zctx is not None:
+      self.zctx.term()
+
+  def connect(self):
+    self.zctx = zmq.Context()
+    self.sock = self.zctx.socket(zmq.PUSH)
+    self.sock.setsockopt(zmq.LINGER, 10)
+    self.sock.connect(Paths.swaglog_ipc())
+    self.pid = os.getpid()
+
+  def emit(self, record):
+    if os.getpid() != self.pid:
+      # TODO suppresses warning about forking proc with zmq socket, fix root cause
+      warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
+      self.connect()
+
+    msg = self.format(record).rstrip('\n')
+    # print("SEND".format(repr(msg)))
+    try:
+      s = chr(record.levelno)+msg
+      self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
+    except zmq.error.Again:
+      # drop :/
+      pass
+
+
+class ForwardingHandler(logging.Handler):
+  def __init__(self, target_logger):
+    super().__init__()
+    self.target_logger = target_logger
+
+  def emit(self, record):
+    self.target_logger.handle(record)
+
+
+def add_file_handler(log):
+  """
+  Function to add the file log handler to swaglog.
+  This can be used to store logs when logmessaged is not running.
+  """
+  handler = get_file_handler()
+  handler.setFormatter(SwagLogFileFormatter(log))
+  log.addHandler(handler)
+
+
+cloudlog = log = SwagLogger()
+log.setLevel(logging.DEBUG)
+
+
+class PrettyConsoleFormatter(logging.Formatter):
+  # StreamHandler writes to stderr, so tty-gate on that
+  _COLOR = sys.stderr.isatty() and os.environ.get('NO_COLOR') is None
+
+  def format(self, record):
+    msg = record.getMessage()
+    if not self._COLOR:
+      return f"{record.filename}: {msg}"
+    lvl = record.levelno
+    if   lvl >= 50: lc, ln = "\033[1;38;5;196m", "CRIT"
+    elif lvl >= 40: lc, ln = "\033[1;38;5;203m", " ERR"
+    elif lvl >= 30: lc, ln = "\033[38;5;214m", "WARN"
+    elif lvl >= 20: lc, ln = "\033[38;5;110m", "info"
+    else:           lc, ln = "\033[38;5;244m", " dbg"
+    body = f"\033[1;38;5;210m{msg}\033[0m" if lvl >= 40 else msg
+    src = "" if record.filename == "(unknown file)" else f"\033[2m{record.filename}\033[0m  "
+    return f"{lc}{ln:>4}\033[0m  {src}{body}"
+
+
+outhandler = logging.StreamHandler()
+outhandler.setFormatter(PrettyConsoleFormatter())
+
+print_level = os.environ.get('LOGPRINT', 'warning')
+if print_level == 'debug':
+  outhandler.setLevel(logging.DEBUG)
+elif print_level == 'info':
+  outhandler.setLevel(logging.INFO)
+elif print_level == 'warning':
+  outhandler.setLevel(logging.WARNING)
+
+ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
+
+log.addHandler(outhandler)
+# logs are sent through IPC before writing to disk to prevent disk I/O blocking
+log.addHandler(ipchandler)

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/time_helpers.py

@@ -0,0 +1,15 @@
+import datetime
+from pathlib import Path
+
+MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
+
+def min_date():
+  # on systemd systems, the default time is the systemd build time
+  systemd_path = Path("/lib/systemd/systemd")
+  if systemd_path.exists():
+    d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime)
+    return max(MIN_DATE, d + datetime.timedelta(days=1))
+  return MIN_DATE
+
+def system_time_valid():
+  return datetime.datetime.now() > min_date()

artifacts/iqpilot_navd_private/python/_iqclosure/iqpilot/common/utils.py

@@ -0,0 +1,271 @@
+import io
+import os
+import tempfile
+import contextlib
+import subprocess
+import time
+import functools
+from subprocess import Popen, PIPE, TimeoutExpired
+import zstandard as zstd
+
+LOG_COMPRESSION_LEVEL = 10  # little benefit up to level 15. level ~17 is a small step change
+
+class Timer:
+  """Simple lap timer for profiling sequential operations."""
+
+  def __init__(self):
+    self._start = self._lap = time.monotonic()
+    self._sections = {}
+
+  def lap(self, name):
+    now = time.monotonic()
+    self._sections[name] = now - self._lap
+    self._lap = now
+
+  @property
+  def total(self):
+    return time.monotonic() - self._start
+
+  def fmt(self, duration):
+    parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items())
+    total = self.total
+    realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A"
+    return f"{duration}s in {total:.1f}s ({realtime}) | {parts}"
+
+def sudo_write(val: str, path: str) -> None:
+  try:
+    with open(path, 'w') as f:
+      f.write(str(val))
+  except PermissionError:
+    os.system(f"sudo chmod a+w {path}")
+    try:
+      with open(path, 'w') as f:
+        f.write(str(val))
+    except PermissionError:
+      # fallback for debugfs files
+      os.system(f"sudo su -c 'echo {val} > {path}'")
+
+
+def sudo_read(path: str) -> str:
+  try:
+    return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
+  except Exception:
+    return ""
+
+
+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
+
+
+class CallbackReader:
+  """Wraps a file, but overrides the read method to also
+  call a callback function with the number of bytes read so far."""
+
+  def __init__(self, f, callback, *args):
+    self.f = f
+    self.callback = callback
+    self.cb_args = args
+    self.total_read = 0
+
+  def __getattr__(self, attr):
+    return getattr(self.f, attr)
+
+  def read(self, *args, **kwargs):
+    chunk = self.f.read(*args, **kwargs)
+    self.total_read += len(chunk)
+    self.callback(*self.cb_args, self.total_read)
+    return chunk
+
+
+@contextlib.contextmanager
+def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None,
+                 overwrite: bool = False):
+  """Write to a file atomically using a temporary file in the same directory as the destination file."""
+  dir_name = os.path.dirname(path)
+
+  if not overwrite and os.path.exists(path):
+    raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.")
+
+  with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file:
+    yield tmp_file
+    tmp_file_name = tmp_file.name
+  os.replace(tmp_file_name, path)
+
+
+def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
+  if not should_compress:
+    file_size = os.path.getsize(filepath)
+    file_stream = open(filepath, "rb")
+    return file_stream, file_size
+
+  # Compress the file on the fly
+  compressed_stream = io.BytesIO()
+  compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
+
+  with open(filepath, "rb") as f:
+    compressor.copy_stream(f, compressed_stream)
+    compressed_size = compressed_stream.tell()
+    compressed_stream.seek(0)
+    return compressed_stream, compressed_size
+
+
+# remove all keys that end in DEPRECATED
+def strip_deprecated_keys(d):
+  for k in list(d.keys()):
+    if isinstance(k, str):
+      if k.endswith('DEPRECATED'):
+        d.pop(k)
+      elif isinstance(d[k], dict):
+        strip_deprecated_keys(d[k])
+  return d
+
+
+def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
+  return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
+
+
+def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
+  try:
+    return run_cmd(cmd, cwd=cwd, env=env)
+  except subprocess.CalledProcessError:
+    return default
+
+
+@contextlib.contextmanager
+def managed_proc(cmd: list[str], env: dict[str, str]):
+  proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
+  try:
+    yield proc
+  finally:
+    if proc.poll() is None:
+      proc.terminate()
+    try:
+      proc.wait(timeout=5)
+    except TimeoutExpired:
+      proc.kill()
+
+
+def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
+  rows = [list(row) for row in tabular_data]
+
+  def fmt(val):
+    if isinstance(val, str):
+      return val
+    if isinstance(val, (bool, int)):
+      return str(val)
+    try:
+      return format(val, floatfmt)
+    except (TypeError, ValueError):
+      return str(val)
+
+  formatted = [[fmt(c) for c in row] for row in rows]
+  hdrs = [str(h) for h in headers] if headers else None
+
+  ncols = max((len(r) for r in formatted), default=0)
+  if hdrs:
+    ncols = max(ncols, len(hdrs))
+  if ncols == 0:
+    return ""
+
+  for r in formatted:
+    r.extend([""] * (ncols - len(r)))
+  if hdrs:
+    hdrs.extend([""] * (ncols - len(hdrs)))
+
+  widths = [0] * ncols
+  if hdrs:
+    for i in range(ncols):
+      widths[i] = len(hdrs[i])
+  for row in formatted:
+    for i in range(ncols):
+      widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n')))
+
+  def _align(s, w):
+    if stralign == "center":
+      return s.center(w)
+    return s.ljust(w)
+
+  if tablefmt == "html":
+    parts = ["<table>"]
+    if hdrs:
+      parts.append("<thead>")
+      parts.append("<tr>" + "".join(f"<th>{h}</th>" for h in hdrs) + "</tr>")
+      parts.append("</thead>")
+    parts.append("<tbody>")
+    for row in formatted:
+      parts.append("<tr>" + "".join(f"<td>{c}</td>" for c in row) + "</tr>")
+    parts.append("</tbody>")
+    parts.append("</table>")
+    return "\n".join(parts)
+
+  if tablefmt == "simple_grid":
+    def _sep(left, mid, right):
+      return left + mid.join("─" * (w + 2) for w in widths) + right
+
+    top, mid_sep, bot = _sep("┌", "┬", "┐"), _sep("├", "┼", "┤"), _sep("└", "┴", "┘")
+
+    def _fmt_row(cells):
+      split = [c.split('\n') for c in cells]
+      nlines = max(len(s) for s in split)
+      for s in split:
+        s.extend([""] * (nlines - len(s)))
+      return ["│" + "│".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "│" for li in range(nlines)]
+
+    lines = [top]
+    if hdrs:
+      lines.extend(_fmt_row(hdrs))
+      lines.append(mid_sep)
+    for ri, row in enumerate(formatted):
+      lines.extend(_fmt_row(row))
+      lines.append(mid_sep if ri < len(formatted) - 1 else bot)
+    return "\n".join(lines)
+
+  gap = "  "
+  lines = []
+  if hdrs:
+    lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True)))
+    lines.append(gap.join("-" * w for w in widths))
+  for row in formatted:
+    lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols)))
+  return "\n".join(lines)
+
+
+def retry(attempts=3, delay=1.0, ignore_failure=False):
+  def decorator(func):
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+      for _ in range(attempts):
+        try:
+          return func(*args, **kwargs)
+        except Exception:
+          print(f"{func.__name__} failed, trying again")
+          time.sleep(delay)
+
+      if ignore_failure:
+        print(f"{func.__name__} failed after retry")
+      else:
+        raise Exception(f"{func.__name__} failed after retry")
+    return wrapper
+  return decorator