IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/common/.gitignore

@@ -0,0 +1 @@
+*.cpp

iqpilot/common/api/__init__.py

@@ -0,0 +1,26 @@
+import iqpilot.common.api.comma_connect
+
+
+class Api:
+  def __init__(self, dongle_id):
+    self.service = iqpilot.common.api.comma_connect.CommaConnectApi(dongle_id)
+
+  def request(self, method, endpoint, **params):
+    return self.service.request(method, endpoint, **params)
+
+  def get(self, *args, **kwargs):
+    return self.service.get(*args, **kwargs)
+
+  def post(self, *args, **kwargs):
+    return self.service.post(*args, **kwargs)
+
+  def get_token(self, payload_extra=None, expiry_hours=1):
+    return self.service.get_token(payload_extra, expiry_hours)
+
+
+def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params):
+  return iqpilot.common.api.comma_connect.CommaConnectApi(None).api_get(endpoint, method, timeout, access_token, session, **params)
+
+
+def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
+  return iqpilot.common.api.comma_connect.CommaConnectApi(None).get_key_pair()

iqpilot/common/api/base.py

@@ -0,0 +1,84 @@
+import jwt
+import os
+import requests
+import unicodedata
+from datetime import datetime, timedelta, UTC
+from functools import lru_cache
+from iqpilot.system.hardware.hw import Paths
+from iqpilot.system.version import get_version
+
+# name: jwt signature algorithm
+KEYS = {"id_rsa": "RS256",
+        "id_ecdsa": "ES256"}
+
+
+@lru_cache(maxsize=4)
+def load_signing_key(private_key: str):
+  # PyJWT re-parses a PEM string on every encode; an RSA parse is ~40ms, so cache the key object
+  try:
+    from cryptography.hazmat.primitives.serialization import load_pem_private_key
+    return load_pem_private_key(private_key.encode(), password=None)
+  except Exception:
+    return private_key
+
+
+class BaseApi:
+  def __init__(self, dongle_id, api_host, user_agent="openpilot-"):
+    self.dongle_id = dongle_id
+    self.api_host = api_host
+    self.user_agent = user_agent
+    self.jwt_algorithm, self.private_key, _ = self.get_key_pair()
+
+  def get(self, *args, **kwargs):
+    return self.request('GET', *args, **kwargs)
+
+  def post(self, *args, **kwargs):
+    return self.request('POST', *args, **kwargs)
+
+  def request(self, method, endpoint, timeout=None, access_token=None, **params):
+    return self.api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
+
+  def _get_token(self, payload_extra=None, expiry_hours=1, **extra_payload):
+    now = datetime.now(UTC).replace(tzinfo=None)
+    payload = {
+      'identity': self.dongle_id,
+      'nbf': now,
+      'iat': now,
+      'exp': now + timedelta(hours=expiry_hours),
+      **extra_payload
+    }
+    if payload_extra is not None:
+      payload.update(payload_extra)
+    key = load_signing_key(self.private_key) if self.private_key else self.private_key
+    token = jwt.encode(payload, key, algorithm=self.jwt_algorithm)
+    if isinstance(token, bytes):
+      token = token.decode('utf8')
+    return token
+
+  def get_token(self, payload_extra=None, expiry_hours=1):
+    return self._get_token(payload_extra, expiry_hours)
+
+  def remove_non_ascii_chars(self, text):
+    normalized_text = unicodedata.normalize('NFD', text)
+    ascii_encoded_text = normalized_text.encode('ascii', 'ignore')
+    return ascii_encoded_text.decode()
+
+  def api_get(self, endpoint, method='GET', timeout=None, access_token=None, session=None, json=None, **params):
+    headers = {}
+    if access_token is not None:
+      headers['Authorization'] = "JWT " + access_token
+
+    version = self.remove_non_ascii_chars(get_version())
+    headers['User-Agent'] = self.user_agent + version
+
+    # TODO: add session to Api
+    req = requests if session is None else session
+    return req.request(method, f"{self.api_host}/{endpoint}", timeout=timeout, headers=headers, json=json, params=params)
+
+  @staticmethod
+  def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
+    for key in KEYS:
+      if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'):
+        with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public:
+          return KEYS[key], private.read(), public.read()
+    return None, None, None

iqpilot/common/api/comma_connect.py

@@ -0,0 +1,11 @@
+import os
+
+from iqpilot.common.api.base import BaseApi
+
+API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
+
+
+class CommaConnectApi(BaseApi):
+  def __init__(self, dongle_id):
+    super().__init__(dongle_id, API_HOST)
+    self.user_agent = "openpilot-"

iqpilot/common/atlas_alerts.py

@@ -0,0 +1,281 @@
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from bisect import insort
+from collections.abc import Callable, Iterable
+from dataclasses import dataclass, field
+from enum import IntEnum
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car, log
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.system.hardware import HARDWARE
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+
+def _frames_for(seconds: float) -> int:
+  return int(seconds / DT_CTRL)
+
+
+class Tier(IntEnum):
+  LOWEST = 0
+  LOWER = 1
+  LOW = 2
+  MID = 3
+  HIGH = 4
+  HIGHEST = 5
+
+
+class Tags:
+  ENABLE = "enable"
+  PRE_ENABLE = "preEnable"
+  OVERRIDE_LATERAL = "overrideLateral"
+  OVERRIDE_LONGITUDINAL = "overrideLongitudinal"
+  NO_ENTRY = "noEntry"
+  WARNING = "warning"
+  USER_DISABLE = "userDisable"
+  SOFT_DISABLE = "softDisable"
+  IMMEDIATE_DISABLE = "immediateDisable"
+  PERMANENT = "permanent"
+
+
+@dataclass(slots=True)
+class AlertCard:
+  alert_text_1: str
+  alert_text_2: str
+  alert_status: log.SelfdriveState.AlertStatus
+  alert_size: log.SelfdriveState.AlertSize
+  priority: Tier
+  visual_alert: car.CarControl.HUDControl.VisualAlert
+  audible_alert: car.CarControl.HUDControl.AudibleAlert
+  duration: int
+  creation_delay: float = 0.0
+  alert_type: str = field(default="", init=False)
+  event_type: str | None = field(default=None, init=False)
+
+  def __init__(self,
+               alert_text_1: str,
+               alert_text_2: str,
+               alert_status: log.SelfdriveState.AlertStatus,
+               alert_size: log.SelfdriveState.AlertSize,
+               priority: Tier,
+               visual_alert: car.CarControl.HUDControl.VisualAlert,
+               audible_alert: car.CarControl.HUDControl.AudibleAlert,
+               duration: float,
+               creation_delay: float = 0.0):
+    self.alert_text_1 = alert_text_1
+    self.alert_text_2 = alert_text_2
+    self.alert_status = alert_status
+    self.alert_size = alert_size
+    self.priority = priority
+    self.visual_alert = visual_alert
+    self.audible_alert = audible_alert
+    self.duration = _frames_for(duration)
+    self.creation_delay = creation_delay
+    self.alert_type = ""
+    self.event_type = None
+
+  def __str__(self) -> str:
+    return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
+
+
+AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard]
+
+
+def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard:
+  del CS, sm, metric, soft_disable_time, personality
+  headline = "Enable Adaptive Cruise to Engage"
+  if CP.brand == "honda":
+    headline = "Enable Main Switch to Engage"
+  return NoEntryCard(headline)
+
+
+class EventBook(ABC):
+  def __init__(self):
+    self._live_names: list[int] = []
+    self._latched_names: list[int] = []
+    self.event_counters: dict[int, int] = {}
+
+  @property
+  def events(self) -> list[int]:
+    return self._live_names
+
+  @events.setter
+  def events(self, values: list[int]) -> None:
+    self._live_names = values
+
+  @property
+  def static_events(self) -> list[int]:
+    return self._latched_names
+
+  @static_events.setter
+  def static_events(self, values: list[int]) -> None:
+    self._latched_names = values
+
+  @property
+  def names(self) -> list[int]:
+    return list(self._live_names)
+
+  def __len__(self) -> int:
+    return len(self._live_names)
+
+  def add(self, event_name: int, static: bool = False) -> None:
+    if static:
+      insort(self._latched_names, event_name)
+    insort(self._live_names, event_name)
+
+  def clear(self) -> None:
+    refreshed: dict[int, int] = {}
+    for event_name, frames_seen in self.event_counters.items():
+      refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0
+    self.event_counters = refreshed
+    self._live_names = list(self._latched_names)
+
+  def contains(self, event_type: str) -> bool:
+    board = self.get_events_mapping()
+    return any(event_type in board.get(event_name, {}) for event_name in self._live_names)
+
+  def has(self, event_name: int) -> bool:
+    return event_name in self._live_names
+
+  def contains_in_list(self, events_list: list[int]) -> bool:
+    return any(event_name in self._live_names for event_name in events_list)
+
+  def remove(self, event_name: int, static: bool = False) -> None:
+    if static and event_name in self._latched_names:
+      self._latched_names.remove(event_name)
+
+    if event_name in self._live_names:
+      self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1
+      self._live_names.remove(event_name)
+
+  def add_from_msg(self, events: Iterable) -> None:
+    for event in events:
+      insort(self._live_names, event.name.raw)
+
+  def to_msg(self):
+    board = self.get_events_mapping()
+    outbound = []
+    for event_name in self._live_names:
+      msg = self.get_event_msg_type().new_message()
+      msg.name = event_name
+      for event_kind in board.get(event_name, {}):
+        setattr(msg, event_kind, True)
+      outbound.append(msg)
+    return outbound
+
+  def create_alerts(self, event_types: list[str], callback_args=None):
+    callback_args = [] if callback_args is None else callback_args
+    board = self.get_events_mapping()
+    spawned: list[AlertCard] = []
+    for event_name in self._live_names:
+      variants = board.get(event_name, {})
+      for event_type in event_types:
+        chosen = variants.get(event_type)
+        if chosen is None:
+          continue
+        alert = self._realize(chosen, callback_args)
+        age_frames = self.event_counters.get(event_name, 0) + 1
+        if age_frames * DT_CTRL < alert.creation_delay:
+          continue
+        alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}"
+        alert.event_type = event_type
+        spawned.append(alert)
+    return spawned
+
+  @staticmethod
+  def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard:
+    return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args)
+
+  @abstractmethod
+  def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_name(self, event: int) -> str:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_msg_type(self):
+    raise NotImplementedError
+
+
+def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]:
+  if HARDWARE.get_device_type() == "mici":
+    return secondary, primary, AlertSize.small
+  return primary, secondary, AlertSize.mid
+
+
+class NoEntryCard(AlertCard):
+  def __init__(self,
+               alert_text_2: str,
+               alert_text_1: str = "IQ.Pilot Unavailable",
+               visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
+               priority: Tier = Tier.LOW):
+    primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
+    super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
+
+
+class GentleDisableCard(AlertCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(
+      "TAKE CONTROL IMMEDIATELY",
+      alert_text_2,
+      AlertStatus.userPrompt,
+      AlertSize.full,
+      Tier.MID,
+      VisualAlert.steerRequired,
+      AudibleAlert.warningSoft,
+      2.0,
+    )
+
+
+class PendingDisableCard(GentleDisableCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(alert_text_2)
+    self.alert_text_1 = "IQ.Pilot will disengage"
+
+
+class HardDisableCard(AlertCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(
+      "TAKE CONTROL IMMEDIATELY",
+      alert_text_2,
+      AlertStatus.critical,
+      AlertSize.full,
+      Tier.HIGHEST,
+      VisualAlert.steerRequired,
+      AudibleAlert.warningImmediate,
+      4.0,
+    )
+
+
+class ChimeCard(AlertCard):
+  def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
+    super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2)
+
+
+class BannerCard(AlertCard):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0):
+    size = AlertSize.mid if alert_text_2 else AlertSize.small
+    super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay)
+
+
+class BootCard(AlertCard):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
+    if HARDWARE.get_device_type() == "mici":
+      compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2
+      super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
+    else:
+      super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
+
+
+class AlertBase(AlertCard):
+  pass
+
+
+NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0)

iqpilot/common/auto_units.py

@@ -0,0 +1,58 @@
+import time
+
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.geo_regions import UNKNOWN_REGION, region_for_position, region_is_metric
+
+CHECK_INTERVAL = 10.0
+CONFIRMATIONS = 3
+
+
+class AutoUnits:
+  def __init__(self, params: Params | None = None):
+    self.params = params or Params()
+    self._next_check = 0.0
+    self._candidate = UNKNOWN_REGION
+    self._confirmations = 0
+
+  def _position(self) -> tuple[float, float, bool]:
+    from iqpilot.selfdrive.ui.lib.nav_helpers import current_or_last_gps_position
+
+    lat, lon, _, valid = current_or_last_gps_position(self.params)
+    return lat, lon, valid
+
+  def update(self, now: float | None = None) -> None:
+    if not self.params.get_bool("IQAutoUnits"):
+      self._candidate = UNKNOWN_REGION
+      self._confirmations = 0
+      return
+
+    now = time.monotonic() if now is None else now
+    if now < self._next_check:
+      return
+    self._next_check = now + CHECK_INTERVAL
+
+    lat, lon, valid = self._position()
+    region = region_for_position(lat, lon) if valid else UNKNOWN_REGION
+    if region == UNKNOWN_REGION:
+      self._confirmations = 0
+      return
+
+    if region != self._candidate:
+      self._candidate = region
+      self._confirmations = 1
+      return
+
+    self._confirmations += 1
+    if self._confirmations < CONFIRMATIONS:
+      return
+
+    if region == self.params.get("IQAutoUnitsRegion"):
+      return
+
+    self.params.put("IQAutoUnitsRegion", region)
+
+    metric = region_is_metric(region)
+    if metric != self.params.get_bool("IsMetric"):
+      self.params.put_bool("IsMetric", metric)
+      cloudlog.warning(f"auto units: {region} detected, switching to {'km/h' if metric else 'mph'}")

iqpilot/common/basedir.py

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

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

iqpilot/common/file_chunker.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+import sys
+import math
+import os
+from pathlib import Path
+
+CHUNK_SIZE = 45 * 1024 * 1024  # 45MB, under GitHub's 50MB limit
+
+def get_chunk_name(name, idx, num_chunks):
+  return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
+
+def get_manifest_path(name):
+  return f"{name}.chunkmanifest"
+
+def _chunk_paths(path, num_chunks):
+  return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
+
+def get_chunk_targets(path, file_size):
+  num_chunks = math.ceil(file_size / CHUNK_SIZE)
+  return _chunk_paths(path, num_chunks)
+
+def chunk_file(path, targets):
+  manifest_path, *chunk_paths = targets
+  with open(path, 'rb') as f:
+    data = f.read()
+  actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE))
+  assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
+  for i, chunk_path in enumerate(chunk_paths):
+    with open(chunk_path, 'wb') as f:
+      f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE])
+  Path(manifest_path).write_text(str(len(chunk_paths)))
+  os.remove(path)
+
+def get_existing_chunks(path):
+  if os.path.isfile(path):
+    return [path]
+  if os.path.isfile(manifest := get_manifest_path(path)):
+    num_chunks = int(Path(manifest).read_text().strip())
+    return _chunk_paths(path, num_chunks)
+  raise FileNotFoundError(path)
+
+def read_file_chunked(path):
+  manifest_path = get_manifest_path(path)
+  if os.path.isfile(manifest_path):
+    num_chunks = int(Path(manifest_path).read_text().strip())
+    return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks))
+  if os.path.isfile(path):
+    return Path(path).read_bytes()
+  raise FileNotFoundError(path)
+
+
+if __name__ == "__main__":
+  path = sys.argv[1]
+  chunk_paths = get_chunk_targets(path, os.path.getsize(path))
+  chunk_file(path, chunk_paths)

iqpilot/common/file_helpers.py

@@ -0,0 +1 @@
+from iqpilot.common.utils import CallbackReader, get_upload_stream

iqpilot/common/filter_simple.py

@@ -0,0 +1,71 @@
+from collections import deque
+
+import numpy as np
+
+
+class FirstOrderFilter:
+  def __init__(self, x0, rc, dt, initialized=True):
+    self.x = x0
+    self.dt = dt
+    self.update_alpha(rc)
+    self.initialized = initialized
+
+  def update_alpha(self, rc):
+    self.alpha = self.dt / (rc + self.dt)
+
+  def update(self, x):
+    if self.initialized:
+      self.x = (1. - self.alpha) * self.x + self.alpha * x
+    else:
+      self.initialized = True
+      self.x = x
+    return self.x
+
+
+class BounceFilter(FirstOrderFilter):
+  def __init__(self, x0, rc, dt, initialized=True, bounce=2):
+    self.velocity = FirstOrderFilter(0.0, 0.15, dt)
+    self.bounce = bounce
+    super().__init__(x0, rc, dt, initialized)
+
+  def update(self, x):
+    super().update(x)
+    scale = self.dt / (1.0 / 60.0)  # tuned at 60 fps
+    self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
+    self.velocity.update(0.0)
+    if abs(self.velocity.x) < 1e-5:
+      self.velocity.x = 0.0
+    self.x += self.velocity.x
+    return self.x
+
+
+class MyMovingAverage:
+  def __init__(self, window_size, value=None):
+    self.window_size = window_size
+    if value is not None:
+      self.values = deque([value] * window_size, maxlen=window_size)
+      self.sum = value * window_size
+      self.result = value
+    else:
+      self.values = deque(maxlen=window_size)
+      self.sum = 0
+      self.result = 0
+
+  def set(self, value):
+    self.values.clear()
+    self.values.append(value)
+    self.sum = value
+    self.result = value
+    return value
+
+  def set_all(self, value):
+    self.values = deque([value] * self.window_size, maxlen=self.window_size)
+    self.sum = value * self.window_size
+    self.result = value
+    return value
+
+  def process(self, value, median=False):
+    self.values.append(value)
+    self.sum = sum(self.values)
+    self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values)
+    return self.result

iqpilot/common/geo_regions.py

@@ -0,0 +1,140 @@
+MPH_REGIONS = ("US", "GB", "LR")
+METRIC_REGION = "METRIC"
+UNKNOWN_REGION = ""
+
+_US_CONUS = [
+  (-123.32, 49.00), (-117.03, 49.00), (-110.00, 49.00), (-104.05, 49.00), (-97.23, 49.00), (-95.15, 49.00),
+  (-95.15, 49.38), (-94.82, 49.30), (-94.68, 48.77), (-93.85, 48.63), (-93.35, 48.62), (-92.72, 48.54),
+  (-92.30, 48.24), (-91.55, 48.10), (-90.84, 48.24), (-89.99, 48.02), (-89.60, 48.02), (-89.10, 48.32),
+  (-88.40, 48.30), (-87.00, 47.80), (-85.60, 47.15), (-84.60, 46.75), (-84.42, 46.56), (-84.30, 46.49),
+  (-84.12, 46.28), (-83.90, 46.05), (-83.40, 45.75), (-82.90, 45.05), (-82.55, 44.00), (-82.42, 43.00),
+  (-82.70, 42.47), (-82.93, 42.34), (-83.00, 42.33), (-83.05, 42.32), (-83.075, 42.312), (-83.13, 42.25),
+  (-83.15, 42.18), (-83.11, 42.10), (-83.09, 42.02),
+  (-82.50, 41.70), (-81.50, 42.00), (-80.20, 42.40), (-79.06, 42.85), (-79.05, 43.27), (-78.00, 43.45),
+  (-77.00, 43.65), (-76.40, 44.10), (-75.80, 44.50), (-74.75, 45.00), (-73.35, 45.01), (-71.50, 45.01),
+  (-71.29, 45.30), (-70.90, 45.30), (-70.72, 45.42), (-70.31, 45.86), (-70.05, 46.44), (-69.99, 46.70),
+  (-69.24, 47.46), (-68.90, 47.20), (-68.38, 47.29), (-67.79, 47.07), (-67.78, 45.94), (-67.42, 45.60),
+  (-67.03, 44.80), (-68.00, 44.30), (-69.06, 43.80), (-70.20, 43.60), (-70.80, 42.85), (-70.00, 41.90),
+  (-70.00, 41.55), (-71.20, 41.30), (-72.00, 41.05), (-73.90, 40.55), (-74.20, 39.60), (-75.05, 38.45),
+  (-75.90, 37.05), (-75.50, 35.20), (-78.50, 33.85), (-80.90, 32.00), (-81.40, 30.70), (-80.03, 26.80),
+  (-80.15, 25.15), (-81.20, 24.55), (-82.00, 26.40), (-82.80, 27.80), (-83.00, 29.15), (-84.30, 29.90),
+  (-85.30, 29.65), (-87.50, 30.25), (-89.00, 29.15), (-89.40, 28.95), (-91.30, 29.10), (-93.80, 29.65),
+  (-95.00, 29.10), (-97.10, 27.80), (-97.14, 25.96), (-98.30, 26.05), (-99.10, 26.40), (-99.50, 27.60),
+  (-100.40, 28.50), (-101.40, 29.77), (-102.30, 29.88), (-102.90, 29.30), (-103.30, 29.00), (-104.37, 29.56),
+  (-104.68, 30.13), (-105.30, 30.80), (-105.85, 31.30), (-106.15, 31.50), (-106.30, 31.68), (-106.45, 31.755),
+  (-106.53, 31.786), (-108.21, 31.783), (-108.21, 31.33), (-111.07, 31.33), (-114.72, 32.72),
+  (-117.13, 32.53), (-118.40, 33.75), (-119.80, 34.40), (-120.65, 35.10), (-121.90, 36.60), (-122.52, 37.78),
+  (-123.75, 39.40), (-124.20, 40.45), (-124.15, 42.00), (-124.05, 43.35), (-123.95, 46.25), (-124.75, 48.40),
+  (-123.30, 48.25), (-123.15, 48.70),
+]
+
+_US_ALASKA = [
+  (-141.00, 70.20), (-141.00, 60.30), (-139.05, 60.35), (-137.45, 58.95), (-136.47, 59.63), (-135.03, 59.57),
+  (-134.30, 58.90), (-133.40, 58.20), (-132.20, 56.90), (-130.60, 56.20), (-130.01, 54.80), (-131.80, 54.70),
+  (-133.80, 55.90), (-136.60, 58.20), (-140.00, 59.70), (-145.00, 60.00), (-149.20, 59.10), (-152.30, 57.30),
+  (-155.20, 55.60), (-160.00, 54.60), (-164.50, 54.40), (-162.00, 57.50), (-165.00, 60.20), (-167.50, 62.50),
+  (-164.00, 64.50), (-168.10, 65.60), (-166.00, 68.30), (-161.00, 70.30), (-156.50, 71.40), (-150.00, 70.50),
+]
+
+_US_ALEUTIANS_EAST = [(-180.00, 51.00), (-158.50, 51.00), (-158.50, 56.00), (-180.00, 56.00)]
+_US_ALEUTIANS_WEST = [(172.00, 51.00), (180.00, 51.00), (180.00, 54.00), (172.00, 54.00)]
+_US_HAWAII = [(-160.50, 18.80), (-154.70, 18.80), (-154.70, 22.30), (-160.50, 22.30)]
+_US_PUERTO_RICO = [(-67.35, 17.85), (-64.55, 17.85), (-64.55, 18.55), (-67.35, 18.55)]
+_US_MARIANAS = [(144.50, 13.10), (146.20, 13.10), (146.20, 20.60), (144.50, 20.60)]
+_US_SAMOA = [(-171.20, -14.60), (-168.10, -14.60), (-168.10, -11.00), (-171.20, -11.00)]
+
+_GB_BRITAIN = [
+  (-5.72, 50.07), (-4.20, 50.32), (-3.41, 50.62), (-2.45, 50.52), (-1.80, 50.72), (-0.90, 50.77),
+  (0.58, 50.85), (1.35, 51.13), (1.38, 51.38), (1.15, 51.79), (1.35, 51.95), (1.75, 52.48),
+  (1.30, 52.94), (0.49, 52.94), (0.34, 53.15), (-0.08, 53.57), (-0.08, 54.12), (-0.61, 54.49),
+  (-1.18, 54.69), (-1.38, 54.91), (-1.50, 55.13), (-2.00, 55.77), (-2.52, 56.00), (-2.62, 56.28),
+  (-2.47, 56.55), (-2.21, 56.96), (-2.08, 57.14), (-1.77, 57.50), (-2.00, 57.70), (-2.96, 57.68),
+  (-3.90, 57.60), (-4.22, 57.48), (-4.05, 57.81), (-3.85, 58.01), (-3.65, 58.12), (-3.09, 58.44),
+  (-3.01, 58.67), (-3.35, 58.62), (-3.52, 58.60), (-4.99, 58.62), (-5.05, 58.45), (-5.16, 57.90), (-5.70, 57.72),
+  (-5.72, 57.28), (-5.83, 57.00), (-5.72, 56.65), (-5.47, 56.41), (-5.79, 55.60), (-5.62, 55.31),
+  (-4.82, 55.64), (-4.63, 55.46), (-4.85, 55.24), (-5.12, 54.84), (-4.86, 54.63), (-4.44, 54.87),
+  (-4.05, 54.83), (-3.26, 54.98), (-3.05, 54.90), (-3.50, 54.72), (-3.23, 54.07), (-3.05, 53.82),
+  (-3.40, 53.34), (-3.83, 53.33), (-4.63, 53.42), (-4.72, 53.28), (-4.35, 53.12), (-4.76, 52.80),
+  (-4.06, 52.72), (-4.09, 52.41), (-4.66, 52.09), (-5.31, 51.88), (-5.06, 51.70), (-4.70, 51.67),
+  (-4.30, 51.62), (-3.95, 51.56), (-3.70, 51.48), (-3.17, 51.45), (-2.99, 51.55), (-2.67, 51.62),
+  (-2.48, 51.72), (-2.30, 51.85), (-2.70, 51.50), (-2.98, 51.35), (-3.00, 51.20), (-3.47, 51.21),
+  (-4.12, 51.21), (-4.55, 50.83), (-5.08, 50.42), (-5.48, 50.21),
+]
+
+_GB_NORTHERN_IRELAND = [
+  (-6.03, 54.05), (-6.28, 54.10), (-6.65, 54.17), (-6.86, 54.33), (-7.16, 54.34), (-7.31, 54.12),
+  (-7.62, 54.14), (-8.00, 54.31), (-8.18, 54.47), (-8.20, 54.52), (-7.90, 54.55), (-7.85, 54.72), (-7.55, 54.75),
+  (-7.44, 54.94), (-7.25, 55.06), (-6.95, 55.22), (-6.50, 55.25), (-6.25, 55.31), (-6.03, 55.22),
+  (-5.43, 54.62), (-5.53, 54.24),
+]
+
+_GB_ISLE_OF_MAN = [(-4.85, 54.03), (-4.30, 54.03), (-4.30, 54.42), (-4.85, 54.42)]
+_GB_CHANNEL_ISLANDS = [(-2.75, 49.15), (-1.95, 49.15), (-1.95, 49.80), (-2.75, 49.80)]
+_GB_ISLE_OF_WIGHT = [(-1.60, 50.55), (-1.05, 50.55), (-1.05, 50.80), (-1.60, 50.80)]
+_GB_OUTER_HEBRIDES = [(-7.75, 56.75), (-6.05, 56.75), (-6.05, 58.55), (-7.75, 58.55)]
+_GB_INNER_HEBRIDES = [(-7.00, 55.45), (-5.55, 55.45), (-5.55, 57.85), (-7.00, 57.85)]
+_GB_ORKNEY = [(-3.50, 58.70), (-2.35, 58.70), (-2.35, 59.45), (-3.50, 59.45)]
+_GB_SHETLAND = [(-1.85, 59.80), (-0.65, 59.80), (-0.65, 60.90), (-1.85, 60.90)]
+
+_LR_LIBERIA = [
+  (-11.46, 6.77), (-11.30, 6.95), (-11.16, 7.15), (-11.05, 7.40), (-10.85, 7.75), (-10.60, 8.00),
+  (-10.28, 8.49), (-9.70, 8.54), (-9.35, 7.80),
+  (-8.85, 7.40), (-8.48, 7.55), (-8.30, 6.90), (-7.95, 6.20), (-7.60, 5.20), (-7.40, 4.55),
+  (-7.74, 4.33), (-8.46, 4.61), (-9.06, 4.97), (-9.52, 5.36), (-10.08, 5.85), (-10.40, 6.11),
+  (-10.83, 6.27),
+]
+
+_REGION_RINGS = {
+  "US": (_US_CONUS, _US_ALASKA, _US_ALEUTIANS_EAST, _US_ALEUTIANS_WEST, _US_HAWAII, _US_PUERTO_RICO,
+         _US_MARIANAS, _US_SAMOA),
+  "GB": (_GB_BRITAIN, _GB_NORTHERN_IRELAND, _GB_ISLE_OF_MAN, _GB_CHANNEL_ISLANDS, _GB_ISLE_OF_WIGHT,
+         _GB_OUTER_HEBRIDES, _GB_INNER_HEBRIDES, _GB_ORKNEY, _GB_SHETLAND),
+  "LR": (_LR_LIBERIA,),
+}
+
+
+def _bounded(rings):
+  out = []
+  for ring in rings:
+    lons = [p[0] for p in ring]
+    lats = [p[1] for p in ring]
+    out.append(((min(lons), min(lats), max(lons), max(lats)), ring))
+  return tuple(out)
+
+
+_REGIONS = tuple((region, _bounded(rings)) for region, rings in _REGION_RINGS.items())
+
+
+def _point_in_ring(lat: float, lon: float, ring) -> bool:
+  inside = False
+  count = len(ring)
+  j = count - 1
+  for i in range(count):
+    lon_i, lat_i = ring[i]
+    lon_j, lat_j = ring[j]
+    if (lat_i > lat) != (lat_j > lat):
+      crossing = (lon_j - lon_i) * (lat - lat_i) / (lat_j - lat_i) + lon_i
+      if lon < crossing:
+        inside = not inside
+    j = i
+  return inside
+
+
+def valid_position(lat: float, lon: float) -> bool:
+  return abs(lat) <= 90.0 and abs(lon) <= 180.0 and (abs(lat) > 1e-4 or abs(lon) > 1e-4)
+
+
+def region_for_position(lat: float, lon: float) -> str:
+  if not valid_position(lat, lon):
+    return UNKNOWN_REGION
+
+  for region, rings in _REGIONS:
+    for (min_lon, min_lat, max_lon, max_lat), ring in rings:
+      if min_lon <= lon <= max_lon and min_lat <= lat <= max_lat and _point_in_ring(lat, lon, ring):
+        return region
+
+  return METRIC_REGION
+
+
+def region_is_metric(region: str) -> bool:
+  return bool(region) and region not in MPH_REGIONS

iqpilot/common/git.py

@@ -0,0 +1,42 @@
+from functools import cache
+import subprocess
+from iqpilot.common.utils import run_cmd, run_cmd_default
+
+
+@cache
+def get_commit(cwd: str | None = None, branch: str = "HEAD") -> str:
+  return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
+
+
+@cache
+def get_commit_date(cwd: str | None = None, commit: str = "HEAD") -> str:
+  return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
+
+
+@cache
+def get_short_branch(cwd: str | None = None) -> str:
+  return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
+
+
+@cache
+def get_branch(cwd: str | None = None) -> str:
+  return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
+
+
+@cache
+def get_origin(cwd: str | None = None) -> str:
+  try:
+    local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
+    tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
+    return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd)
+  except subprocess.CalledProcessError:  # Not on a branch, fallback
+    return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd)
+
+
+@cache
+def get_normalized_origin(cwd: str | None = None) -> str:
+  return get_origin(cwd) \
+    .replace("git@", "", 1) \
+    .replace(".git", "", 1) \
+    .replace("https://", "", 1) \
+    .replace(":", "/", 1)

iqpilot/common/git_creds.py

@@ -0,0 +1,250 @@
+import base64
+import json
+import os
+import subprocess
+
+from iqpilot.common.params import Params
+
+PARAM = "GitAuthBlob"
+PARAMS_DIR = os.environ.get("PARAMS_DIR", "/data/params/d")
+KEY_DIR = "/data/konn3kt"
+KEY_PATH = os.path.join(KEY_DIR, "git_auth.key")
+HELPER_PATH = os.path.join(KEY_DIR, "git_credential_helper.py")
+DEFAULT_REPO_DIR = "/data/openpilot"
+CREDENTIAL_HOSTS = ("git.konn3kt.com", "gitlvb.teallvbs.xyz")
+
+_HELPER_SCRIPT = '''#!/usr/bin/env python3
+import json
+import os
+import sys
+
+KEY_PATH = "{key_path}"
+
+
+def main() -> None:
+  if len(sys.argv) < 2 or sys.argv[1] != "get":
+    return
+  # drain git's request on stdin (terminated by a blank line)
+  for line in sys.stdin:
+    if not line.strip():
+      break
+  params_dir = os.environ.get("PARAMS_DIR", "/data/params/d")
+  blob_path = os.path.join(params_dir, "GitAuthBlob")
+  try:
+    with open(KEY_PATH, "rb") as f:
+      key = f.read().strip()
+    with open(blob_path, "rb") as f:
+      blob = f.read()
+    if not blob:
+      return
+    from cryptography.fernet import Fernet
+    data = json.loads(Fernet(key).decrypt(blob).decode())
+    username = data.get("u", "")
+    token = data.get("t", "")
+    if username and token:
+      sys.stdout.write("username=%s\\npassword=%s\\n" % (username, token))
+  except Exception:
+    return
+
+
+if __name__ == "__main__":
+  main()
+'''
+
+
+def _params_get(name: str) -> bytes | None:
+  # Params -> cereal -> iqdbc: that chain is unavailable mid-bootstrap (this module's
+  # callers install those very packages), so fall back to the params file directly,
+  # exactly like the embedded credential helper does.
+  try:
+    return Params().get(name)
+  except Exception:
+    try:
+      with open(os.path.join(PARAMS_DIR, name), "rb") as f:
+        return f.read()
+    except OSError:
+      return None
+
+
+def _params_put(name: str, value: bytes) -> None:
+  try:
+    Params().put(name, value)
+    return
+  except Exception:
+    pass
+  os.makedirs(PARAMS_DIR, exist_ok=True)
+  tmp = os.path.join(PARAMS_DIR, f".tmp_{name}")
+  fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o644)
+  with os.fdopen(fd, "wb") as f:
+    f.write(value)
+    f.flush()
+    os.fsync(f.fileno())
+  os.replace(tmp, os.path.join(PARAMS_DIR, name))
+
+
+def _params_remove(name: str) -> None:
+  try:
+    Params().remove(name)
+    return
+  except Exception:
+    pass
+  try:
+    os.unlink(os.path.join(PARAMS_DIR, name))
+  except OSError:
+    pass
+
+
+def _load_or_create_key() -> bytes:
+  from cryptography.fernet import Fernet
+  try:
+    with open(KEY_PATH, "rb") as f:
+      return f.read().strip()
+  except FileNotFoundError:
+    pass
+  key = Fernet.generate_key()
+  os.makedirs(KEY_DIR, exist_ok=True)
+  # write atomically with restrictive perms
+  tmp = KEY_PATH + ".tmp"
+  fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o600)
+  with os.fdopen(fd, "wb") as f:
+    f.write(key)
+  os.replace(tmp, KEY_PATH)
+  return key
+
+
+def set_credentials(username: str, token: str) -> None:
+  """Encrypt and store credentials. Empty username AND token clears them."""
+  username = (username or "").strip()
+  token = (token or "").strip()
+  if not username and not token:
+    clear_credentials()
+    return
+  from cryptography.fernet import Fernet
+  blob = Fernet(_load_or_create_key()).encrypt(
+    json.dumps({"u": username, "t": token}).encode()
+  )
+  _params_put(PARAM, blob)
+  try:
+    install_credential_helper(DEFAULT_REPO_DIR)
+  except Exception:
+    pass
+
+
+def get_credentials() -> tuple[str, str] | None:
+  """Return (username, token), or None if unset / unreadable."""
+  blob = _params_get(PARAM)
+  if not blob:
+    return None
+  try:
+    from cryptography.fernet import Fernet
+    data = json.loads(Fernet(_load_or_create_key()).decrypt(blob).decode())
+    return data.get("u", ""), data.get("t", "")
+  except Exception:
+    return None
+
+
+def clear_credentials() -> None:
+  _params_remove(PARAM)
+
+
+def has_credentials() -> bool:
+  return get_credentials() is not None
+
+
+def _auth_header(username: str, token: str) -> str:
+  return "Authorization: Basic " + base64.b64encode(f"{username}:{token}".encode()).decode()
+
+
+def ssh_to_https(url: str) -> str:
+  """Convert an SSH git URL to its HTTPS equivalent. Returns url unchanged if it
+  is not an SSH URL. A leading ssh. host label is dropped (ssh.host -> host)."""
+  url = url.strip()
+  host = path = ""
+  if url.startswith("ssh://"):
+    rest = url[len("ssh://"):]
+    rest = rest.split("@", 1)[-1]          # drop user@
+    hostport, _, path = rest.partition("/")
+    host = hostport.split(":", 1)[0]       # drop :port
+  elif url.startswith("git@") or ("@" in url and ":" in url.split("@", 1)[-1] and "://" not in url):
+    rest = url.split("@", 1)[-1]           # host:owner/repo.git
+    host, _, path = rest.partition(":")
+  else:
+    return url  # already https/http or unrecognised
+  if host.startswith("ssh."):
+    host = host[len("ssh."):]
+  return f"https://{host}/{path}"
+
+
+def install_credential_helper(repo_dir: str = DEFAULT_REPO_DIR) -> None:
+  if get_credentials() is None:
+    return
+
+  scopes = {f"https://{host}" for host in CREDENTIAL_HOSTS}
+  origin = subprocess.run(
+    ["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
+    capture_output=True, text=True, check=False,
+  ).stdout.strip()
+  https = ssh_to_https(origin)
+  if https.startswith("https://"):
+    from urllib.parse import urlsplit
+    parts = urlsplit(https)
+    if parts.hostname:
+      scopes.add(f"{parts.scheme}://{parts.hostname}")
+
+  try:
+    os.makedirs(KEY_DIR, exist_ok=True)
+    tmp = HELPER_PATH + ".tmp"
+    fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o755)
+    with os.fdopen(fd, "w") as f:
+      f.write(_HELPER_SCRIPT.format(key_path=KEY_PATH))
+    os.replace(tmp, HELPER_PATH)
+  except Exception:
+    return
+
+  helper_cmd = f"!/usr/bin/env python3 {HELPER_PATH}"
+  for scope in scopes:
+    subprocess.run(
+      ["git", "config", "--global", f"credential.{scope}.helper", helper_cmd],
+      check=False, capture_output=True,
+    )
+
+
+def configure(repo_dir: str) -> None:
+  """Apply on-device credentials to the git repo at repo_dir before a remote op.
+
+  No-op when no credentials are stored. If the origin is an SSH URL it is
+  rewritten in-place to the HTTPS equivalent so the Basic-auth header applies.
+  The header is injected via GIT_CONFIG_* env (never persisted to .git/config).
+  Idempotent."""
+  creds = get_credentials()
+  if creds is None:
+    return
+  username, token = creds
+
+  try:
+    install_credential_helper(repo_dir)
+  except Exception:
+    pass
+
+  origin = subprocess.run(
+    ["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
+    capture_output=True, text=True, check=False,
+  ).stdout.strip()
+  if not origin:
+    return
+
+  https = ssh_to_https(origin)
+  if https != origin and https.startswith("https://"):
+    subprocess.run(
+      ["git", "-C", repo_dir, "config", "remote.origin.url", https],
+      check=False, capture_output=True,
+    )
+
+  if not https.startswith("https://"):
+    return  # header auth only works over https
+
+  # scope to this exact repo URL prefix (trailing slash => component boundary)
+  key = https if https.endswith("/") else https + "/"
+  os.environ["GIT_CONFIG_COUNT"] = "1"
+  os.environ["GIT_CONFIG_KEY_0"] = f"http.{key}.extraHeader"
+  os.environ["GIT_CONFIG_VALUE_0"] = _auth_header(username, token)

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)

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"

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]]

iqpilot/common/iq_perf.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import time
+from collections import deque
+from dataclasses import dataclass
+from typing import Any
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.swaglog import cloudlog
+
+
+TRACE_SERVICE = "iqPerfTrace"
+MAX_TRACE_SAMPLES = 16
+_SHARED_PM: messaging.PubMaster | None = None
+
+
+@dataclass(slots=True)
+class PerfSample:
+  frame_id: int = 0
+  loop_dt_us: int = 0
+  update_us: int = 0
+  state_control_us: int = 0
+  publish_us: int = 0
+  tail_work_us: int = 0
+  rk_remaining_us: int = 0
+  stale_carcontrol_us: int = 0
+  stale_carcontrol_frames: int = 0
+  sendcan_gap_us: int = 0
+  model_eval_us: int = 0
+  model_dropped_frames: int = 0
+  model_backlog: int = 0
+  texture_decode_us: int = 0
+  texture_upload_us: int = 0
+  texture_unload_us: int = 0
+  texture_prune_us: int = 0
+  texture_consume_us: int = 0
+  texture_batch_size: int = 0
+  texture_bytes: int = 0
+  texture_cache_before: int = 0
+  texture_cache_after: int = 0
+  texture_unloaded: int = 0
+  memory_usage_percent: int = 0
+  gpu_usage_percent: int = 0
+  cpu_usage_percent: int = 0
+  flags: int = 0
+
+
+class PerfTraceRing:
+  def __init__(self, size: int = MAX_TRACE_SAMPLES):
+    self._samples: deque[PerfSample] = deque(maxlen=size)
+
+  def push(self, sample: PerfSample) -> None:
+    self._samples.append(sample)
+
+  def snapshot(self) -> list[PerfSample]:
+    return list(self._samples)
+
+
+class PerfTraceEmitter:
+  _SEVERITY_MAP = {
+    "info": custom.IQPerfTrace.Severity.info,
+    "warning": custom.IQPerfTrace.Severity.warning,
+    "error": custom.IQPerfTrace.Severity.error,
+    "critical": custom.IQPerfTrace.Severity.critical,
+  }
+
+  def __init__(self, process_name: str, pubmaster: messaging.PubMaster | None = None):
+    self.process_name = process_name
+    self._pm: messaging.PubMaster | None = pubmaster
+    self._last_emit_mono: dict[str, float] = {}
+    self._disabled = False
+
+  def _pubmaster(self) -> messaging.PubMaster:
+    global _SHARED_PM
+    if self._pm is not None:
+      return self._pm
+    if _SHARED_PM is None:
+      _SHARED_PM = messaging.PubMaster([TRACE_SERVICE])
+    self._pm = _SHARED_PM
+    return self._pm
+
+  @staticmethod
+  def _clamp_uint(value: int, bits: int) -> int:
+    return max(0, min(value, (1 << bits) - 1))
+
+  @staticmethod
+  def _clamp_int(value: int, bits: int) -> int:
+    lo = -(1 << (bits - 1))
+    hi = (1 << (bits - 1)) - 1
+    return max(lo, min(value, hi))
+
+  def emit(self, event_class: str, *,
+           severity: str = "warning",
+           frame_id: int = 0,
+           total_time_us: int = 0,
+           rk_remaining_us: int = 0,
+           batch_size: int = 0,
+           dropped_frames: int = 0,
+           backlog: int = 0,
+           flags: int = 0,
+           samples: list[PerfSample] | None = None,
+           missing_services: list[str] | None = None,
+           top_processes: list[str] | None = None,
+           detail: str = "",
+           min_interval_s: float = 0.0,
+           mirror_cloudlog: bool = True) -> bool:
+    if self._disabled:
+      return False
+    now = time.monotonic()
+    last_emit = self._last_emit_mono.get(event_class, 0.0)
+    if min_interval_s > 0.0 and (now - last_emit) < min_interval_s:
+      return False
+    self._last_emit_mono[event_class] = now
+
+    msg = messaging.new_message(TRACE_SERVICE)
+    trace = msg.iqPerfTrace
+    trace.process = self.process_name
+    trace.eventClass = event_class
+    trace.severity = self._SEVERITY_MAP.get(severity, custom.IQPerfTrace.Severity.warning)
+    trace.frameId = self._clamp_uint(int(frame_id), 32)
+    trace.totalTimeUs = self._clamp_uint(int(total_time_us), 32)
+    trace.rkRemainingUs = self._clamp_int(int(rk_remaining_us), 32)
+    trace.batchSize = self._clamp_uint(int(batch_size), 16)
+    trace.droppedFrames = self._clamp_uint(int(dropped_frames), 16)
+    trace.backlog = self._clamp_uint(int(backlog), 16)
+    trace.flags = self._clamp_uint(int(flags), 32)
+    trace.missingServices = list(missing_services or [])
+    trace.topProcesses = list(top_processes or [])
+    trace.detail = detail
+
+    trace_samples = samples or []
+    samples_builder = trace.init("samples", len(trace_samples))
+    for i, sample in enumerate(trace_samples):
+      builder = samples_builder[i]
+      builder.frameId = self._clamp_uint(int(sample.frame_id), 32)
+      builder.loopDtUs = self._clamp_uint(int(sample.loop_dt_us), 32)
+      builder.updateUs = self._clamp_uint(int(sample.update_us), 32)
+      builder.stateControlUs = self._clamp_uint(int(sample.state_control_us), 32)
+      builder.publishUs = self._clamp_uint(int(sample.publish_us), 32)
+      builder.tailWorkUs = self._clamp_uint(int(sample.tail_work_us), 32)
+      builder.rkRemainingUs = self._clamp_int(int(sample.rk_remaining_us), 32)
+      builder.staleCarControlUs = self._clamp_uint(int(sample.stale_carcontrol_us), 32)
+      builder.staleCarControlFrames = self._clamp_uint(int(sample.stale_carcontrol_frames), 16)
+      builder.sendcanGapUs = self._clamp_uint(int(sample.sendcan_gap_us), 32)
+      builder.modelEvalUs = self._clamp_uint(int(sample.model_eval_us), 32)
+      builder.modelDroppedFrames = self._clamp_uint(int(sample.model_dropped_frames), 16)
+      builder.modelBacklog = self._clamp_uint(int(sample.model_backlog), 16)
+      builder.textureDecodeUs = self._clamp_uint(int(sample.texture_decode_us), 32)
+      builder.textureUploadUs = self._clamp_uint(int(sample.texture_upload_us), 32)
+      builder.textureUnloadUs = self._clamp_uint(int(sample.texture_unload_us), 32)
+      builder.texturePruneUs = self._clamp_uint(int(sample.texture_prune_us), 32)
+      builder.textureConsumeUs = self._clamp_uint(int(sample.texture_consume_us), 32)
+      builder.textureBatchSize = self._clamp_uint(int(sample.texture_batch_size), 16)
+      builder.textureBytes = self._clamp_uint(int(sample.texture_bytes), 32)
+      builder.textureCacheBefore = self._clamp_uint(int(sample.texture_cache_before), 16)
+      builder.textureCacheAfter = self._clamp_uint(int(sample.texture_cache_after), 16)
+      builder.textureUnloaded = self._clamp_uint(int(sample.texture_unloaded), 16)
+      builder.memoryUsagePercent = self._clamp_uint(int(sample.memory_usage_percent), 16)
+      builder.gpuUsagePercent = self._clamp_uint(int(sample.gpu_usage_percent), 16)
+      builder.cpuUsagePercent = self._clamp_uint(int(sample.cpu_usage_percent), 16)
+      builder.flags = self._clamp_uint(int(sample.flags), 32)
+
+    try:
+      self._pubmaster().send(TRACE_SERVICE, msg)
+    except messaging.MultiplePublishersError:
+      self._disabled = True
+      cloudlog.error(f"iq_perf_trace disabled for {self.process_name}: duplicate publisher for {TRACE_SERVICE}")
+      return False
+    except Exception:
+      cloudlog.exception(f"iq_perf_trace publish failed for {self.process_name}")
+      return False
+
+    if mirror_cloudlog:
+      cloudlog.event(
+        "iq_perf_trace",
+        process=self.process_name,
+        event_class=event_class,
+        severity=severity,
+        frame_id=int(frame_id),
+        total_time_us=int(total_time_us),
+        dropped_frames=int(dropped_frames),
+        flags=int(flags),
+        detail=detail,
+      )
+    return True

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

iqpilot/common/k3_slc_log.py

@@ -0,0 +1,15 @@
+from datetime import datetime
+
+from iqpilot.common.swaglog import cloudlog
+
+K3_SLC_LOG_FILE = "/data/openpilot/k3_slc.txt"
+
+
+def k3_slc_log(message: str) -> None:
+  try:
+    with open(K3_SLC_LOG_FILE, "a") as f:
+      timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+      f.write(f"[{timestamp}] {message}\n")
+      f.flush()
+  except Exception as e:
+    cloudlog.error(f"[K3_SLC] Failed to write debug log: {e}")

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")

iqpilot/common/markdown.py

@@ -0,0 +1,45 @@
+HTML_REPLACEMENTS = [
+  (r'&', r'&'),
+  (r'"', r'"'),
+]
+
+def parse_markdown(text: str, tab_length: int = 2) -> str:
+  lines = text.split("\n")
+  output: list[str] = []
+  list_level = 0
+
+  def end_outstanding_lists(level: int, end_level: int) -> int:
+    while level > end_level:
+      level -= 1
+      output.append("</ul>")
+      if level > 0:
+        output.append("</li>")
+    return end_level
+
+  for i, line in enumerate(lines):
+    if i + 1 < len(lines) and lines[i + 1].startswith("==="):  # heading
+      output.append(f"<h1>{line}</h1>")
+    elif line.startswith("==="):
+      pass
+    elif line.lstrip().startswith("* "):  # list
+      line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
+      if list_level >= line_level:
+        list_level = end_outstanding_lists(list_level, line_level)
+      else:
+        list_level += 1
+        if list_level > 1:
+          output[-1] = output[-1].replace("</li>", "")
+        output.append("<ul>")
+      output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>")
+    else:
+      list_level = end_outstanding_lists(list_level, 0)
+      if len(line) > 0:
+        output.append(line)
+
+  end_outstanding_lists(list_level, 0)
+  output_str = "\n".join(output) + "\n"
+
+  for (fr, to) in HTML_REPLACEMENTS:
+    output_str = output_str.replace(fr, to)
+
+  return output_str

iqpilot/common/mock/__init__.py

@@ -0,0 +1,50 @@
+"""
+Utilities for generating mock messages for testing.
+example in common/tests/test_mock.py
+"""
+
+
+import functools
+import threading
+from iqpilot.cereal.messaging import PubMaster
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.common.realtime import Ratekeeper
+
+
+MOCK_GENERATOR = {
+  "deviceMotion": generate_deviceMotion
+}
+
+
+def generate_messages_loop(services: list[str], done: threading.Event):
+  pm = PubMaster(services)
+  rk = Ratekeeper(100)
+  i = 0
+  while not done.is_set():
+    for s in services:
+      should_send = i % (100/SERVICE_LIST[s].frequency) == 0
+      if should_send:
+        message = MOCK_GENERATOR[s]()
+        pm.send(s, message)
+    i += 1
+    rk.keep_time()
+
+
+def mock_messages(services: list[str] | str):
+  if isinstance(services, str):
+    services = [services]
+
+  def decorator(func):
+    @functools.wraps(func)
+    def wrapper(*args, **kwargs):
+      done = threading.Event()
+      t = threading.Thread(target=generate_messages_loop, args=(services, done))
+      t.start()
+      try:
+        return func(*args, **kwargs)
+      finally:
+        done.set()
+        t.join()
+    return wrapper
+  return decorator

iqpilot/common/mock/generators.py

@@ -0,0 +1,14 @@
+from iqpilot.cereal import messaging
+
+
+def generate_deviceMotion():
+  msg = messaging.new_message('deviceMotion')
+  meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
+  msg.deviceMotion.orientationNED = meas
+  msg.deviceMotion.velocityDevice = meas
+  msg.deviceMotion.angularVelocityDevice = meas
+  msg.deviceMotion.accelerationDevice = meas
+  msg.deviceMotion.inputsOK = True
+  msg.deviceMotion.posenetOK = True
+  msg.deviceMotion.sensorsOK = True
+  return msg

iqpilot/common/params.py

@@ -0,0 +1,158 @@
+try:
+  from iqpilot.common.params_pyx import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
+except ImportError:
+  import datetime
+  import os
+  import threading
+  from enum import IntEnum, IntFlag
+
+  class UnknownKeyName(Exception):
+    pass
+
+  class ParamKeyFlag(IntFlag):
+    # must stay in lockstep with enum ParamKeyFlag in common/params.h
+    PERSISTENT = 0x02
+    CLEAR_ON_MANAGER_START = 0x04
+    CLEAR_ON_ONROAD_TRANSITION = 0x08
+    CLEAR_ON_OFFROAD_TRANSITION = 0x10
+    DONT_LOG = 0x20
+    DEVELOPMENT_ONLY = 0x40
+    CLEAR_ON_IGNITION_ON = 0x80
+    ALL = 0xFFFFFFFF
+
+  class ParamKeyType(IntEnum):
+    STRING = 0
+    BOOL = 1
+    INT = 2
+    FLOAT = 3
+    TIME = 4
+    JSON = 5
+    BYTES = 6
+
+  class Params:
+    def __init__(self, path: str = ""):
+      if path:
+        root = path
+      else:
+        from iqpilot.system.hardware.hw import Paths
+        root = Paths.params()
+      self._d = os.path.join(root, os.environ.get("OPENPILOT_PREFIX", "d"))
+      self._lock = threading.Lock()
+
+    def _p(self, key):
+      if isinstance(key, bytes):
+        key = key.decode()
+      return os.path.join(self._d, key)
+
+    def check_key(self, key):
+      return True
+
+    def get(self, key, block: bool = False, return_default: bool = False, encoding=None):
+      try:
+        with open(self._p(key), "rb") as f:
+          dat = f.read()
+      except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
+        return None
+      if encoding is not None:
+        return dat.decode(encoding)
+      # params_pyx returns string-typed values decoded; default to utf-8, fall back to raw bytes
+      try:
+        return dat.decode("utf-8")
+      except UnicodeDecodeError:
+        return dat
+
+    def get_bool(self, key, block: bool = False) -> bool:
+      try:
+        with open(self._p(key), "rb") as f:
+          return f.read() == b"1"
+      except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
+        return False
+
+    def get_int(self, key, block: bool = False) -> int:
+      value = self.get(key, block=block)
+      return int(value) if value else 0
+
+    def get_float(self, key, block: bool = False) -> float:
+      value = self.get(key, block=block)
+      return float(value) if value else 0.0
+
+    def put(self, key, dat):
+      if isinstance(dat, datetime.datetime):
+        dat = dat.isoformat()
+      if isinstance(dat, (int, float)):
+        # Params are strings on disk and half the fleet's writers spell numeric
+        # puts as put(key, int). Letting that reach f.write() raises
+        # "a bytes-like object is required" -- which, when the writer sits in a
+        # connection's recv loop (hephaestusd's ping handler), tears down the
+        # transport on the first server ping and flaps the device offline on a
+        # timer. A params write must not be able to do that: coerce losslessly.
+        dat = str(dat)
+      if isinstance(dat, str):
+        dat = dat.encode("utf-8")
+      with self._lock:
+        os.makedirs(self._d, exist_ok=True)
+        p = self._p(key)
+        tmp = p + ".tmp"
+        with open(tmp, "wb") as f:
+          f.write(dat)
+          f.flush()
+          os.fsync(f.fileno())
+        os.rename(tmp, p)
+
+    def put_bool(self, key, val: bool):
+      self.put(key, b"1" if val else b"0")
+
+    def put_int(self, key, val: int):
+      self.put(key, str(val))
+
+    def put_float(self, key, val: float):
+      self.put(key, str(val))
+
+    def put_nonblocking(self, key, dat):
+      self.put(key, dat)
+
+    def put_bool_nonblocking(self, key, val: bool):
+      self.put_bool(key, val)
+
+    def put_int_nonblocking(self, key, val: int):
+      self.put_int(key, val)
+
+    def put_float_nonblocking(self, key, val: float):
+      self.put_float(key, val)
+
+    def remove(self, key):
+      try:
+        os.remove(self._p(key))
+      except FileNotFoundError:
+        pass
+
+    def clear_all(self, tx_type=None):
+      pass
+
+    def get_param_path(self, key: str = "") -> str:
+      return self._p(key) if key else self._d
+
+    def all_keys(self):
+      try:
+        return [k.encode() for k in os.listdir(self._d)]
+      except FileNotFoundError:
+        return []
+
+assert Params
+assert ParamKeyFlag
+assert ParamKeyType
+assert UnknownKeyName
+
+if __name__ == "__main__":
+  import sys
+
+  params = Params()
+  key = sys.argv[1]
+  assert params.check_key(key), f"unknown param: {key}"
+
+  if len(sys.argv) == 3:
+    val = sys.argv[2]
+    print(f"SET: {key} = {val}")
+    params.put(key, val)
+  elif len(sys.argv) == 2:
+    print(f"GET: {key} = {params.get(key)}")

iqpilot/common/pid.py

@@ -0,0 +1,57 @@
+import numpy as np
+from numbers import Number
+
+class PIDController:
+  def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
+    self._k_p: list[list[float]] = [[0], [k_p]] if isinstance(k_p, Number) else k_p
+    self._k_i: list[list[float]] = [[0], [k_i]] if isinstance(k_i, Number) else k_i
+    self._k_d: list[list[float]] = [[0], [k_d]] if isinstance(k_d, Number) else k_d
+
+    self.set_limits(pos_limit, neg_limit)
+
+    self.i_dt = 1.0 / rate
+    self.speed = 0.0
+
+    self.reset()
+
+  @property
+  def k_p(self):
+    return np.interp(self.speed, self._k_p[0], self._k_p[1])
+
+  @property
+  def k_i(self):
+    return np.interp(self.speed, self._k_i[0], self._k_i[1])
+
+  @property
+  def k_d(self):
+    return np.interp(self.speed, self._k_d[0], self._k_d[1])
+
+  def reset(self):
+    self.p = 0.0
+    self.i = 0.0
+    self.d = 0.0
+    self.f = 0.0
+    self.control = 0
+
+  def set_limits(self, pos_limit, neg_limit):
+    self.pos_limit = pos_limit
+    self.neg_limit = neg_limit
+
+  def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
+    self.speed = speed
+    self.p = self.k_p * float(error)
+    self.d = self.k_d * error_rate
+    self.f = feedforward
+
+    if not freeze_integrator:
+      i = self.i + self.k_i * self.i_dt * error
+
+      # Don't allow windup if already clipping
+      test_control = self.p + i + self.d + self.f
+      i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
+      i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
+      self.i = np.clip(i, i_lowerbound, i_upperbound)
+
+    control = self.p + self.i + self.d + self.f
+    self.control = np.clip(control, self.neg_limit, self.pos_limit)
+    return self.control

iqpilot/common/prefix.py

@@ -0,0 +1,66 @@
+import os
+import platform
+import shutil
+import uuid
+
+
+from iqpilot.common.params import Params
+from iqpilot.system.hardware import PC
+from iqpilot.system.hardware.hw import Paths
+from iqpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
+
+class OpenpilotPrefix:
+  def __init__(self, prefix: str | None = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
+    self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
+    shm_path = "/tmp" if platform.system() == "Darwin" else "/dev/shm"
+    self.msgq_path = os.path.join(shm_path, "msgq_" + self.prefix)
+    self.create_dirs_on_enter = create_dirs_on_enter
+    self.clean_dirs_on_exit = clean_dirs_on_exit
+    self.shared_download_cache = shared_download_cache
+
+  def __enter__(self):
+    self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None)
+    os.environ['OPENPILOT_PREFIX'] = self.prefix
+
+    if self.create_dirs_on_enter:
+      self.create_dirs()
+
+    if self.shared_download_cache:
+      os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT
+
+    return self
+
+  def __exit__(self, exc_type, exc_obj, exc_tb):
+    if self.clean_dirs_on_exit:
+      self.clean_dirs()
+    try:
+      del os.environ['OPENPILOT_PREFIX']
+      if self.original_prefix is not None:
+        os.environ['OPENPILOT_PREFIX'] = self.original_prefix
+    except KeyError:
+      pass
+    return False
+
+  def create_dirs(self):
+    try:
+      os.mkdir(self.msgq_path)
+    except FileExistsError:
+      pass
+    os.makedirs(Paths.log_root(), exist_ok=True)
+
+  def clean_dirs(self):
+    symlink_path = Params().get_param_path()
+    if os.path.islink(symlink_path):
+      shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
+      try:
+        os.remove(symlink_path)
+      except FileNotFoundError:
+        pass
+    else:
+      shutil.rmtree(symlink_path, ignore_errors=True)
+    shutil.rmtree(self.msgq_path, ignore_errors=True)
+    if PC:
+      shutil.rmtree(Paths.log_root(), ignore_errors=True)
+    if not os.environ.get("COMMA_CACHE", False):
+      shutil.rmtree(Paths.download_cache_root(), ignore_errors=True)
+    shutil.rmtree(Paths.comma_home(), ignore_errors=True)

iqpilot/common/pt2.py

@@ -0,0 +1,40 @@
+import math
+
+
+class PT2Filter:
+  def __init__(self, w0: float, zeta: float, dt: float):
+    self.w0 = w0
+    self.zeta = zeta
+    self.dt = dt
+    self.a1, self.a2, self.b0, self.b1, self.b2 = self._design(w0, zeta, dt)
+    self.y1 = 0.0
+    self.y2 = 0.0
+    self.u1 = 0.0
+    self.u2 = 0.0
+
+  @staticmethod
+  def _design(w0: float, zeta: float, dt: float):
+    # bilinear transform of H(s) = w0^2 / (s^2 + 2*zeta*w0*s + w0^2)
+    alpha = 2.0 / dt
+    a2_den = alpha**2 + (2.0 * zeta * w0 * alpha) + w0**2
+    a1_den = (-2.0 * alpha**2) + (2.0 * w0**2)
+    a0_den = alpha**2 - (2.0 * zeta * w0 * alpha) + w0**2
+    return (a1_den / a2_den, a0_den / a2_den,
+            w0**2 / a2_den, 2.0 * w0**2 / a2_den, w0**2 / a2_den)
+
+  def reset(self, value: float = 0.0) -> None:
+    self.y1 = value
+    self.y2 = value
+    self.u1 = value
+    self.u2 = value
+
+  def update(self, u: float) -> float:
+    y = (-self.a1 * self.y1) - (self.a2 * self.y2) + (self.b0 * u) + (self.b1 * self.u1) + (self.b2 * self.u2)
+    self.y2 = self.y1
+    self.y1 = y
+    self.u2 = self.u1
+    self.u1 = u
+    return y
+
+  def steady_state_steps(self) -> int:
+    return math.ceil((4.0 / (self.zeta * self.w0)) / self.dt)

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

iqpilot/common/simple_kalman.py

@@ -0,0 +1,54 @@
+import numpy as np
+
+
+def get_kalman_gain(dt, A, C, Q, R, iterations=100):
+  P = np.zeros_like(Q)
+  for _ in range(iterations):
+    P = A.dot(P).dot(A.T) + dt * Q
+    S = C.dot(P).dot(C.T) + R
+    K = P.dot(C.T).dot(np.linalg.inv(S))
+    P = (np.eye(len(P)) - K.dot(C)).dot(P)
+  return K
+
+
+class KF1D:
+  # this EKF assumes constant covariance matrix, so calculations are much simpler
+  # the Kalman gain also needs to be precomputed using the control module
+
+  def __init__(self, x0, A, C, K):
+    self.x0_0 = x0[0][0]
+    self.x1_0 = x0[1][0]
+    self.A0_0 = A[0][0]
+    self.A0_1 = A[0][1]
+    self.A1_0 = A[1][0]
+    self.A1_1 = A[1][1]
+    self.C0_0 = C[0]
+    self.C0_1 = C[1]
+    self.K0_0 = K[0][0]
+    self.K1_0 = K[1][0]
+
+    self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
+    self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
+    self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
+    self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
+
+    # K matrix needs to  be pre-computed as follow:
+    # import control
+    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
+    # self.K = np.transpose(K)
+
+  def update(self, meas):
+    #self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
+    x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
+    x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
+    self.x0_0 = x0_0
+    self.x1_0 = x1_0
+    return [self.x0_0, self.x1_0]
+
+  @property
+  def x(self):
+    return [[self.x0_0], [self.x1_0]]
+
+  def set_x(self, x):
+    self.x0_0 = x[0][0]
+    self.x1_0 = x[1][0]

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

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"

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

iqpilot/common/spinner.py

@@ -0,0 +1,52 @@
+import os
+import subprocess
+from iqpilot.common.basedir import BASEDIR
+
+
+class Spinner:
+  def __init__(self):
+    try:
+      self.spinner_proc = subprocess.Popen(["./spinner.py"],
+                                           stdin=subprocess.PIPE,
+                                           cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
+                                           close_fds=True)
+    except OSError:
+      self.spinner_proc = None
+
+  def __enter__(self):
+    return self
+
+  def update(self, spinner_text: str):
+    if self.spinner_proc is not None:
+      self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
+      try:
+        self.spinner_proc.stdin.flush()
+      except BrokenPipeError:
+        pass
+
+  def update_progress(self, cur: float, total: float):
+    self.update(str(round(100 * cur / total)))
+
+  def close(self):
+    if self.spinner_proc is not None:
+      self.spinner_proc.kill()
+      try:
+        self.spinner_proc.communicate(timeout=2.)
+      except subprocess.TimeoutExpired:
+        print("WARNING: failed to kill spinner")
+      self.spinner_proc = None
+
+  def __del__(self):
+    self.close()
+
+  def __exit__(self, exc_type, exc_value, traceback):
+    self.close()
+
+
+if __name__ == "__main__":
+  import time
+  with Spinner() as s:
+    s.update("Spinner text")
+    time.sleep(5.0)
+  print("gone")
+  time.sleep(5.0)

iqpilot/common/stat_live.py

@@ -0,0 +1,73 @@
+import numpy as np
+
+class RunningStat:
+  # tracks realtime mean and standard deviation without storing any data
+  def __init__(self, priors=None, max_trackable=-1):
+    self.max_trackable = max_trackable
+    if priors is not None:
+      # initialize from history
+      self.M = priors[0]
+      self.S = priors[1]
+      self.n = priors[2]
+      self.M_last = self.M
+      self.S_last = self.S
+
+    else:
+      self.reset()
+
+  def reset(self):
+    self.M = 0.
+    self.S = 0.
+    self.M_last = 0.
+    self.S_last = 0.
+    self.n = 0
+
+  def push_data(self, new_data):
+    # short term memory hack
+    if self.max_trackable < 0 or self.n < self.max_trackable:
+      self.n += 1
+    if self.n == 0:
+      self.M_last = new_data
+      self.M = self.M_last
+      self.S_last = 0.
+    else:
+      self.M = self.M_last + (new_data - self.M_last) / self.n
+      self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M)
+      self.M_last = self.M
+      self.S_last = self.S
+
+  def mean(self):
+    return self.M
+
+  def variance(self):
+    if self.n >= 2:
+      return self.S / (self.n - 1.)
+    else:
+      return 0
+
+  def std(self):
+    return np.sqrt(self.variance())
+
+  def params_to_save(self):
+    return [self.M, self.S, self.n]
+
+class RunningStatFilter:
+  def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
+    self.raw_stat = RunningStat(raw_priors, -1)
+    self.filtered_stat = RunningStat(filtered_priors, max_trackable)
+
+  def reset(self):
+    self.raw_stat.reset()
+    self.filtered_stat.reset()
+
+  def push_and_update(self, new_data):
+    _std_last = self.raw_stat.std()
+    self.raw_stat.push_data(new_data)
+    _delta_std = self.raw_stat.std() - _std_last
+    if _delta_std <= 0:
+      self.filtered_stat.push_data(new_data)
+    else:
+      pass
+      # self.filtered_stat.push_data(self.filtered_stat.mean())
+
+# class SequentialBayesian():

iqpilot/common/steer_delay.py

@@ -0,0 +1,60 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Chooses which steer-actuator delay the lateral controllers run with: the value the
+live estimator learned, or the driver's fixed software delay — gated by the
+"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
+not subscribe to lateralDelay can still read the current value.
+"""
+from iqpilot.cereal import car
+from iqpilot.common.params import Params
+
+_ENABLE_KEY = "IQLiveSteerDelay"
+_FIXED_KEY = "IQSoftwareSteerDelay"
+_CACHE_KEY = "IQSteerDelayCache"
+
+
+def fixed_steer_delay(params, stock_delay):
+  """The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it."""
+  return stock_delay + float(params.get(_FIXED_KEY, return_default=True))
+
+
+def resolve_steer_delay(params, stock_delay):
+  """Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay."""
+  if not params.get_bool(_ENABLE_KEY):
+    return fixed_steer_delay(params, stock_delay)
+  return float(params.get(_CACHE_KEY, return_default=True))
+
+
+def lateral_action_delay(params, car_params, live_delay):
+  """Delay the lateral path should be planned against.
+
+  Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the
+  estimate never reaches the path: lagd cross-correlates against localizer lateral
+  accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on
+  Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the
+  live estimate.
+  """
+  if car_params.steerControlType == car.CarParams.SteerControlType.angle:
+    return resolve_steer_delay(params, car_params.steerActuatorDelay)
+  return live_delay
+
+
+def cached_steer_delay():
+  """Last value SteerDelayPublisher mirrored into the param — usable without a
+  lateralDelay subscription (e.g. at process startup)."""
+  return Params().get(_CACHE_KEY, return_default=True)
+
+
+class SteerDelayPublisher:
+  """Refreshes IQSteerDelayCache every lag message: the learned live delay when the
+  toggle is on, else the actuator delay plus the driver's fixed software offset."""
+
+  def __init__(self, car_params):
+    self._params = Params()
+    self._actuator_delay = car_params.steerActuatorDelay
+
+  def update(self, lag_msg):
+    live = self._params.get_bool(_ENABLE_KEY)
+    value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay)
+    self._params.put_nonblocking(_CACHE_KEY, value)

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)

iqpilot/common/tests/.gitignore

@@ -0,0 +1 @@
+test_common

iqpilot/common/tests/test_file_helpers.py

@@ -0,0 +1,19 @@
+import os
+from uuid import uuid4
+
+from iqpilot.common.utils import atomic_write
+
+
+class TestFileHelpers:
+  def run_atomic_write_func(self, atomic_write_func):
+    path = f"/tmp/tmp{uuid4()}"
+    with atomic_write_func(path) as f:
+      f.write("test")
+      assert not os.path.exists(path)
+
+    with open(path) as f:
+      assert f.read() == "test"
+    os.remove(path)
+
+  def test_atomic_write(self):
+    self.run_atomic_write_func(atomic_write)

iqpilot/common/tests/test_markdown.py

@@ -0,0 +1,15 @@
+import os
+
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.markdown import parse_markdown
+
+
+class TestMarkdown:
+  def test_all_release_notes(self):
+    with open(os.path.join(BASEDIR, "iqpilot", "docs", "CHANGELOG.md")) as f:
+      release_notes = f.read().split("\n\n")
+      assert len(release_notes) > 10
+
+      for rn in release_notes:
+        md = parse_markdown(rn)
+        assert len(md) > 0

iqpilot/common/tests/test_params.py

@@ -0,0 +1,145 @@
+import pytest
+import datetime
+import os
+import threading
+import time
+import uuid
+
+from iqpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
+
+class TestParams:
+  def setup_method(self):
+    self.params = Params()
+
+  def test_params_put_and_get(self):
+    self.params.put("DongleId", "cb38263377b873ee")
+    assert self.params.get("DongleId") == "cb38263377b873ee"
+
+  def test_params_non_ascii(self):
+    st = b"\xe1\x90\xff"
+    self.params.put("CarParams", st)
+    assert self.params.get("CarParams") == st
+
+  def test_params_get_cleared_manager_start(self):
+    self.params.put("CarParams", b"test")
+    self.params.put("DongleId", "cb38263377b873ee")
+    assert self.params.get("CarParams") == b"test"
+
+    undefined_param = self.params.get_param_path(uuid.uuid4().hex)
+    with open(undefined_param, "w") as f:
+      f.write("test")
+    assert os.path.isfile(undefined_param)
+
+    self.params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
+    assert self.params.get("CarParams") is None
+    assert self.params.get("DongleId") is not None
+    assert not os.path.isfile(undefined_param)
+
+  def test_params_two_things(self):
+    self.params.put("DongleId", "bob")
+    self.params.put("AthenadPid", 123)
+    assert self.params.get("DongleId") == "bob"
+    assert self.params.get("AthenadPid") == 123
+
+  def test_params_get_block(self):
+    def _delayed_writer():
+      time.sleep(0.1)
+      self.params.put("CarParams", b"test")
+    threading.Thread(target=_delayed_writer).start()
+    assert self.params.get("CarParams") is None
+    assert self.params.get("CarParams", block=True) == b"test"
+
+  def test_params_unknown_key_fails(self):
+    with pytest.raises(UnknownKeyName):
+      self.params.get("swag")
+
+    with pytest.raises(UnknownKeyName):
+      self.params.get_bool("swag")
+
+    with pytest.raises(UnknownKeyName):
+      self.params.put("swag", "abc")
+
+    with pytest.raises(UnknownKeyName):
+      self.params.put_bool("swag", True)
+
+  def test_remove_not_there(self):
+    assert self.params.get("CarParams") is None
+    self.params.remove("CarParams")
+    assert self.params.get("CarParams") is None
+
+  def test_get_bool(self):
+    self.params.remove("IsMetric")
+    assert not self.params.get_bool("IsMetric")
+
+    self.params.put_bool("IsMetric", True)
+    assert self.params.get_bool("IsMetric")
+
+    self.params.put_bool("IsMetric", False)
+    assert not self.params.get_bool("IsMetric")
+
+    self.params.put("IsMetric", True)
+    assert self.params.get_bool("IsMetric")
+
+    self.params.put("IsMetric", False)
+    assert not self.params.get_bool("IsMetric")
+
+  def test_navigation_disabled_default(self):
+    self.params.remove("NavigationEnabled")
+    assert not self.params.get_bool("NavigationEnabled")
+
+  def test_put_non_blocking_with_get_block(self):
+    q = Params()
+    def _delayed_writer():
+      time.sleep(0.1)
+      Params().put_nonblocking("CarParams", b"test")
+    threading.Thread(target=_delayed_writer).start()
+    assert q.get("CarParams") is None
+    assert q.get("CarParams", True) == b"test"
+
+  def test_put_bool_non_blocking_with_get_block(self):
+    q = Params()
+    def _delayed_writer():
+      time.sleep(0.1)
+      Params().put_bool_nonblocking("CarParams", True)
+    threading.Thread(target=_delayed_writer).start()
+    assert q.get("CarParams") is None
+    assert q.get("CarParams", True) == b"1"
+
+  def test_params_all_keys(self):
+    keys = Params().all_keys()
+
+    # sanity checks
+    assert len(keys) > 20
+    assert len(keys) == len(set(keys))
+    assert b"CarParams" in keys
+
+  def test_params_default_value(self):
+    self.params.remove("LanguageSetting")
+    self.params.remove("LongitudinalPersonality")
+    self.params.remove("LiveParameters")
+
+    assert self.params.get("LanguageSetting") is None
+    assert self.params.get("LanguageSetting", return_default=False) is None
+    assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
+    assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
+    assert self.params.get("LiveParameters") is None
+    assert self.params.get("LiveParameters", return_default=True) is None
+
+  def test_params_get_type(self):
+    # json
+    self.params.put("ApiCache_FirehoseStats", {"a": 0})
+    assert self.params.get("ApiCache_FirehoseStats") == {"a": 0}
+
+    # int
+    self.params.put("BootCount", 1441)
+    assert self.params.get("BootCount") == 1441
+
+    # bool
+    self.params.put("AdbEnabled", True)
+    assert self.params.get("AdbEnabled")
+    assert isinstance(self.params.get("AdbEnabled"), bool)
+
+    # time
+    now = datetime.datetime.now(datetime.UTC)
+    self.params.put("InstallDate", now)
+    assert self.params.get("InstallDate") == now

iqpilot/common/tests/test_realtime.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python3
+import pytest
+
+from iqpilot.common.realtime import config_background_thread, Ratekeeper
+
+
+class MonotonicClock:
+  def __init__(self) -> None:
+    self.now = 0.
+
+  def advance(self, seconds: float) -> None:
+    self.now += seconds
+
+  def __call__(self) -> float:
+    return self.now
+
+
+def test_ratekeeper_reset_discards_accumulated_lag(monkeypatch):
+  clock = MonotonicClock()
+  monkeypatch.setattr("iqpilot.common.realtime.time.monotonic", clock)
+  rk = Ratekeeper(100)
+
+  rk.monitor_time()
+  clock.advance(0.075)
+  rk.monitor_time()
+  assert rk.remaining == pytest.approx(-0.055)
+  assert rk.lag == pytest.approx(0.055)
+
+  rk.reset()
+  assert rk.remaining == 0.
+  assert rk.lag == 0.
+
+  rk.monitor_time()
+  assert rk.remaining == pytest.approx(0.01)
+  assert rk.lag == 0.
+
+
+def test_ratekeeper_reset_preserves_frame_count(monkeypatch):
+  clock = MonotonicClock()
+  monkeypatch.setattr("iqpilot.common.realtime.time.monotonic", clock)
+  rk = Ratekeeper(100)
+
+  rk.monitor_time()
+  clock.advance(0.01)
+  rk.monitor_time()
+  frame = rk.frame
+
+  rk.reset()
+  assert rk.frame == frame
+
+
+def test_config_background_thread_restores_normal_scheduling(monkeypatch):
+  calls = []
+  monkeypatch.setattr("iqpilot.common.realtime.sys.platform", "linux")
+  monkeypatch.setattr("iqpilot.common.realtime.PC", False)
+  monkeypatch.setattr("iqpilot.common.realtime.os.cpu_count", lambda: 8)
+  monkeypatch.setattr("iqpilot.common.realtime.os.SCHED_OTHER", 0, raising=False)
+  monkeypatch.setattr("iqpilot.common.realtime.os.sched_param", lambda priority: priority, raising=False)
+  monkeypatch.setattr("iqpilot.common.realtime.os.sched_setscheduler", lambda pid, policy, param: calls.append((pid, policy, param)), raising=False)
+  monkeypatch.setattr("iqpilot.common.realtime.os.sched_setaffinity", lambda pid, cores: calls.append((pid, set(cores))), raising=False)
+
+  config_background_thread()
+
+  assert calls == [(0, 0, 0), (0, set(range(8)))]

iqpilot/common/tests/test_simple_kalman.py

@@ -0,0 +1,29 @@
+from iqpilot.common.simple_kalman import KF1D
+
+
+class TestSimpleKalman:
+  def setup_method(self):
+    dt = 0.01
+    x0_0 = 0.0
+    x1_0 = 0.0
+    A0_0 = 1.0
+    A0_1 = dt
+    A1_0 = 0.0
+    A1_1 = 1.0
+    C0_0 = 1.0
+    C0_1 = 0.0
+    K0_0 = 0.12287673
+    K1_0 = 0.29666309
+
+    self.kf = KF1D(x0=[[x0_0], [x1_0]],
+                   A=[[A0_0, A0_1], [A1_0, A1_1]],
+                   C=[C0_0, C0_1],
+                   K=[[K0_0], [K1_0]])
+
+  def test_getter_setter(self):
+    self.kf.set_x([[1.0], [1.0]])
+    assert self.kf.x == [[1.0], [1.0]]
+
+  def test_update_returns_state(self):
+    x = self.kf.update(100)
+    assert x == [i[0] for i in self.kf.x]

iqpilot/common/tests/test_steer_delay.py

@@ -0,0 +1,91 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import time
+
+import pytest
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car
+from iqpilot.common.params import Params
+from iqpilot.common.steer_delay import (
+  SteerDelayPublisher,
+  cached_steer_delay,
+  fixed_steer_delay,
+  lateral_action_delay,
+  resolve_steer_delay,
+)
+
+ANGLE = car.CarParams.SteerControlType.angle
+TORQUE = car.CarParams.SteerControlType.torque
+
+LIVE_DELAY = 0.4387
+RACK_DELAY = 0.10
+OFFSET = 0.05
+
+
+@pytest.fixture
+def params(tmp_path, monkeypatch):
+  monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
+  p = Params()
+  p.put("IQSteerDelayCache", LIVE_DELAY)
+  p.put("IQSoftwareSteerDelay", OFFSET)
+  return p
+
+
+def _car_params(steer_control_type):
+  cp = car.CarParams.new_message()
+  cp.steerControlType = steer_control_type
+  cp.steerActuatorDelay = RACK_DELAY
+  return cp
+
+
+def _lateral_delay_msg(value):
+  msg = messaging.new_message("lateralDelay")
+  msg.lateralDelay.lateralDelay = value
+  return msg.as_reader()
+
+
+def test_params_fixture_is_isolated_from_the_real_device(params, tmp_path):
+  assert str(tmp_path) in params.get_param_path("")
+
+
+@pytest.mark.parametrize("live_enabled", [True, False])
+def test_torque_cars_always_use_live_delay(params, live_enabled):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  assert lateral_action_delay(params, _car_params(TORQUE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
+
+
+def test_angle_cars_ignore_live_delay_when_self_tuning_is_off(params):
+  params.put_bool("IQLiveSteerDelay", False)
+  delay = lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY)
+  assert delay == pytest.approx(RACK_DELAY + OFFSET)
+  assert delay != pytest.approx(LIVE_DELAY)
+
+
+def test_angle_cars_use_cached_delay_when_self_tuning_is_on(params):
+  params.put_bool("IQLiveSteerDelay", True)
+  assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
+
+
+@pytest.mark.parametrize("offset", [0.05, 0.20, 0.50])
+def test_manual_offset_reaches_the_path_and_matches_what_the_ui_reports(params, offset):
+  params.put_bool("IQLiveSteerDelay", False)
+  params.put("IQSoftwareSteerDelay", offset)
+  ui_total = RACK_DELAY + offset
+  assert fixed_steer_delay(params, RACK_DELAY) == pytest.approx(ui_total)
+  assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(ui_total)
+
+
+@pytest.mark.parametrize("live_enabled", [False, True])
+def test_publisher_writes_the_value_the_resolver_reads(params, live_enabled):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  params.put("IQSteerDelayCache", -1.0)
+  SteerDelayPublisher(_car_params(ANGLE)).update(_lateral_delay_msg(LIVE_DELAY))
+
+  expected = LIVE_DELAY if live_enabled else RACK_DELAY + OFFSET
+  deadline = time.monotonic() + 5.0
+  while cached_steer_delay() != pytest.approx(expected) and time.monotonic() < deadline:
+    time.sleep(0.01)
+  assert cached_steer_delay() == pytest.approx(expected)
+  assert resolve_steer_delay(params, RACK_DELAY) == pytest.approx(expected)

iqpilot/common/text_window.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+import os
+import time
+import subprocess
+from iqpilot.common.basedir import BASEDIR
+
+
+class TextWindow:
+  def __init__(self, text):
+    try:
+      self.text_proc = subprocess.Popen(["./text.py", text],
+                                        stdin=subprocess.PIPE,
+                                        cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
+                                        close_fds=True)
+    except OSError:
+      self.text_proc = None
+
+  def get_status(self):
+    if self.text_proc is not None:
+      self.text_proc.poll()
+      return self.text_proc.returncode
+    return None
+
+  def __enter__(self):
+    return self
+
+  def close(self):
+    if self.text_proc is not None:
+      self.text_proc.terminate()
+      self.text_proc = None
+
+  def wait_for_exit(self):
+    if self.text_proc is not None:
+      while True:
+        if self.get_status() == 1:
+          return
+        time.sleep(0.1)
+
+  def __del__(self):
+    self.close()
+
+  def __exit__(self, exc_type, exc_value, traceback):
+    self.close()
+
+
+if __name__ == "__main__":
+  text = """Traceback (most recent call last):
+  File "./controlsd.py", line 608, in <module>
+    main()
+  File "./controlsd.py", line 604, in main
+    controlsd_thread(sm, pm, logcan)
+  File "./controlsd.py", line 455, in controlsd_thread
+    1/0
+ZeroDivisionError: division by zero"""
+  print(text)
+
+  with TextWindow(text) as s:
+    for _ in range(100):
+      if s.get_status() == 1:
+        print("Got exit button")
+        break
+      time.sleep(0.1)
+  print("gone")

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()

iqpilot/common/timeout.py

@@ -0,0 +1,27 @@
+import signal
+
+class TimeoutException(Exception):
+  pass
+
+class Timeout:
+  """
+  Timeout context manager.
+  For example this code will raise a TimeoutException:
+  with Timeout(seconds=5, error_msg="Sleep was too long"):
+    time.sleep(10)
+  """
+  def __init__(self, seconds, error_msg=None):
+    if error_msg is None:
+      error_msg = f'Timed out after {seconds} seconds'
+    self.seconds = seconds
+    self.error_msg = error_msg
+
+  def handle_timeout(self, signume, frame):
+    raise TimeoutException(self.error_msg)
+
+  def __enter__(self):
+    signal.signal(signal.SIGALRM, self.handle_timeout)
+    signal.alarm(self.seconds)
+
+  def __exit__(self, exc_type, exc_val, exc_tb):
+    signal.alarm(0)

iqpilot/common/transformations/.gitignore

@@ -0,0 +1,2 @@
+transformations
+transformations.cpp

iqpilot/common/transformations/README.md

@@ -0,0 +1,70 @@
+
+Reference Frames
+------
+Many reference frames are used throughout. This
+folder contains all helper functions needed to
+transform between them. Generally this is done
+by generating a rotation matrix and multiplying.
+
+
+| Name | [x, y, z] | Units | Notes |
+| :-------------: |:-------------:| :-----:| :----: |
+| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. |
+| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83. <br> This is the global Mesh3D frame. |
+| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. |
+| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame. <br> Relative to camera, **not imu.** <br> ![img](http://upload.wikimedia.org/wikipedia/commons/thumb/2/2f/RPY_angles_of_airplanes.png/440px-RPY_angles_of_airplanes.png)|
+| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in. <br> More details below. <br>|
+| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road. <br> More details below. <br>|
+| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. |
+| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.|
+| Normalized Camera | [u / focal, v / focal, 1] | / | |
+| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. |
+| Normalized Model | [u / focal, v / focal, 1] | / | |
+
+
+
+
+Orientation Conventions
+------
+Quaternions, rotation matrices and euler angles are three
+equivalent representations of orientation and all three are
+used throughout the code base.
+
+For euler angles the preferred convention is [roll, pitch, yaw]
+which corresponds to rotations around the [x, y, z] axes. All
+euler angles should always be in radians or radians/s unless
+for plotting or display purposes. For quaternions the hamilton
+notations is preferred which is [q<sub>w</sub>, q<sub>x</sub>, q<sub>y</sub>, q<sub>z</sub>]. All quaternions
+should always be normalized with a strictly positive q<sub>w</sub>. **These
+quaternions are a unique representation of orientation whereas euler angles
+or rotation matrices are not.**
+
+To rotate from one frame into another with euler angles the
+convention is to rotate around roll, then pitch and then yaw,
+while rotating around the rotated axes, not the original axes.
+
+
+Car frame
+------
+Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different.
+
+The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects.
+
+
+Calibrated frame
+------
+It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame.
+
+
+Example
+------
+To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the
+first position and orientation from Mesh3D one would do:
+```
+ecef_from_local = rot_from_quat(quats_ecef[0])
+local_from_ecef = ecef_from_local.T
+positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0])
+rotations_global = rot_from_quat(quats_ecef)
+rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global)
+eulers_local = euler_from_rot(rotations_local)
+```

iqpilot/common/transformations/camera.py

@@ -0,0 +1,179 @@
+import itertools
+import numpy as np
+from dataclasses import dataclass
+
+import iqpilot.common.transformations.orientation as orient
+
+## -- hardcoded hardware params --
+@dataclass(frozen=True)
+class CameraConfig:
+  width: int
+  height: int
+  focal_length: float
+
+  @property
+  def size(self):
+    return (self.width, self.height)
+
+  @property
+  def intrinsics(self):
+    # aka 'K' aka camera_frame_from_view_frame
+    return np.array([
+      [self.focal_length,  0.0, float(self.width)/2],
+      [0.0, self.focal_length, float(self.height)/2],
+      [0.0,  0.0, 1.0]
+    ])
+
+  @property
+  def intrinsics_inv(self):
+    # aka 'K_inv' aka view_frame_from_camera_frame
+    return np.linalg.inv(self.intrinsics)
+
+@dataclass(frozen=True)
+class _NoneCameraConfig(CameraConfig):
+  width: int = 0
+  height: int = 0
+  focal_length: float = 0
+
+@dataclass(frozen=True)
+class DeviceCameraConfig:
+  fcam: CameraConfig
+  dcam: CameraConfig
+  ecam: CameraConfig
+
+  def all_cams(self):
+    for cam in ['fcam', 'dcam', 'ecam']:
+      if not isinstance(getattr(self, cam), _NoneCameraConfig):
+        yield cam, getattr(self, cam)
+
+_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0)  # focal length probably wrong? magnification is not consistent across frame
+_os_fisheye = CameraConfig(2688 // 2, 1520 // 2, 567.0 / 4 * 3)
+_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye)
+_os_config = DeviceCameraConfig(CameraConfig(2688 // 2, 1520 // 2, 1522.0 * 3 / 4), _os_fisheye, _os_fisheye)
+_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig())
+
+DEVICE_CAMERAS = {
+  # A "device camera" is defined by a device type and sensor
+
+  # sensor type was never set on eon/neo/two
+  ("neo", "unknown"): _neo_config,
+  # unknown here is AR0231, field was added with OX03C10 support
+  ("tici", "unknown"): _ar_ox_config,
+
+  # before deviceState.deviceType was set, assume tici AR config
+  ("unknown", "ar0231"): _ar_ox_config,
+  ("unknown", "ox03c10"): _ar_ox_config,
+
+  # simulator (emulates a tici)
+  ("pc", "unknown"): _ar_ox_config,
+}
+prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config)))
+DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
+
+# device/mesh : x->forward, y-> right, z->down
+# view : x->right, y->down, z->forward
+device_frame_from_view_frame = np.array([
+  [ 0.,  0.,  1.],
+  [ 1.,  0.,  0.],
+  [ 0.,  1.,  0.]
+])
+view_frame_from_device_frame = device_frame_from_view_frame.T
+
+
+# aka 'extrinsic_matrix'
+# road : x->forward, y -> left, z->up
+def get_view_frame_from_road_frame(roll, pitch, yaw, height):
+  device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
+  view_from_road = view_frame_from_device_frame.dot(device_from_road)
+  return np.hstack((view_from_road, [[0], [height], [0]]))
+
+
+
+# aka 'extrinsic_matrix'
+def get_view_frame_from_calib_frame(roll, pitch, yaw, height):
+  device_from_calib= orient.rot_from_euler([roll, pitch, yaw])
+  view_from_calib = view_frame_from_device_frame.dot(device_from_calib)
+  return np.hstack((view_from_calib, [[0], [height], [0]]))
+
+
+def vp_from_ke(m):
+  """
+  Computes the vanishing point from the product of the intrinsic and extrinsic
+  matrices C = KE.
+
+  The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T
+  """
+  return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
+
+
+def roll_from_ke(m):
+  # note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
+  return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
+                    -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
+
+
+def normalize(img_pts, intrinsics):
+  # normalizes image coordinates
+  # accepts single pt or array of pts
+  intrinsics_inv = np.linalg.inv(intrinsics)
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
+  img_pts_normalized = img_pts.dot(intrinsics_inv.T)
+  img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
+  return img_pts_normalized[:, :2].reshape(input_shape)
+
+
+def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
+  # denormalizes image coordinates
+  # accepts single pt or array of pts
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1), dtype=img_pts.dtype)))
+  img_pts_denormalized = img_pts.dot(intrinsics.T)
+  if np.isfinite(width):
+    img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
+    img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
+  if np.isfinite(height):
+    img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan
+    img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan
+  return img_pts_denormalized[:, :2].reshape(input_shape)
+
+
+def get_calib_from_vp(vp, intrinsics):
+  vp_norm = normalize(vp, intrinsics)
+  yaw_calib = np.arctan(vp_norm[0])
+  pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
+  roll_calib = 0
+  return roll_calib, pitch_calib, yaw_calib
+
+
+def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
+  # device from ecef frame
+  # device frame is x -> forward, y-> right, z -> down
+  # accepts single pt or array of pts
+  input_shape = pt_ecef.shape
+  pt_ecef = np.atleast_2d(pt_ecef)
+  ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef)
+  device_from_ecef_rot = ecef_from_device_rot.T
+  pt_ecef_rel = pt_ecef - pos_ecef
+  pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
+  return pt_device.reshape(input_shape)
+
+
+def img_from_device(pt_device):
+  # img coordinates from pts in device frame
+  # first transforms to view frame, then to img coords
+  # accepts single pt or array of pts
+  input_shape = pt_device.shape
+  pt_device = np.atleast_2d(pt_device)
+  pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device)
+
+  # This function should never return negative depths
+  pt_view[pt_view[:, 2] < 0] = np.nan
+
+  pt_img = pt_view/pt_view[:, 2:3]
+  return pt_img.reshape(input_shape)[:, :2]
+

iqpilot/common/transformations/coordinates.py

@@ -0,0 +1,18 @@
+from iqpilot.common.transformations.orientation import numpy_wrap
+from iqpilot.common.transformations.transformations import (ecef2geodetic_single,
+                                                    geodetic2ecef_single)
+from iqpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
+
+
+class LocalCoord(LocalCoord_single):
+  ecef2ned = numpy_wrap(LocalCoord_single.ecef2ned_single, (3,), (3,))
+  ned2ecef = numpy_wrap(LocalCoord_single.ned2ecef_single, (3,), (3,))
+  geodetic2ned = numpy_wrap(LocalCoord_single.geodetic2ned_single, (3,), (3,))
+  ned2geodetic = numpy_wrap(LocalCoord_single.ned2geodetic_single, (3,), (3,))
+
+
+geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
+ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
+
+geodetic_from_ecef = ecef2geodetic
+ecef_from_geodetic = geodetic2ecef

iqpilot/common/transformations/model.py

@@ -0,0 +1,70 @@
+import numpy as np
+
+from iqpilot.common.transformations.orientation import rot_from_euler
+from iqpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye
+
+# segnet
+SEGNET_SIZE = (512, 384)
+
+# MED model
+MEDMODEL_INPUT_SIZE = (512, 256)
+MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
+MEDMODEL_CY = 47.6
+
+medmodel_fl = 910.0
+medmodel_intrinsics = np.array([
+  [medmodel_fl,  0.0,  0.5 * MEDMODEL_INPUT_SIZE[0]],
+  [0.0,  medmodel_fl,                   MEDMODEL_CY],
+  [0.0,  0.0,                                   1.0]])
+
+
+# BIG model
+BIGMODEL_INPUT_SIZE = (1024, 512)
+BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+bigmodel_fl = 910.0
+bigmodel_intrinsics = np.array([
+  [bigmodel_fl,  0.0,  0.5 * BIGMODEL_INPUT_SIZE[0]],
+  [0.0,  bigmodel_fl,             256 + MEDMODEL_CY],
+  [0.0,  0.0,                                   1.0]])
+
+
+# SBIG model (big model with the size of small model)
+SBIGMODEL_INPUT_SIZE = (512, 256)
+SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+sbigmodel_fl = 455.0
+sbigmodel_intrinsics = np.array([
+  [sbigmodel_fl,  0.0,  0.5 * SBIGMODEL_INPUT_SIZE[0]],
+  [0.0,  sbigmodel_fl,      0.5 * (256 + MEDMODEL_CY)],
+  [0.0,  0.0,                                     1.0]])
+
+DM_INPUT_SIZE = (1440, 960)
+dmonitoringmodel_fl = _ar_ox_fisheye.focal_length
+dmonitoringmodel_intrinsics = np.array([
+  [dmonitoringmodel_fl,  0.0, DM_INPUT_SIZE[0]/2],
+  [0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2],
+  [0.0,  0.0, 1.0]])
+
+bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+
+sbigmodel_frame_from_calib_frame = np.dot(sbigmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
+
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
+
+# This function is verified to give similar results to xx.uncommon.utils.transform_img
+def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
+  return warp_matrix

iqpilot/common/transformations/orientation.py

@@ -0,0 +1,52 @@
+import numpy as np
+from collections.abc import Callable
+
+from iqpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
+                                                    euler2quat_single,
+                                                    euler2rot_single,
+                                                    ned_euler_from_ecef_single,
+                                                    quat2euler_single,
+                                                    quat2rot_single,
+                                                    rot2euler_single,
+                                                    rot2quat_single)
+
+
+def numpy_wrap(function, input_shape, output_shape) -> Callable[..., np.ndarray]:
+  """Wrap a function to take either an input or list of inputs and return the correct shape"""
+  def f(*inps):
+    *args, inp = inps
+    inp = np.array(inp)
+    shape = inp.shape
+
+    if len(shape) == len(input_shape):
+      out_shape = output_shape
+    else:
+      out_shape = (shape[0],) + output_shape
+
+    # Add empty dimension if inputs is not a list
+    if len(shape) == len(input_shape):
+      inp.shape = (1, ) + inp.shape
+
+    result = np.asarray([function(*args, i) for i in inp])
+    result.shape = out_shape
+    return result
+  return f
+
+
+euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
+quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
+quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
+rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
+euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
+rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
+ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
+ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
+
+quats_from_rotations = rot2quat
+quat_from_rot = rot2quat
+rotations_from_quats = quat2rot
+rot_from_quat = quat2rot
+euler_from_rot = rot2euler
+euler_from_quat = quat2euler
+rot_from_euler = euler2rot
+quat_from_euler = euler2quat

iqpilot/common/transformations/tests/test_coordinates.py

@@ -0,0 +1,137 @@
+import numpy as np
+
+import iqpilot.common.transformations.coordinates as coord
+
+geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
+                                 [27.4840915, -68.5867592, 2380],
+                                 [32.4916858, -113.652821, -6],
+                                 [15.1392514, 103.6976037, 24],
+                                 [24.2302229, 44.2835412, 1650]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613,  3884605.16002147],
+                                  [ 2068074.30639499, -5273413.78835412,  2927012.48741131],
+                                  [-2160344.53748176, -4932586.20092211,  3406636.2962545 ],
+                                  [-1458211.98517094,  5983151.11161276,  1655077.02698447],
+                                  [ 4167271.20055269,  4064398.22619263,  2602238.95265847]])
+
+
+ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
+                       [10.699003365155221, 37.319278617604269, 4.1084100025050407],
+                       [95.282646251726959, 61.266689955574428, -25.376506058505054],
+                       [68.535769283630003, -56.285970011848889, -100.54840137956515],
+                       [-33.066609321880179, 46.549821994306861, -84.062540548335591]])
+
+ecef_init_batch = np.array([2068042.69652729, -5273435.40316622,  2927004.89190746])
+ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148,  2927074.04783672],
+                                        [ 2068103.31628647, -5273393.92275431,  2927102.08725987],
+                                        [ 2068108.49939636, -5273359.27047121,  2927045.07091581],
+                                        [ 2068075.12395611, -5273381.69432566,  2927041.08207992],
+                                        [ 2068060.72033399, -5273430.6061505,  2927094.54928305]])
+
+ned_offsets_batch = np.array([[  53.88103168,   43.83445935,  -46.27488057],
+                              [  93.83378995,   71.57943024,  -30.23113187],
+                              [  57.26725796,   89.05602684,   23.02265814],
+                              [  49.71775195,   49.79767572,   17.15351015],
+                              [  78.56272609,   18.53100158,  -43.25290759]])
+
+
+class TestNED:
+  def test_small_distances(self):
+    start_geodetic = np.array([33.8042184, -117.888593, 0.0])
+    local_coord = coord.LocalCoord.from_geodetic(start_geodetic)
+
+    start_ned = local_coord.geodetic2ned(start_geodetic)
+    np.testing.assert_array_equal(start_ned, np.zeros(3,))
+
+    west_geodetic = start_geodetic + [0, -0.0005, 0]
+    west_ned = local_coord.geodetic2ned(west_geodetic)
+    assert np.abs(west_ned[0]) < 1e-3
+    assert west_ned[1] < 0
+
+    southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
+    southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
+    assert southwest_ned[0] < 0
+    assert southwest_ned[1] < 0
+
+  def test_ecef_geodetic(self):
+    # testing single
+    np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)
+
+    np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
+    np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)
+
+
+  def test_ned(self):
+    for ecef_pos in ecef_positions:
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      ecef_pos_moved = ecef_pos + [25, -25, 25]
+      ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
+      np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)
+
+    for geo_pos in geodetic_positions:
+      converter = coord.LocalCoord.from_geodetic(geo_pos)
+      geo_pos_moved = geo_pos + np.array([0, 0, 10])
+      geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
+      np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
+      np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)
+
+  def test_ned_saved_results(self):
+    for i, ecef_pos in enumerate(ecef_positions):
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
+                                 ecef_positions_offset[i],
+                                 rtol=1e-9, atol=1e-4)
+      np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
+                                 ned_offsets[i],
+                                 rtol=1e-9, atol=1e-4)
+
+  def test_ned_batch(self):
+    converter = coord.LocalCoord.from_ecef(ecef_init_batch)
+    np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
+                                                           ned_offsets_batch,
+                                                           rtol=1e-9, atol=1e-7)
+    np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
+                                                           ecef_positions_offset_batch,
+                                                           rtol=1e-9, atol=1e-7)
+
+  def test_errors(self):
+    # Test wrong shape/type for geodetic2ecef
+    # numpy_wrap raises IndexError for scalar input
+    with np.testing.assert_raises(IndexError):
+      coord.geodetic2ecef(1.0)
+
+    with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
+      coord.geodetic2ecef([0, 0])
+
+    with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
+      coord.geodetic2ecef([0, 0, 0, 0])
+
+    with np.testing.assert_raises(TypeError):
+      coord.geodetic2ecef(['a', 'b', 'c'])
+
+    # Test LocalCoord constructor errors
+    with np.testing.assert_raises(ValueError):
+      coord.LocalCoord.from_geodetic([0, 0])
+
+    with np.testing.assert_raises(ValueError):
+      coord.LocalCoord.from_geodetic(1)
+
+    with np.testing.assert_raises(TypeError):
+      coord.LocalCoord.from_geodetic(['a', 'b', 'c'])
+
+    # Test wrong shape/type for ecef2geodetic
+    with np.testing.assert_raises(ValueError):
+      coord.ecef2geodetic([1, 2])
+    with np.testing.assert_raises(ValueError):
+      coord.ecef2geodetic([1, 2, 3, 4])
+    with np.testing.assert_raises(IndexError):
+      coord.ecef2geodetic(1.0)

iqpilot/common/transformations/tests/test_orientation.py

@@ -0,0 +1,91 @@
+import numpy as np
+import pytest
+
+from iqpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
+                                               rot2quat, quat2rot, \
+                                               ned_euler_from_ecef
+
+eulers = np.array([[ 1.46520501,  2.78688383,  2.92780854],
+       [ 4.86909526,  3.60618161,  4.30648981],
+       [ 3.72175965,  2.68763705,  5.43895988],
+       [ 5.92306687,  5.69573614,  0.81100357],
+       [ 0.67838374,  5.02402037,  2.47106426]])
+
+quats = np.array([[ 0.66855182, -0.71500939,  0.19539353,  0.06017818],
+       [ 0.43163717,  0.70013301,  0.28209145,  0.49389021],
+       [ 0.44121991, -0.08252646,  0.34257534,  0.82532207],
+       [ 0.88578382, -0.04515356, -0.32936046,  0.32383617],
+       [ 0.06578165,  0.61282835,  0.07126891,  0.78424163]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ned_eulers = np.array([[ 0.46806039, -0.4881889 ,  1.65697808],
+                    [-2.14525969, -0.36533066,  0.73813479],
+                    [-1.39523364, -0.58540761, -1.77376356],
+                    [-1.84220435,  0.61828016, -1.03310421],
+                    [ 2.50450101,  0.36304151,  0.33136365]])
+
+
+class TestOrientation:
+  def test_quat_euler(self):
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
+    np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
+    np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
+
+  def test_rot_euler(self):
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
+    np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
+
+  def test_rot_quat(self):
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
+    np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
+
+  def test_euler_ned(self):
+    for i in range(len(eulers)):
+      np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
+      #np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
+    # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
+
+  def test_inputs(self):
+    with pytest.raises(ValueError):
+      euler2quat([1, 2])
+
+    with pytest.raises(ValueError):
+      quat2rot([1, 2, 3])
+
+    with pytest.raises(IndexError):
+      rot2quat(np.zeros((2, 2)))
+
+  def test_euler_rot_consistency(self):
+    rpy = [0.1, 0.2, 0.3]
+    R = euler2rot(rpy)
+
+    # R -> q -> R
+    q = rot2quat(R)
+    R_new = quat2rot(q)
+    np.testing.assert_allclose(R, R_new, atol=1e-15)
+
+    # q -> R -> Euler (quat2euler) -> R
+    rpy_new = quat2euler(q)
+    R_new2 = euler2rot(rpy_new)
+    np.testing.assert_allclose(R, R_new2, atol=1e-15)
+
+    # R -> Euler (rot2euler) -> R
+    rpy_from_rot = rot2euler(R)
+    R_new3 = euler2rot(rpy_from_rot)
+    np.testing.assert_allclose(R, R_new3, atol=1e-15)

iqpilot/common/transformations/transformations.py

@@ -0,0 +1,342 @@
+import numpy as np
+
+
+# Constants
+a = 6378137.0
+b = 6356752.3142
+esq = 6.69437999014e-3
+e1sq = 6.73949674228e-3
+
+
+def geodetic2ecef_single(g):
+  """
+  Convert geodetic coordinates (latitude, longitude, altitude) to ECEF.
+  """
+  try:
+    if len(g) != 3:
+      raise ValueError("Geodetic must be size 3")
+  except TypeError:
+    raise ValueError("Geodetic must be a sequence of length 3") from None
+
+  lat, lon, alt = g
+  lat = np.radians(lat)
+  lon = np.radians(lon)
+  xi = np.sqrt(1.0 - esq * np.sin(lat)**2)
+  x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
+  y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
+  z = (a / xi * (1.0 - esq) + alt) * np.sin(lat)
+  return np.array([x, y, z])
+
+
+def ecef2geodetic_single(e):
+  """
+  Convert ECEF to geodetic coordinates using Ferrari's solution.
+  """
+  x, y, z = e
+  r = np.sqrt(x**2 + y**2)
+  Esq = a**2 - b**2
+  F = 54 * b**2 * z**2
+  G = r**2 + (1 - esq) * z**2 - esq * Esq
+  C = (esq**2 * F * r**2) / (G**3)
+  S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C))
+  P = F / (3 * (S + 1 / S + 1)**2 * G**2)
+  Q = np.sqrt(1 + 2 * esq**2 * P)
+  r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2)
+  U = np.sqrt((r - esq * r_0)**2 + z**2)
+  V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2)
+  Z_0 = b**2 * z / (a * V)
+  h = U * (1 - b**2 / (a * V))
+  lat = np.arctan((z + e1sq * Z_0) / r)
+  lon = np.arctan2(y, x)
+  return np.array([np.degrees(lat), np.degrees(lon), h])
+
+
+def euler2quat_single(euler):
+  """
+  Convert Euler angles (roll, pitch, yaw) to a quaternion.
+  Rotation order: Z-Y-X (yaw, pitch, roll).
+  """
+  phi, theta, psi = euler
+
+  c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2)
+  c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2)
+  c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2)
+
+  w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi
+  x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi
+  y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi
+  z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi
+
+  if w < 0:
+    return np.array([-w, -x, -y, -z])
+  return np.array([w, x, y, z])
+
+
+def quat2euler_single(q):
+  """
+  Convert a quaternion to Euler angles (roll, pitch, yaw).
+  """
+  w, x, y, z = q
+  gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
+  sin_arg = 2 * (w * y - z * x)
+  sin_arg = np.clip(sin_arg, -1.0, 1.0)
+  theta = np.arcsin(sin_arg)
+  psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
+  return np.array([gamma, theta, psi])
+
+
+def quat2rot_single(q):
+  """
+  Convert a quaternion to a 3x3 rotation matrix.
+  """
+  w, x, y, z = q
+  xx, yy, zz = x * x, y * y, z * z
+  xy, xz, yz = x * y, x * z, y * z
+  wx, wy, wz = w * x, w * y, w * z
+
+  mat = np.array([
+    [1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)],
+    [2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)],
+    [2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]
+  ])
+  return mat
+
+
+def rot2quat_single(rot):
+  """
+  Convert a 3x3 rotation matrix to a quaternion.
+  """
+  trace = np.trace(rot)
+  if trace > 0:
+    s = 0.5 / np.sqrt(trace + 1.0)
+    w = 0.25 / s
+    x = (rot[2, 1] - rot[1, 2]) * s
+    y = (rot[0, 2] - rot[2, 0]) * s
+    z = (rot[1, 0] - rot[0, 1]) * s
+  else:
+    if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]:
+      s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2])
+      w = (rot[2, 1] - rot[1, 2]) / s
+      x = 0.25 * s
+      y = (rot[0, 1] + rot[1, 0]) / s
+      z = (rot[0, 2] + rot[2, 0]) / s
+    elif rot[1, 1] > rot[2, 2]:
+      s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2])
+      w = (rot[0, 2] - rot[2, 0]) / s
+      x = (rot[0, 1] + rot[1, 0]) / s
+      y = 0.25 * s
+      z = (rot[1, 2] + rot[2, 1]) / s
+    else:
+      s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1])
+      w = (rot[1, 0] - rot[0, 1]) / s
+      x = (rot[0, 2] + rot[2, 0]) / s
+      y = (rot[1, 2] + rot[2, 1]) / s
+      z = 0.25 * s
+
+  if w < 0:
+    return np.array([-w, -x, -y, -z])
+  return np.array([w, x, y, z])
+
+
+def euler2rot_single(euler):
+  """
+  Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix.
+  Rotation order: Z-Y-X (yaw, pitch, roll).
+  """
+  phi, theta, psi = euler
+
+  cx, sx = np.cos(phi), np.sin(phi)
+  cy, sy = np.cos(theta), np.sin(theta)
+  cz, sz = np.cos(psi), np.sin(psi)
+
+  Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
+  Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
+  Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
+
+  return Rz @ Ry @ Rx
+
+
+def rot2euler_single(rot):
+  """
+  Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw).
+  """
+  return quat2euler_single(rot2quat_single(rot))
+
+
+def rot_matrix(roll, pitch, yaw):
+  """
+  Create a 3x3 rotation matrix from roll, pitch, and yaw angles.
+  """
+  return euler2rot_single([roll, pitch, yaw])
+
+
+def axis_angle_to_rot(axis, angle):
+  """
+  Convert an axis-angle representation to a 3x3 rotation matrix.
+  """
+  c = np.cos(angle / 2)
+  s = np.sin(angle / 2)
+  q = np.array([c, s*axis[0], s*axis[1], s*axis[2]])
+  return quat2rot_single(q)
+
+
+class LocalCoord:
+  """
+  A class to handle conversions between ECEF and local NED coordinates.
+  """
+  def __init__(self, geodetic=None, ecef=None):
+    """
+    Initialize LocalCoord with either geodetic or ECEF coordinates.
+    """
+    if geodetic is not None:
+      self.init_ecef = geodetic2ecef_single(geodetic)
+      lat, lon, _ = geodetic
+    elif ecef is not None:
+      self.init_ecef = np.array(ecef)
+      lat, lon, _ = ecef2geodetic_single(ecef)
+    else:
+      raise ValueError("Must provide geodetic or ecef")
+
+    lat = np.radians(lat)
+    lon = np.radians(lon)
+
+    self.ned2ecef_matrix = np.array([
+      [-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)],
+      [-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)],
+      [np.cos(lat), 0, -np.sin(lat)]
+    ])
+    self.ecef2ned_matrix = self.ned2ecef_matrix.T
+
+  @classmethod
+  def from_geodetic(cls, geodetic):
+    """
+    Create a LocalCoord instance from geodetic coordinates.
+    """
+    return cls(geodetic=geodetic)
+
+  @classmethod
+  def from_ecef(cls, ecef):
+    """
+    Create a LocalCoord instance from ECEF coordinates.
+    """
+    return cls(ecef=ecef)
+
+  def ecef2ned_single(self, ecef):
+    """
+    Convert a single ECEF point to NED coordinates relative to the origin.
+    """
+    return self.ecef2ned_matrix @ (ecef - self.init_ecef)
+
+  def ned2ecef_single(self, ned):
+    """
+    Convert a single NED point to ECEF coordinates.
+    """
+    return self.ned2ecef_matrix @ ned + self.init_ecef
+
+  def geodetic2ned_single(self, geodetic):
+    """
+    Convert a single geodetic point to NED coordinates.
+    """
+    ecef = geodetic2ecef_single(geodetic)
+    return self.ecef2ned_single(ecef)
+
+  def ned2geodetic_single(self, ned):
+    """
+    Convert a single NED point to geodetic coordinates.
+    """
+    ecef = self.ned2ecef_single(ned)
+    return ecef2geodetic_single(ecef)
+
+  @property
+  def ned_from_ecef_matrix(self):
+    """
+    Returns the rotation matrix from ECEF to NED coordinates.
+    """
+    return self.ecef2ned_matrix
+
+  @property
+  def ecef_from_ned_matrix(self):
+    """
+    Returns the rotation matrix from NED to ECEF coordinates.
+    """
+    return self.ned2ecef_matrix
+
+
+def ecef_euler_from_ned_single(ecef_init, ned_pose):
+  """
+  Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin
+  to equivalent ECEF Euler angles.
+  """
+  converter = LocalCoord(ecef=ecef_init)
+  zero = np.array(ecef_init)
+
+  x0 = converter.ned2ecef_single([1, 0, 0]) - zero
+  y0 = converter.ned2ecef_single([0, 1, 0]) - zero
+  z0 = converter.ned2ecef_single([0, 0, 1]) - zero
+
+  phi, theta, psi = ned_pose
+
+  x1 = axis_angle_to_rot(z0, psi) @ x0
+  y1 = axis_angle_to_rot(z0, psi) @ y0
+  z1 = axis_angle_to_rot(z0, psi) @ z0
+
+  x2 = axis_angle_to_rot(y1, theta) @ x1
+  y2 = axis_angle_to_rot(y1, theta) @ y1
+  z2 = axis_angle_to_rot(y1, theta) @ z1
+
+  x3 = axis_angle_to_rot(x2, phi) @ x2
+  y3 = axis_angle_to_rot(x2, phi) @ y2
+
+  x0 = np.array([1.0, 0, 0])
+  y0 = np.array([0, 1.0, 0])
+  z0 = np.array([0, 0, 1.0])
+
+  psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
+  theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
+
+  y2 = axis_angle_to_rot(z0, psi_out) @ y0
+  z2 = axis_angle_to_rot(y2, theta_out) @ z0
+
+  phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
+
+  return np.array([phi_out, theta_out, psi_out])
+
+
+def ned_euler_from_ecef_single(ecef_init, ecef_pose):
+  """
+  Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin
+  to equivalent NED Euler angles.
+  """
+  converter = LocalCoord(ecef=ecef_init)
+
+  x0 = np.array([1.0, 0, 0])
+  y0 = np.array([0, 1.0, 0])
+  z0 = np.array([0, 0, 1.0])
+
+  phi, theta, psi = ecef_pose
+
+  x1 = axis_angle_to_rot(z0, psi) @ x0
+  y1 = axis_angle_to_rot(z0, psi) @ y0
+  z1 = axis_angle_to_rot(z0, psi) @ z0
+
+  x2 = axis_angle_to_rot(y1, theta) @ x1
+  y2 = axis_angle_to_rot(y1, theta) @ y1
+  z2 = axis_angle_to_rot(y1, theta) @ z1
+
+  x3 = axis_angle_to_rot(x2, phi) @ x2
+  y3 = axis_angle_to_rot(x2, phi) @ y2
+
+  zero = np.array(ecef_init)
+  x0 = converter.ned2ecef_single([1, 0, 0]) - zero
+  y0 = converter.ned2ecef_single([0, 1, 0]) - zero
+  z0 = converter.ned2ecef_single([0, 0, 1]) - zero
+
+  psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
+  theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
+
+  y2 = axis_angle_to_rot(z0, psi_out) @ y0
+  z2 = axis_angle_to_rot(y2, theta_out) @ z0
+
+  phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
+
+  return np.array([phi_out, theta_out, psi_out])

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

iqpilot/common/version.h

@@ -0,0 +1 @@
+#define COMMA_VERSION "IQ.Pilot 1.0c"

iqpilot/common/version.py

@@ -0,0 +1,5 @@
+from iqpilot.common.git import get_normalized_origin
+
+
+def get_version() -> str:
+  return "IQ.Pilot 1.0c"