IQ.Pilot Prebuilt Release @ 27f668a

artifacts/iqpilot_emac_private/python/_iqclosure/_vendored.json

@@ -0,0 +1,86 @@
+[
+  "iqpilot.common.api",
+  "iqpilot.common.api.base",
+  "iqpilot.common.api.comma_connect",
+  "iqpilot.common.atlas_alerts",
+  "iqpilot.common.basedir",
+  "iqpilot.common.constants",
+  "iqpilot.common.file_chunker",
+  "iqpilot.common.filter_simple",
+  "iqpilot.common.gpio",
+  "iqpilot.common.i2c",
+  "iqpilot.common.iq_perf",
+  "iqpilot.common.issue_debug",
+  "iqpilot.common.logging_extra",
+  "iqpilot.common.realtime",
+  "iqpilot.common.spinner",
+  "iqpilot.common.steer_delay",
+  "iqpilot.common.swaglog",
+  "iqpilot.common.time_helpers",
+  "iqpilot.common.utils",
+  "iqpilot.konn3kt.cloud_client",
+  "iqpilot.konn3kt.registration",
+  "iqpilot.selfdrive.controls.lib.desire_helper",
+  "iqpilot.selfdrive.controls.lib.drive_helpers",
+  "iqpilot.selfdrive.controls.lib.helpers.lane_change",
+  "iqpilot.selfdrive.controls.lib.helpers.lane_turn",
+  "iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard",
+  "iqpilot.selfdrive.iqmodeld.big_catalog",
+  "iqpilot.selfdrive.iqmodeld.camera",
+  "iqpilot.selfdrive.iqmodeld.config",
+  "iqpilot.selfdrive.iqmodeld.daemon",
+  "iqpilot.selfdrive.iqmodeld.driving_action",
+  "iqpilot.selfdrive.iqmodeld.egpu_helpers",
+  "iqpilot.selfdrive.iqmodeld.egpu_model",
+  "iqpilot.selfdrive.iqmodeld.emac_model_meta",
+  "iqpilot.selfdrive.iqmodeld.messaging",
+  "iqpilot.selfdrive.iqmodeld.metadata",
+  "iqpilot.selfdrive.iqmodeld.model_bundle_downloader",
+  "iqpilot.selfdrive.iqmodeld.model_channel",
+  "iqpilot.selfdrive.iqmodeld.model_warp",
+  "iqpilot.selfdrive.iqmodeld.models",
+  "iqpilot.selfdrive.iqmodeld.models.combined_artifact",
+  "iqpilot.selfdrive.iqmodeld.models.fetcher",
+  "iqpilot.selfdrive.iqmodeld.models.helpers",
+  "iqpilot.selfdrive.iqmodeld.models.inference_state",
+  "iqpilot.selfdrive.iqmodeld.models.runners.model_runner",
+  "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner",
+  "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner",
+  "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types",
+  "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner",
+  "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner",
+  "iqpilot.selfdrive.iqmodeld.models.split_model_constants",
+  "iqpilot.selfdrive.iqmodeld.parser",
+  "iqpilot.selfdrive.iqmodeld.runtime.tinygrad",
+  "iqpilot.selfdrive.iqmodeld.temporal_state",
+  "iqpilot.selfdrive.iqmodeld.tools.compile_supercombo",
+  "iqpilot.selfdrive.iqmodeld.tools.compile_warp",
+  "iqpilot.selfdrive.locationd.calibration_helpers",
+  "iqpilot.selfdrive.locationd.calibrationd",
+  "iqpilot.selfdrive.locationd.helpers",
+  "iqpilot.selfdrive.locationd.locationd",
+  "iqpilot.selfdrive.locationd.models.constants",
+  "iqpilot.selfdrive.locationd.models.pose_kf",
+  "iqpilot.selfdrive.selfdrived.alertmanager",
+  "iqpilot.selfdrive.selfdrived.events",
+  "iqpilot.selfdrive.state_estimation",
+  "iqpilot.selfdrive.state_estimation.estimator",
+  "iqpilot.selfdrive.ui.feedback.feedbackd",
+  "iqpilot.system",
+  "iqpilot.system.camerad.cameras.nv12_info",
+  "iqpilot.system.hardware",
+  "iqpilot.system.hardware.base",
+  "iqpilot.system.hardware.hw",
+  "iqpilot.system.hardware.pc.hardware",
+  "iqpilot.system.hardware.tici",
+  "iqpilot.system.hardware.tici.amplifier",
+  "iqpilot.system.hardware.tici.hardware",
+  "iqpilot.system.hardware.tici.iwlist",
+  "iqpilot.system.hardware.tici.lpa",
+  "iqpilot.system.hardware.tici.pins",
+  "iqpilot.system.hardware.tici.usb_storage",
+  "iqpilot.system.hardware.usb",
+  "iqpilot.system.micd",
+  "iqpilot.system.sentry",
+  "iqpilot.system.version"
+]

artifacts/iqpilot_emac_private/python/_iqclosure/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()

artifacts/iqpilot_emac_private/python/_iqclosure/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

artifacts/iqpilot_emac_private/python/_iqclosure/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-"

artifacts/iqpilot_emac_private/python/_iqclosure/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)

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

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

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

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

artifacts/iqpilot_emac_private/python/_iqclosure/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)

artifacts/iqpilot_emac_private/python/_iqclosure/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

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

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

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

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

artifacts/iqpilot_emac_private/python/_iqclosure/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

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

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

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

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

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

@@ -0,0 +1,134 @@
+"""Utilities for reading real time clocks and keeping soft real time constraints."""
+import gc
+import os
+import sys
+import time
+
+from setproctitle import getproctitle
+
+from iqpilot.common.utils import MovingAverage
+from iqpilot.system.hardware import PC
+
+
+# time step for each process
+DT_CTRL = 0.01  # controlsd
+DT_MDL = 0.05  # model
+DT_HW = 0.5  # hardwared and manager
+DT_DMON = 0.05  # driver monitoring
+
+
+class Priority:
+  # CORE 2
+  # - modeld = 55
+  # - camerad = 54
+  CTRL_LOW = 51 # plannerd & radard
+
+  # CORE 3
+  # - pandad = 55
+  CTRL_HIGH = 53
+
+
+def set_core_affinity(cores: list[int]) -> None:
+  if sys.platform == 'linux' and not PC:
+    os.sched_setaffinity(0, cores)
+
+
+def config_realtime_process(cores: int | list[int], priority: int) -> None:
+  gc.disable()
+  if sys.platform == 'linux' and not PC:
+    os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
+  c = cores if isinstance(cores, list) else [cores, ]
+  set_core_affinity(c)
+
+
+def config_background_thread() -> None:
+  if sys.platform == 'linux' and not PC:
+    os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
+    set_core_affinity(list(range(os.cpu_count() or 1)))
+
+
+def lock_memory() -> None:
+  """mlockall this process so memory reclaim/compaction can't stall it. RT control
+  procs only (locking ui/modeld would worsen pressure). Best-effort."""
+  if sys.platform != 'linux' or PC:
+    return
+  try:
+    import ctypes
+    import resource
+    resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY))
+    MCL_CURRENT, MCL_FUTURE = 0x1, 0x2
+    libc = ctypes.CDLL("libc.so.6", use_errno=True)
+    if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0:
+      raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno()))
+  except Exception as e:
+    try:
+      from iqpilot.common.swaglog import cloudlog
+      cloudlog.warning(f"lock_memory (mlockall) failed: {e}")
+    except Exception:
+      pass
+
+
+class Ratekeeper:
+  def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
+    """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
+    self._interval = 1. / rate
+    self._print_delay_threshold = print_delay_threshold
+    self._frame = 0
+    self._remaining = 0.0
+    self._process_name = getproctitle()
+    self._last_monitor_time = -1.
+    self._next_frame_time = -1.
+
+    self.avg_dt = MovingAverage(100)
+    self.avg_dt.add_value(self._interval)
+
+  def reset(self) -> None:
+    self._remaining = 0.0
+    self._last_monitor_time = -1.
+    self._next_frame_time = -1.
+    self.avg_dt = MovingAverage(100)
+    self.avg_dt.add_value(self._interval)
+
+  @property
+  def frame(self) -> int:
+    return self._frame
+
+  @property
+  def remaining(self) -> float:
+    return self._remaining
+
+  @property
+  def lag(self) -> float:
+    return max(0., -self._remaining)
+
+  @property
+  def lagging(self) -> bool:
+    expected_dt = self._interval * (1 / 0.9)
+    return self.avg_dt.get_average() > expected_dt
+
+  # Maintain loop rate by calling this at the end of each loop
+  def keep_time(self) -> bool:
+    lagged = self.monitor_time()
+    if self._remaining > 0:
+      time.sleep(self._remaining)
+    return lagged
+
+  # Monitors the cumulative lag, but does not enforce a rate
+  def monitor_time(self) -> bool:
+    if self._last_monitor_time < 0:
+      self._next_frame_time = time.monotonic() + self._interval
+      self._last_monitor_time = time.monotonic()
+
+    prev = self._last_monitor_time
+    self._last_monitor_time = time.monotonic()
+    self.avg_dt.add_value(self._last_monitor_time - prev)
+
+    lagged = False
+    remaining = self._next_frame_time - time.monotonic()
+    self._next_frame_time += self._interval
+    if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
+      print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
+      lagged = True
+    self._frame += 1
+    self._remaining = remaining
+    return lagged

artifacts/iqpilot_emac_private/python/_iqclosure/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)

artifacts/iqpilot_emac_private/python/_iqclosure/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)

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

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

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

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

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

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

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/cloud_client.py

@@ -0,0 +1,17 @@
+"""
+Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+
+import os
+from iqpilot.common.api.base import BaseApi
+API_HOST = os.getenv('KONN3KT_API_HOST', 'https://api-iqlabs.konn3kt.com')
+
+class Konn3ktApi(BaseApi):
+
+  def __init__(self, dongle_id):
+    super().__init__(dongle_id, API_HOST)
+    self.user_agent = "konn3kt-device-"
+
+  def get_token(self, expiry_hours=1):
+    return super()._get_token(expiry_hours=expiry_hours)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/registration.py

@@ -0,0 +1,226 @@
+#!/usr/bin/env python3
+import os
+import time
+import json
+import jwt
+import re
+import secrets
+from typing import cast
+from pathlib import Path
+
+from datetime import datetime, timedelta, UTC
+from iqpilot.common.api import api_get, get_key_pair
+from iqpilot.common.params import Params
+from iqpilot.common.spinner import Spinner
+from iqpilot.system.hardware import HARDWARE, PC
+from iqpilot.system.hardware.hw import Paths
+from iqpilot.common.swaglog import cloudlog
+
+
+UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
+
+_DONGLE_ID_RE = re.compile(r"^[a-fA-F0-9]{16}$")
+IMEI_WAIT_TIMEOUT = 15.0
+
+
+def _read_persist_dongle_id() -> str | None:
+  p = Path(Paths.persist_root()) / "comma" / "dongle_id"
+  try:
+    if not p.is_file():
+      return None
+    s = p.read_text().strip()
+    return s or None
+  except Exception:
+    cloudlog.exception("failed to read persist dongle_id")
+    return None
+
+
+def get_cached_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str | None:
+  ro = _read_persist_dongle_id()
+  if is_valid_dongle_id(ro):
+    ro = ro.lower()
+  if prefer_readonly and ro:
+    return ro
+  p = Params() if params is None else params
+  v = p.get("DongleId")
+  if v and v != UNREGISTERED_DONGLE_ID:
+    return v.lower() if is_valid_dongle_id(v) else v
+  return ro or None
+def is_valid_dongle_id(dongle_id: str | None) -> bool:
+  return bool(dongle_id and _DONGLE_ID_RE.fullmatch(dongle_id))
+def get_or_create_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str:
+  p = Params() if params is None else params
+  dongle_id = get_cached_dongle_id(p, prefer_readonly=prefer_readonly)
+  if dongle_id and dongle_id != UNREGISTERED_DONGLE_ID:
+    return dongle_id
+  dongle_id = secrets.token_hex(8)
+  p.put("DongleId", dongle_id)
+  cloudlog.warning(f"generated new DongleId={dongle_id} (no readonly dongle_id found)")
+  return dongle_id
+def ensure_dev_pairing_identity(params: Params | None = None, force_reset: bool = False) -> dict[str, str]:
+  p = Params() if params is None else params
+
+  persist_dir = Path(Paths.persist_root()) / "comma"
+  persist_dir.mkdir(parents=True, exist_ok=True)
+
+  dongle_path = persist_dir / "dongle_id"
+  priv_path = persist_dir / "id_rsa"
+  pub_path = persist_dir / "id_rsa.pub"
+
+  if force_reset:
+    for fp in (dongle_path, priv_path, pub_path):
+      try:
+        fp.unlink(missing_ok=True)
+      except Exception:
+        cloudlog.exception(f"failed to remove {fp}")
+    try:
+      (persist_dir / "konn3kt_prime_type").unlink(missing_ok=True)
+    except Exception:
+      pass
+    try:
+      p.remove("PrimeType")
+    except Exception:
+      pass
+  forced_dongle = os.getenv("KONN3KT_DEV_DONGLE_ID")
+  dongle_id = forced_dongle.strip().lower() if forced_dongle else None
+  if dongle_id and not is_valid_dongle_id(dongle_id):
+    cloudlog.error("KONN3KT_DEV_DONGLE_ID must be 16 hex chars")
+    dongle_id = None
+  if dongle_id is None:
+    existing = None
+    try:
+      existing = dongle_path.read_text().strip().lower() if dongle_path.is_file() else None
+    except Exception:
+      cloudlog.exception("failed reading existing dev dongle_id")
+    dongle_id = existing if is_valid_dongle_id(existing) else secrets.token_hex(8)
+  try:
+    dongle_path.write_text(dongle_id)
+  except Exception:
+    cloudlog.exception("failed writing dev dongle_id")
+  p.put("DongleId", dongle_id)
+  p.put("HardwareSerial", p.get("HardwareSerial") or f"DEV-{dongle_id}")
+  if force_reset or (not priv_path.is_file()) or (not pub_path.is_file()):
+    try:
+      from cryptography.hazmat.primitives import serialization
+      from cryptography.hazmat.primitives.asymmetric import rsa
+      key = rsa.generate_private_key(public_exponent=65537, key_size=2048)
+      priv_bytes = key.private_bytes(
+        encoding=serialization.Encoding.PEM,
+        format=serialization.PrivateFormat.TraditionalOpenSSL,
+        encryption_algorithm=serialization.NoEncryption(),
+      )
+      pub_bytes = key.public_key().public_bytes(
+        encoding=serialization.Encoding.PEM,
+        format=serialization.PublicFormat.SubjectPublicKeyInfo,
+      )
+      priv_path.write_bytes(priv_bytes)
+      pub_path.write_bytes(pub_bytes)
+    except Exception:
+      cloudlog.exception("failed generating dev RSA keys")
+      raise
+  return {
+    "dongle_id": dongle_id,
+    "serial": p.get("HardwareSerial") or f"DEV-{dongle_id}",
+    "persist_dir": str(persist_dir),
+  }
+def is_registered_device() -> bool:
+  dongle = Params().get("DongleId")
+  return dongle not in (None, UNREGISTERED_DONGLE_ID)
+
+
+def _normalize_imei(value: str | None) -> str:
+  return value or ""
+
+
+def get_registration_identifiers(wait_timeout: float = IMEI_WAIT_TIMEOUT, show_spinner: bool = False) -> tuple[str, str, str]:
+  serial = HARDWARE.get_serial()
+  spinner = Spinner() if show_spinner else None
+  start_time = time.monotonic()
+  imei1: str | None = None
+  imei2: str | None = None
+
+  while time.monotonic() - start_time < wait_timeout:
+    try:
+      imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
+      if imei1 or imei2:
+        break
+    except RuntimeError as e:
+      if "no modems" in str(e).lower():
+        cloudlog.warning("No cellular modem available, proceeding without IMEI")
+        break
+      cloudlog.exception("Error getting imei, trying again...")
+    except Exception:
+      cloudlog.exception("Error getting imei, trying again...")
+    time.sleep(1)
+
+  imei1 = _normalize_imei(imei1)
+  imei2 = _normalize_imei(imei2)
+
+  if not imei1 and not imei2:
+    cloudlog.warning(f"proceeding with serial-only registration for serial={serial}")
+  if spinner is not None:
+    spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
+    spinner.close()
+
+  return serial, imei1, imei2
+
+
+def register(show_spinner=False) -> str | None:
+  params = Params()
+
+  dongle_id: str | None = get_cached_dongle_id(params, prefer_readonly=True)
+  if dongle_id in ("", UNREGISTERED_DONGLE_ID):
+    dongle_id = None
+
+  jwt_algo, private_key, public_key = get_key_pair()
+
+  if not public_key:
+    dongle_id = UNREGISTERED_DONGLE_ID
+    cloudlog.warning("missing public key")
+  elif dongle_id is None:
+    if show_spinner:
+      spinner = Spinner()
+      spinner.update("registering device")
+
+    serial, imei1, imei2 = get_registration_identifiers(wait_timeout=IMEI_WAIT_TIMEOUT, show_spinner=False)
+
+    backoff = 0
+    start_time = time.monotonic()
+    while True:
+      try:
+        register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)},
+                                    cast(str, private_key), algorithm=jwt_algo)
+        cloudlog.info("getting pilotauth")
+        cloudlog.info("getting pilotauth")
+        resp = api_get("v2/pilotauth/", method='POST', timeout=15,
+                       imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
+
+        if resp.status_code in (400, 402, 403):
+          cloudlog.info(f"Unable to register device, got {resp.status_code}")
+          dongle_id = UNREGISTERED_DONGLE_ID
+        else:
+          dongleauth = json.loads(resp.text)
+          dongle_id = dongleauth["dongle_id"]
+        break
+      except Exception:
+        cloudlog.exception("failed to authenticate")
+        backoff = min(backoff + 1, 15)
+        time.sleep(backoff)
+
+      if time.monotonic() - start_time > 60 and show_spinner:
+        spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
+        return UNREGISTERED_DONGLE_ID
+
+    if show_spinner:
+      spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
+      spinner.close()
+
+  if dongle_id:
+    params.put("DongleId", dongle_id)
+    from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
+    set_offroad_alert("Offroad_UnregisteredHardware", False)
+  return dongle_id
+
+
+if __name__ == "__main__":
+  print(register())

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py

@@ -0,0 +1,294 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from iqpilot.cereal import car, custom, log
+
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
+  IQLaneSwapController,
+  AutoLaneChangeMode,
+  NavExitLaneChangeController,
+)
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard
+from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
+
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = custom.IQTurnSignalDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+NavManeuverPhase = custom.IQNavState.ManeuverPhase
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.0
+TURN_DESIRE_STOP_HOLD_TIME = 3.4
+TURN_DESIRE_STOP_GAP_TIME = 0.2
+TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
+TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS
+TURN_DESIRE_COMMIT_YAW_RATE = 0.08
+
+_LANE_CHANGE_DESIRES = {
+  (LaneChangeDirection.none, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none,
+  (LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft,
+  (LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft,
+  (LaneChangeDirection.right, LaneChangeState.off): log.Desire.none,
+  (LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none,
+  (LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight,
+  (LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight,
+}
+
+_TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
+
+_STOP_CYCLING_TURN_DESIRES = {
+  log.Desire.turnLeft,
+  log.Desire.turnRight,
+}
+
+
+def turn_desire(turn_direction) -> log.Desire:
+  return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)]
+
+
+def _direction_from_blinkers(carstate) -> int:
+  if carstate.leftBlinker:
+    return LaneChangeDirection.left
+  if carstate.rightBlinker:
+    return LaneChangeDirection.right
+  return LaneChangeDirection.none
+
+
+def _steering_nudge_matches(carstate, direction: int) -> bool:
+  if not carstate.steeringPressed:
+    return False
+  return (
+    (direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
+    (direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
+  )
+
+
+def _blindspot_matches(carstate, direction: int) -> bool:
+  return (
+    (direction == LaneChangeDirection.left and carstate.leftBlindspot) or
+    (direction == LaneChangeDirection.right and carstate.rightBlindspot)
+  )
+
+
+def _read_enable_bsm() -> bool:
+  try:
+    with car.CarParams.from_bytes(Params().get("CarParams")) as cp:
+      return bool(cp.enableBsm)
+  except Exception:
+    return False
+
+
+class DesireHelper:
+  def __init__(self):
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_direction = LaneChangeDirection.none
+    self.lane_change_timer = 0.0
+    self.lane_change_ll_prob = 1.0
+    self.prev_one_blinker = False
+    self.prev_nav_exit_active = False
+    self.desire = log.Desire.none
+
+    self.alc = IQLaneSwapController(self)
+    self.lane_turn_controller = IQNavTurnController(self)
+    self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
+    self.lateral_edge_guard = LateralEdgeGuard()
+    self.lateral_edge_block = LateralEdgeBlock.none
+    self.lane_turn_direction = TurnDirection.none
+    self.nav_turn_direction = TurnDirection.none
+    self.turn_desire_stop_timer = 0.0
+    self.turn_desire_stop_active = False
+    self.turn_desire_cycle_input = log.Desire.none
+    self.turn_desire_committed = False
+
+  @staticmethod
+  def get_lane_change_direction(carstate):
+    return _direction_from_blinkers(carstate)
+
+  @staticmethod
+  def _nav_turn_desire(nav_state):
+    if nav_state is None or not getattr(nav_state, "active", False):
+      return TurnDirection.none
+    if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive:
+      return TurnDirection.none
+    if not getattr(nav_state, "shouldSendTurnDesire", False):
+      return TurnDirection.none
+    return getattr(nav_state, "turnDesireDirection", TurnDirection.none)
+
+  def _clear_lane_change(self) -> None:
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_direction = LaneChangeDirection.none
+
+  def _refresh_turn_overrides(self, carstate, nav_state) -> bool:
+    speed_mps = carstate.vEgo
+    self.lane_turn_controller.update_params()
+    self.lane_turn_controller.update_lane_turn(
+      blindspot_left=carstate.leftBlindspot,
+      blindspot_right=carstate.rightBlindspot,
+      left_blinker=carstate.leftBlinker,
+      right_blinker=carstate.rightBlinker,
+      v_ego=speed_mps,
+    )
+    self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
+    self.nav_turn_direction = self._nav_turn_desire(nav_state)
+
+    self.nav_exit.update_params()
+    self.nav_exit.update(nav_state, carstate)
+    return bool(self.nav_exit.active)
+
+  def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool:
+    timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX
+    feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active
+    return (not lateral_active) or timed_out or feature_disabled
+
+  def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
+    if below_speed:
+      return
+    if one_blinker and not self.prev_one_blinker:
+      self.lane_change_state = LaneChangeState.preLaneChange
+      self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
+      self.lane_change_ll_prob = 1.0
+      return
+    if nav_exit_active and not self.prev_nav_exit_active:
+      self.lane_change_state = LaneChangeState.preLaneChange
+      self.lane_change_direction = self.nav_exit.direction
+      self.lane_change_ll_prob = 1.0
+
+  def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None:
+    if one_blinker:
+      self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
+    elif nav_exit_active:
+      self.lane_change_direction = self.nav_exit.direction
+
+  def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
+    self._refresh_requested_direction(one_blinker, nav_exit_active)
+    blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
+    self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction)
+    lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none
+    steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
+    nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed
+
+    self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed)
+    allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start
+
+    if (not (one_blinker or nav_exit_active)) or below_speed:
+      self._clear_lane_change()
+    elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked:
+      self.lane_change_state = LaneChangeState.laneChangeStarting
+
+  def _step_lane_change_starting(self, lane_change_prob: float) -> None:
+    self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0)
+    if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
+      self.lane_change_state = LaneChangeState.laneChangeFinishing
+
+  def _step_lane_change_finishing(self, one_blinker: bool) -> None:
+    self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
+    if self.lane_change_ll_prob <= 0.99:
+      return
+    self.lane_change_direction = LaneChangeDirection.none
+    self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off
+
+  def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None:
+    if self.lane_change_state == LaneChangeState.off:
+      self._begin_from_idle(one_blinker, nav_exit_active, below_speed)
+      return
+    if self.lane_change_state == LaneChangeState.preLaneChange:
+      self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed)
+      return
+    if self.lane_change_state == LaneChangeState.laneChangeStarting:
+      self._step_lane_change_starting(lane_change_prob)
+      return
+    if self.lane_change_state == LaneChangeState.laneChangeFinishing:
+      self._step_lane_change_finishing(one_blinker)
+
+  def _update_timer(self) -> None:
+    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
+      self.lane_change_timer = 0.0
+    else:
+      self.lane_change_timer += DT_MDL
+
+  def _clear_turn_desire_stop_cycle(self) -> None:
+    self.turn_desire_stop_timer = 0.0
+    self.turn_desire_stop_active = False
+    self.turn_desire_cycle_input = log.Desire.none
+    self.turn_desire_committed = False
+
+  def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire:
+    if desired_output not in _STOP_CYCLING_TURN_DESIRES:
+      self._clear_turn_desire_stop_cycle()
+      return desired_output
+
+    if desired_output != self.turn_desire_cycle_input:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      self.turn_desire_cycle_input = desired_output
+      self.turn_desire_committed = False
+
+    if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE:
+      self.turn_desire_committed = True
+
+    if self.turn_desire_committed:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      return desired_output
+
+    if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX:
+      self.turn_desire_stop_timer = 0.0
+      self.turn_desire_stop_active = False
+      return desired_output
+
+    if not self.turn_desire_stop_active:
+      self.turn_desire_stop_active = True
+      self.turn_desire_stop_timer = 0.0
+
+    cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME
+    self.turn_desire_stop_timer += DT_MDL
+    if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME:
+      return log.Desire.none
+    return desired_output
+
+  def _pick_desire_output(self) -> None:
+    desired_output = log.Desire.none
+    if self.nav_turn_direction != TurnDirection.none:
+      desired_output = turn_desire(self.nav_turn_direction)
+    elif self.lane_turn_direction != TurnDirection.none:
+      desired_output = turn_desire(self.lane_turn_direction)
+    else:
+      desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)]
+
+    self.desire = self._cycle_turn_desire_when_stopped(desired_output)
+
+  def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
+    self._last_carstate = carstate
+    self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL)
+    self.lateral_edge_block = LateralEdgeBlock.none
+    one_blinker = carstate.leftBlinker != carstate.rightBlinker
+    below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
+    nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
+
+    self.alc.update_params()
+    if self._reset_required(lateral_active, nav_exit_active):
+      self._clear_lane_change()
+    else:
+      self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob)
+
+    self._update_timer()
+    self.prev_one_blinker = one_blinker and lateral_active
+    self.prev_nav_exit_active = nav_exit_active
+    self.alc.update_state()
+    self._pick_desire_output()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,80 @@
+import numpy as np
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.realtime import DT_CTRL, DT_MDL
+
+MIN_SPEED = 1.0
+CONTROL_N = 17
+CAR_ROTATION_RADIUS = 0.0
+# This is a turn radius smaller than most cars can achieve
+MAX_CURVATURE = 0.4
+MAX_VEL_ERR = 5.0  # m/s
+
+MAX_LATERAL_JERK = 5.0  # m/s^3
+MAX_LATERAL_ACCEL_NO_ROLL = 5.0  # m/s^2
+MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0  # m/s^2
+DEFAULT_STOPPING_SPEED = 0.25  # m/s
+
+
+def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
+  return bool(v_ego < stopping_speed and a_target < 0.1)
+
+
+def clamp(val, min_val, max_val):
+  clamped_val = float(np.clip(val, min_val, max_val))
+  return clamped_val, clamped_val != val
+
+def smooth_value(val, prev_val, tau, dt=DT_MDL):
+  alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
+  return alpha * val + (1 - alpha) * prev_val
+
+# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the
+# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing
+# time constant so a noisy/uncertain model output doesn't jerk the wheel.
+MODEL_SMOOTHING_STD_LOW = 0.15  # m, plan y_std at 1s below which no extra smoothing is added
+MODEL_SMOOTHING_STD_HIGH = 0.25  # m, plan y_std at 1s at/above which the full max_extra_seconds is added
+MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60  # hard ceiling on base + dynamic lat smoothing seconds
+
+def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float:
+  if max_extra_seconds <= 0.0:
+    return 0.0
+  return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds]))
+
+def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]:
+  # This function respects ISO lateral jerk and acceleration limits + a max curvature
+  v_ego = max(v_ego, MIN_SPEED)
+  max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2)  # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
+  new_curvature = np.clip(new_curvature,
+                          prev_curvature - max_curvature_rate * DT_CTRL,
+                          prev_curvature + max_curvature_rate * DT_CTRL)
+
+  max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL
+  roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
+  max_lat_accel = max_lat_accel_no_roll + roll_compensation
+  min_lat_accel = -max_lat_accel_no_roll + roll_compensation
+  new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
+
+  new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
+  return float(new_curvature), limited_accel or limited_max_curv
+
+
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
+  if len(speeds) == len(t_idxs):
+    v_now = speeds[0]
+    a_now = accels[0]
+    v_target = np.interp(action_t, t_idxs, speeds)
+    a_target = 2 * (v_target - v_now) / (action_t) - a_now
+  else:
+    v_now = 0.0
+    v_target = 0.0
+    a_target = 0.0
+  return a_target, should_stop(v_now, a_target, stopping_speed)
+
+def curv_from_psis(psi_target, psi_rate, vego, action_t):
+  vego = np.clip(vego, MIN_SPEED, np.inf)
+  curv_from_psi = psi_target / (vego * action_t)
+  return 2*curv_from_psi - psi_rate / vego
+
+def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
+  psi_target = np.interp(action_t, t_idxs, yaws)
+  psi_rate = yaw_rates[0]
+  return curv_from_psis(psi_target, psi_rate, vego, action_t)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py

@@ -0,0 +1,293 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.cereal import custom, log
+
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+
+NAV_EXIT_COMMIT_DISTANCE = 500.0  # m before a route exit to begin moving into the exit lane
+_ManeuverType = custom.IQNavState.ManeuverType
+_NavDirection = custom.NavDirection
+
+
+class LaneSwapPreset:
+  DISABLED = -1
+  STEERING_NUDGE = 0
+  DIRECT = 1
+  DELAY_HALF = 2
+  DELAY_ONE = 3
+  DELAY_TWO = 4
+  DELAY_THREE = 5
+  OFF = DISABLED
+  NUDGE = STEERING_NUDGE
+  NUDGELESS = DIRECT
+  HALF_SECOND = DELAY_HALF
+  ONE_SECOND = DELAY_ONE
+  TWO_SECONDS = DELAY_TWO
+  THREE_SECONDS = DELAY_THREE
+
+
+PRESET_SECONDS = {
+  LaneSwapPreset.DISABLED: 0.0,
+  LaneSwapPreset.STEERING_NUDGE: 0.0,
+  LaneSwapPreset.DIRECT: 0.05,
+  LaneSwapPreset.DELAY_HALF: 0.5,
+  LaneSwapPreset.DELAY_ONE: 1.0,
+  LaneSwapPreset.DELAY_TWO: 2.0,
+  LaneSwapPreset.DELAY_THREE: 3.0,
+}
+
+LANE_SWAP_SECONDS = dict(PRESET_SECONDS)
+BLINDSPOT_WAIT_OFFSET = -1
+
+
+class LaneSwapEngine:
+  def __init__(self, desire_hub):
+    self._hub = desire_hub
+    self._kv = Params()
+    self._mem = {
+      "sec": 0.0,
+      "tick": 0,
+      "gate": 0.0,
+      "preset": self._kv.get("IQLaneChangeTimer", return_default=True),
+      "bsm_hold": False,
+      "braked": False,
+      "ready": False,
+      "used": False,
+    }
+    self.reload_setup()
+
+  def _pull_setup(self) -> None:
+    self._mem["bsm_hold"] = self._kv.get_bool("IQLaneChangeBsmDelay")
+    self._mem["preset"] = self._kv.get("IQLaneChangeTimer", return_default=True)
+
+  def _idle_phase(self) -> bool:
+    return (
+      self._hub.lane_change_state == log.LaneChangeState.off and
+      self._hub.lane_change_direction == log.LaneChangeDirection.none
+    )
+
+  def _seconds_for_preset(self) -> float:
+    picked = self._mem["preset"]
+    return PRESET_SECONDS.get(picked, PRESET_SECONDS[LaneSwapPreset.STEERING_NUDGE])
+
+  def _auto_preset_active(self) -> bool:
+    picked = self._mem["preset"]
+    return picked not in (LaneSwapPreset.DISABLED, LaneSwapPreset.STEERING_NUDGE)
+
+  def _advance_clock(self, blindspot_now: bool) -> None:
+    wait_s = self._seconds_for_preset()
+    self._mem["gate"] = wait_s
+    self._mem["sec"] += DT_MDL
+    if self._mem["bsm_hold"] and blindspot_now and wait_s > 0.0:
+      if wait_s == PRESET_SECONDS[LaneSwapPreset.DIRECT]:
+        self._mem["sec"] = BLINDSPOT_WAIT_OFFSET
+      else:
+        self._mem["sec"] = wait_s + BLINDSPOT_WAIT_OFFSET
+
+  def _ready_to_fire(self) -> bool:
+    return (
+      self._auto_preset_active() and
+      (not self._mem["braked"]) and
+      (not self._mem["used"]) and
+      (self._mem["sec"] > self._mem["gate"])
+    )
+
+  def reload_setup(self) -> None:
+    self._pull_setup()
+
+  def heartbeat(self) -> None:
+    if (self._mem["tick"] % 50) == 0:
+      self._pull_setup()
+    self._mem["tick"] += 1
+
+  def sample(self, blindspot_now: bool = False, brake_now: bool = False, **legacy) -> None:
+    blindspot_now = bool(legacy.get("blindspot_detected", blindspot_now))
+    brake_now = bool(legacy.get("brake_pressed", brake_now))
+    self._mem["braked"] = self._mem["braked"] or brake_now
+    self._advance_clock(blindspot_now)
+    self._mem["ready"] = self._ready_to_fire()
+
+  def finalize(self) -> None:
+    started = self._hub.lane_change_state == log.LaneChangeState.laneChangeStarting
+    self._mem["used"] = self._mem["used"] or started
+    if self._idle_phase():
+      self._mem["sec"] = 0.0
+      self._mem["braked"] = False
+      self._mem["used"] = False
+
+  @property
+  def ready(self):
+    return self._mem["ready"]
+
+  @property
+  def delay(self):
+    return self._mem["gate"]
+
+  @property
+  def elapsed(self):
+    return self._mem["sec"]
+
+  @property
+  def preset(self):
+    return self._mem["preset"]
+
+  @preset.setter
+  def preset(self, value):
+    self._mem["preset"] = value
+
+  @property
+  def bsm_hold(self):
+    return self._mem["bsm_hold"]
+
+  @bsm_hold.setter
+  def bsm_hold(self, value):
+    self._mem["bsm_hold"] = bool(value)
+
+  @property
+  def braked(self):
+    return self._mem["braked"]
+
+  @braked.setter
+  def braked(self, value):
+    self._mem["braked"] = bool(value)
+
+  @property
+  def used(self):
+    return self._mem["used"]
+
+  @used.setter
+  def used(self, value):
+    self._mem["used"] = bool(value)
+
+
+class NavExitLaneChangeController:
+  def __init__(self, enable_bsm: bool):
+    self._params = Params()
+    self._enable_bsm = bool(enable_bsm)
+    self.enabled = self._read_enabled()
+    self._tick = 0
+    self.active = False
+    self.direction = log.LaneChangeDirection.none
+    self.auto_allowed = False
+
+  def _read_enabled(self) -> bool:
+    try:
+      return self._params.get_bool("NavExitLaneChange")
+    except Exception:
+      return False
+
+  def update_params(self) -> None:
+    if self._tick % 50 == 0:
+      self.enabled = self._read_enabled()
+    self._tick += 1
+
+  @staticmethod
+  def _raw(value):
+    return getattr(value, "raw", value)
+
+  def update(self, nav_state, carstate) -> None:
+    self.active = False
+    self.direction = log.LaneChangeDirection.none
+    self.auto_allowed = False
+
+    if not self.enabled or nav_state is None or not getattr(nav_state, "active", False):
+      return
+    if not getattr(nav_state, "nextManeuverValid", False):
+      return
+    if self._raw(getattr(nav_state, "nextManeuverType", _ManeuverType.none)) != int(_ManeuverType.exit):
+      return
+    distance = float(getattr(nav_state, "nextManeuverDistance", 0.0))
+    if not 0.0 < distance <= NAV_EXIT_COMMIT_DISTANCE:
+      return
+
+    direction = self._raw(getattr(nav_state, "nextManeuverDirection", _NavDirection.none))
+    if direction == int(_NavDirection.left):
+      self.direction = log.LaneChangeDirection.left
+    elif direction == int(_NavDirection.right):
+      self.direction = log.LaneChangeDirection.right
+    else:
+      return
+
+    self.active = True
+    blindspot = carstate.leftBlindspot if self.direction == log.LaneChangeDirection.left else carstate.rightBlindspot
+    self.auto_allowed = (not blindspot) if self._enable_bsm else False
+
+
+AutoLaneChangeMode = LaneSwapPreset
+AUTO_LANE_CHANGE_TIMER = LANE_SWAP_SECONDS
+ONE_SECOND_DELAY = BLINDSPOT_WAIT_OFFSET
+
+
+class IQLaneSwapController(LaneSwapEngine):
+  def __init__(self, desire_helper):
+    super().__init__(desire_helper)
+
+  def reset(self) -> None:
+    self.finalize()
+
+  def update_params(self) -> None:
+    self.heartbeat()
+
+  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+    self.sample(blindspot_now=blindspot_detected, brake_now=brake_pressed)
+
+  def update_state(self) -> None:
+    self.finalize()
+
+  @property
+  def lane_change_wait_timer(self):
+    return self.elapsed
+
+  @lane_change_wait_timer.setter
+  def lane_change_wait_timer(self, value):
+    self._mem["sec"] = float(value)
+
+  @property
+  def lane_change_delay(self):
+    return self.delay
+
+  @lane_change_delay.setter
+  def lane_change_delay(self, value):
+    self._mem["gate"] = float(value)
+
+  @property
+  def lane_change_set_timer(self):
+    return self.preset
+
+  @lane_change_set_timer.setter
+  def lane_change_set_timer(self, value):
+    self.preset = value
+
+  @property
+  def lane_change_bsm_delay(self):
+    return self.bsm_hold
+
+  @lane_change_bsm_delay.setter
+  def lane_change_bsm_delay(self, value):
+    self.bsm_hold = value
+
+  @property
+  def prev_brake_pressed(self):
+    return self.braked
+
+  @prev_brake_pressed.setter
+  def prev_brake_pressed(self, value):
+    self.braked = value
+
+  @property
+  def auto_lane_change_allowed(self):
+    return self.ready
+
+  @auto_lane_change_allowed.setter
+  def auto_lane_change_allowed(self, value):
+    self._mem["ready"] = bool(value)
+
+  @property
+  def prev_lane_change(self):
+    return self.used
+
+  @prev_lane_change.setter
+  def prev_lane_change(self, value):
+    self.used = value

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py

@@ -0,0 +1,157 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+from iqpilot.cereal import custom
+
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+
+TurnDirection = custom.IQTurnSignalDirection
+
+TURN_TRIGGER_MPS = 20 * CV.MPH_TO_MS
+TURN_SPEED_GATE_MPS = TURN_TRIGGER_MPS
+LANE_CHANGE_SPEED_MIN = TURN_SPEED_GATE_MPS
+
+
+@dataclass
+class _TurnGateState:
+  active: bool = False
+  speed_limit_mps: float = TURN_TRIGGER_MPS
+  outcome: int = TurnDirection.none
+  refresh_tick: int = 0
+
+
+def _mph_param_to_mps(raw_value) -> float:
+  try:
+    return float(raw_value) * CV.MPH_TO_MS
+  except (TypeError, ValueError):
+    return TURN_TRIGGER_MPS
+
+
+def _resolve_signal_choice(speed_mps: float,
+                           speed_limit_mps: float,
+                           left_signal: bool,
+                           right_signal: bool,
+                           left_blocked: bool,
+                           right_blocked: bool) -> int:
+  if speed_mps >= speed_limit_mps:
+    return TurnDirection.none
+  if left_signal and not right_signal and not left_blocked:
+    return TurnDirection.turnLeft
+  if right_signal and not left_signal and not right_blocked:
+    return TurnDirection.turnRight
+  return TurnDirection.none
+
+
+class TurnSignalPlanner:
+  _REFRESH_STRIDE = 50
+
+  def __init__(self, desire_hub):
+    self._desire_hub = desire_hub
+    self._params = Params()
+    self._state = _TurnGateState()
+    self.reload_setup()
+
+  def _refresh_from_params(self) -> None:
+    requested_gate = _mph_param_to_mps(self._params.get("IQLaneTurnValue", return_default=True))
+    self._state.active = self._params.get_bool("IQLaneTurnDesire")
+    self._state.speed_limit_mps = min(TURN_TRIGGER_MPS, requested_gate)
+
+  def _consume_legacy_kwargs(self, **legacy) -> tuple[bool, bool, bool, bool, float]:
+    return (
+      bool(legacy.get("blindspot_left", False)),
+      bool(legacy.get("blindspot_right", False)),
+      bool(legacy.get("left_blinker", False)),
+      bool(legacy.get("right_blinker", False)),
+      float(legacy.get("v_ego", 0.0)),
+    )
+
+  def reload_setup(self):
+    self._refresh_from_params()
+
+  def heartbeat(self) -> None:
+    if self._state.refresh_tick % self._REFRESH_STRIDE == 0:
+      self._refresh_from_params()
+    self._state.refresh_tick += 1
+
+  def sample(self,
+             blocked_l: bool = False,
+             blocked_r: bool = False,
+             blink_l: bool = False,
+             blink_r: bool = False,
+             speed_mps: float = 0.0,
+             **legacy) -> None:
+    if legacy:
+      blocked_l, blocked_r, blink_l, blink_r, speed_mps = self._consume_legacy_kwargs(**legacy)
+    self._state.outcome = _resolve_signal_choice(speed_mps,
+                                                 self._state.speed_limit_mps,
+                                                 blink_l,
+                                                 blink_r,
+                                                 blocked_l,
+                                                 blocked_r)
+
+  def output(self):
+    return self._state.outcome if self._state.active else TurnDirection.none
+
+  @property
+  def enabled(self):
+    return self._state.active
+
+  @enabled.setter
+  def enabled(self, value):
+    self._state.active = bool(value)
+
+  @property
+  def speed_gate(self):
+    return self._state.speed_limit_mps
+
+  @speed_gate.setter
+  def speed_gate(self, value):
+    self._state.speed_limit_mps = float(value)
+
+  @property
+  def turn_direction(self):
+    return self._state.outcome
+
+  @turn_direction.setter
+  def turn_direction(self, value):
+    self._state.outcome = value
+
+
+class IQNavTurnController(TurnSignalPlanner):
+  def __init__(self, desire_helper):
+    super().__init__(desire_helper)
+
+  def read_params(self):
+    self.reload_setup()
+
+  def update_params(self) -> None:
+    self.heartbeat()
+
+  def update_lane_turn(self,
+                       blindspot_left: bool,
+                       blindspot_right: bool,
+                       left_blinker: bool,
+                       right_blinker: bool,
+                       v_ego: float) -> None:
+    self.sample(blocked_l=blindspot_left,
+                blocked_r=blindspot_right,
+                blink_l=left_blinker,
+                blink_r=right_blinker,
+                speed_mps=v_ego)
+
+  def get_turn_direction(self):
+    return self.output()
+
+  @property
+  def lane_turn_value(self):
+    return self.speed_gate
+
+  @lane_turn_value.setter
+  def lane_turn_value(self, value):
+    self.speed_gate = value

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py

@@ -0,0 +1,268 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, replace
+from enum import IntEnum
+from typing import Any
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+
+
+MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS
+MAX_VALID_ROAD_EDGE_STD_M = 1.0
+EDGE_CONFIDENCE_SIGMA = 1.0
+ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0
+ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0
+LANE_CENTER_OFFSET_M = 3.5
+VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
+EDGE_CLEARANCE_MARGIN_M = 0.25
+ADJACENT_LANE_LINE_PROB = 0.5
+EGO_LANE_LINE_PROB_MIN = 0.5
+MIN_MEASURED_LANE_WIDTH_M = 2.5
+MAX_MEASURED_LANE_WIDTH_M = 4.5
+OUTER_LANE_LINE_INDEX = (0, 3)
+EGO_LANE_LINE_INDEX = (1, 2)
+REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
+BLOCK_DEBOUNCE_S = 0.30
+CLEAR_DEBOUNCE_S = 0.50
+UNAVAILABLE_HOLD_S = 0.50
+TIMER_EPSILON_S = 1e-9
+PARAM_REFRESH_FRAMES = 50
+
+LaneChangeDirection = log.LaneChangeDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+
+
+class RoadEdgeDataState(IntEnum):
+  VALID = 0
+  UNAVAILABLE = 1
+  INVALID = 2
+
+
+@dataclass(frozen=True, slots=True)
+class RoadEdgeMeasurement:
+  state: RoadEdgeDataState
+  lateral_distance_m: float | None = None
+  conservative_distance_m: float | None = None
+  should_block: bool | None = None
+
+
+@dataclass(frozen=True, slots=True)
+class _SideState:
+  blocked: bool = False
+  block_timer_s: float = 0.0
+  clear_timer_s: float = 0.0
+  unavailable_timer_s: float = 0.0
+  fallback_reported: bool = False
+
+
+def evaluate_road_edge(edge: Any, std_m: Any, direction: int,
+                       lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement:
+  if edge is None or std_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    xs = edge.x
+    ys = edge.y
+    count = len(xs)
+    y_count = len(ys)
+  except (AttributeError, TypeError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  if count == 0 or y_count != count:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    std = float(std_m)
+  except (TypeError, ValueError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+  if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M:
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+
+  lateral_distance_m: float | None = None
+  for idx in range(count):
+    try:
+      x_m = float(xs[idx])
+      y_m = float(ys[idx])
+    except (IndexError, TypeError, ValueError):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not math.isfinite(x_m) or not math.isfinite(y_m):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M:
+      continue
+    if ((direction == LaneChangeDirection.left and y_m >= 0.0) or
+        (direction == LaneChangeDirection.right and y_m <= 0.0)):
+      return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+    distance_m = abs(y_m)
+    lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m)
+
+  if lateral_distance_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
+  required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
+  return RoadEdgeMeasurement(
+    RoadEdgeDataState.VALID,
+    lateral_distance_m,
+    conservative_distance_m,
+    conservative_distance_m < required_distance_m,
+  )
+
+
+def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool,
+                    dt_s: float) -> tuple[_SideState, bool]:
+  if not speed_active:
+    return _SideState(), False
+
+  if measurement.state == RoadEdgeDataState.UNAVAILABLE:
+    unavailable_timer_s = state.unavailable_timer_s + dt_s
+    if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S:
+      return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s,
+                        fallback_reported=state.fallback_reported), False
+    fallback_started = not state.fallback_reported
+    return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started
+
+  should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False
+  if should_block == state.blocked:
+    return _SideState(blocked=state.blocked), False
+
+  if should_block:
+    block_timer_s = state.block_timer_s + dt_s
+    if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S:
+      return _SideState(blocked=True), False
+    return _SideState(block_timer_s=block_timer_s), False
+
+  clear_timer_s = state.clear_timer_s + dt_s
+  if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S:
+    return _SideState(), False
+  return _SideState(blocked=True, clear_timer_s=clear_timer_s), False
+
+
+class LateralEdgeGuard:
+  def __init__(self, enabled: bool | None = None) -> None:
+    self._params = Params() if enabled is None else None
+    self._param_refresh_frame = 0
+    self.enabled = self._read_enabled() if enabled is None else enabled
+    self._active = False
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  def _read_enabled(self) -> bool:
+    try:
+      return bool(self._params and self._params.get_bool("IQEdgeGuard"))
+    except Exception:
+      return False
+
+  def _refresh_enabled(self) -> None:
+    if self._params is not None and self._param_refresh_frame % PARAM_REFRESH_FRAMES == 0:
+      self.enabled = self._read_enabled()
+    self._param_refresh_frame += 1
+
+  def _reset(self) -> None:
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  @staticmethod
+  def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]:
+    if modeldata is None:
+      return None, None
+    try:
+      edges = modeldata.roadEdges
+      stds = modeldata.roadEdgeStds
+      if len(edges) <= side_index or len(stds) <= side_index:
+        return None, None
+      return edges[side_index], stds[side_index]
+    except (AttributeError, TypeError):
+      return None, None
+
+  @staticmethod
+  def _lane_line_prob(modeldata: Any, index: int) -> float | None:
+    if modeldata is None:
+      return None
+    try:
+      probs = modeldata.laneLineProbs
+      if len(probs) <= index:
+        return None
+      value = float(probs[index])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return None
+    return value if math.isfinite(value) else None
+
+  @classmethod
+  def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool:
+    prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index])
+    return prob is not None and prob > ADJACENT_LANE_LINE_PROB
+
+  @classmethod
+  def _measured_lane_width(cls, modeldata: Any) -> float:
+    left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0])
+    right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1])
+    if left_prob is None or right_prob is None:
+      return LANE_CENTER_OFFSET_M
+    if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN:
+      return LANE_CENTER_OFFSET_M
+    try:
+      lines = modeldata.laneLines
+      left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0])
+      right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0])
+    except (AttributeError, TypeError, IndexError, ValueError):
+      return LANE_CENTER_OFFSET_M
+    width = abs(right_y - left_y)
+    if not math.isfinite(width):
+      return LANE_CENTER_OFFSET_M
+    return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M)
+
+  @staticmethod
+  def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement:
+    if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block:
+      return replace(measurement, should_block=False)
+    return measurement
+
+  def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
+    self._refresh_enabled()
+    if not self.enabled:
+      if self._active:
+        self._reset()
+      self._active = False
+      return
+    if not self._active:
+      self._reset()
+      self._active = True
+
+    dt = max(float(dt_s), 0.0)
+    left_edge, left_std = self._model_side(modeldata, 0)
+    right_edge, right_std = self._model_side(modeldata, 1)
+    lane_width_m = self._measured_lane_width(modeldata)
+    self.left_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 0))
+    self.right_measurement = self._apply_lane_evidence(
+      evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m),
+      self._adjacent_lane_visible(modeldata, 1))
+    speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
+    self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
+    self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)
+    if left_fallback:
+      cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+    if right_fallback:
+      cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+
+  def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock:
+    if not self.enabled:
+      return LateralEdgeBlock.none
+    if direction == LaneChangeDirection.left and self._left.blocked:
+      return LateralEdgeBlock.left
+    if direction == LaneChangeDirection.right and self._right.blocked:
+      return LateralEdgeBlock.right
+    return LateralEdgeBlock.none

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py

@@ -0,0 +1,9 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
+
+try:
+  load_private_module(__name__, "iqpilot_private.models.big_catalog")
+except ProprietaryModuleMissing:
+  from iqpilot.models_private_src.big_catalog import *

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py

@@ -0,0 +1,70 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import numpy as np
+
+from iqpilot.common.transformations.camera import DEVICE_CAMERAS
+
+MAX_CAMERA_OFFSET_METERS = 0.35
+
+
+class _OffsetSmoother:
+  def __init__(self, blend: float = 0.1):
+    self._blend = blend
+    self._value = 0.0
+
+  def step(self, target: float) -> float:
+    self._value = ((1.0 - self._blend) * self._value) + (self._blend * float(target))
+    return self._value
+
+
+def _clamped_offset(raw_offset) -> float:
+  try:
+    parsed = float(raw_offset)
+  except (TypeError, ValueError):
+    parsed = 0.0
+  return float(np.clip(parsed, -MAX_CAMERA_OFFSET_METERS, MAX_CAMERA_OFFSET_METERS))
+
+
+def _camera_profile(sm):
+  return DEVICE_CAMERAS[(str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))]
+
+
+def _calibration_height(sm) -> float:
+  from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
+  h = sm["extrinsicsCalibration"].height[0] if sm["extrinsicsCalibration"].height else 1.22
+  return h if HEIGHT_SANE_MIN <= h <= HEIGHT_SANE_MAX else 1.22
+
+
+def _sheared_transform(model_transform, intrinsics, height: float, lateral_offset: float):
+  optical_center_y = intrinsics[1, 2]
+  projection_bias = np.eye(3, dtype=np.float32)
+  projection_bias[0, 1] = lateral_offset / height
+  projection_bias[0, 2] = -(lateral_offset / height) * optical_center_y
+  return (projection_bias @ model_transform).astype(np.float32)
+
+
+class CameraOffsetHelper:
+  def __init__(self):
+    self.camera_offset = 0.0
+    self.actual_camera_offset = 0.0
+    self._smoother = _OffsetSmoother()
+
+  @staticmethod
+  def apply_camera_offset(model_transform, intrinsics, height, offset_param):
+    return _sheared_transform(model_transform, intrinsics, height, offset_param)
+
+  def set_offset(self, offset):
+    self.camera_offset = _clamped_offset(offset)
+
+  def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera, extra_uses_wide_camera=True):
+    self.actual_camera_offset = self._smoother.step(self.camera_offset)
+    camera_bundle = _camera_profile(sm)
+    camera_height = _calibration_height(sm)
+    main_intrinsics = camera_bundle.ecam.intrinsics if main_wide_camera else camera_bundle.fcam.intrinsics
+    extra_intrinsics = camera_bundle.ecam.intrinsics if extra_uses_wide_camera else camera_bundle.fcam.intrinsics
+
+    return (
+      self.apply_camera_offset(model_transform_main, main_intrinsics, camera_height, self.actual_camera_offset),
+      self.apply_camera_offset(model_transform_extra, extra_intrinsics, camera_height, self.actual_camera_offset),
+    )

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py

@@ -0,0 +1,133 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import numpy as np
+
+
+def index_function(index: int, max_val: float = 192, max_idx: int = 32) -> float:
+  return max_val * ((index / max_idx) ** 2)
+
+
+def _quadratic_series(limit: float, steps: int) -> list[float]:
+  return [index_function(index, max_val=limit, max_idx=steps - 1) for index in range(steps)]
+
+
+def _probability_window(*values: float) -> np.ndarray:
+  return np.asarray(values, dtype=np.float32)
+
+
+def _field_group(start: int, stop: int, stride: int) -> slice:
+  return slice(start, stop, stride)
+
+
+_IDX_COUNT = 33
+_T_AXIS = _quadratic_series(10.0, _IDX_COUNT)
+_X_AXIS = _quadratic_series(192.0, _IDX_COUNT)
+
+
+class ModelConstants:
+  IDX_N = _IDX_COUNT
+  T_IDXS = _T_AXIS
+  X_IDXS = _X_AXIS
+  LEAD_T_IDXS = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]
+  LEAD_T_OFFSETS = [0.0, 2.0, 4.0]
+  META_T_IDXS = [2.0, 4.0, 6.0, 8.0, 10.0]
+
+  MODEL_FREQ = 20
+  FEATURE_LEN = 512
+  FULL_HISTORY_BUFFER_LEN = 99
+  HISTORY_BUFFER_LEN = FULL_HISTORY_BUFFER_LEN
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  NAV_FEATURE_LEN = 256
+  NAV_INSTRUCTION_LEN = 150
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  FCW_THRESHOLDS_5MS2 = _probability_window(0.05, 0.05, 0.15, 0.15, 0.15)
+  FCW_THRESHOLDS_3MS2 = _probability_window(0.7, 0.7)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  SIM_POSE_WIDTH = 6
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+  DESIRED_CURV_WIDTH = 1
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+  POLY_PATH_DEGREE = 4
+
+
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+
+class Meta:
+  ENGAGED = _field_group(0, 1, 1)
+  GAS_DISENGAGE = _field_group(1, 31, 6)
+  BRAKE_DISENGAGE = _field_group(2, 31, 6)
+  STEER_OVERRIDE = _field_group(3, 31, 6)
+  HARD_BRAKE_3 = _field_group(4, 31, 6)
+  HARD_BRAKE_4 = _field_group(5, 31, 6)
+  HARD_BRAKE_5 = _field_group(6, 31, 6)
+  GAS_PRESS = _field_group(31, 55, 4)
+  BRAKE_PRESS = _field_group(32, 55, 4)
+  LEFT_BLINKER = _field_group(33, 55, 4)
+  RIGHT_BLINKER = _field_group(34, 55, 4)
+
+
+class MetaTombRaider:
+  ENGAGED = _field_group(0, 1, 1)
+  GAS_DISENGAGE = _field_group(1, 41, 8)
+  BRAKE_DISENGAGE = _field_group(2, 41, 8)
+  STEER_OVERRIDE = _field_group(3, 41, 8)
+  HARD_BRAKE_3 = _field_group(4, 41, 8)
+  HARD_BRAKE_4 = _field_group(5, 41, 8)
+  HARD_BRAKE_5 = _field_group(6, 41, 8)
+  GAS_PRESS = _field_group(7, 41, 8)
+  BRAKE_PRESS = _field_group(8, 41, 8)
+  LEFT_BLINKER = _field_group(41, 53, 2)
+  RIGHT_BLINKER = _field_group(42, 53, 2)
+
+
+class MetaSimPose:
+  ENGAGED = _field_group(0, 1, 1)
+  GAS_DISENGAGE = _field_group(1, 36, 7)
+  BRAKE_DISENGAGE = _field_group(2, 36, 7)
+  STEER_OVERRIDE = _field_group(3, 36, 7)
+  HARD_BRAKE_3 = _field_group(4, 36, 7)
+  HARD_BRAKE_4 = _field_group(5, 36, 7)
+  HARD_BRAKE_5 = _field_group(6, 36, 7)
+  GAS_PRESS = _field_group(7, 36, 7)
+  LEFT_BLINKER = _field_group(36, 48, 2)
+  RIGHT_BLINKER = _field_group(37, 48, 2)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py

@@ -0,0 +1,767 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import sys
+import time
+from dataclasses import dataclass
+from typing import Any
+
+import iqpilot.cereal.messaging as messaging
+import numpy as np
+
+from iqpilot.cereal import car, custom, log
+from iqpilot.cereal.messaging import PubMaster, SubMaster
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionBuf, VisionIpcClient
+from iqdbc.car.car_helpers import get_demo_car_params
+from setproctitle import setproctitle
+
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.transformations.camera import DEVICE_CAMERAS
+from iqpilot.common.transformations.model import get_warp_matrix
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from iqpilot.selfdrive.controls.lib.drive_helpers import (
+  MODEL_SMOOTHING_MAX_TOTAL_SEC,
+  dynamic_lat_smooth_extra_seconds,
+  get_accel_from_plan,
+  smooth_value,
+)
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from iqpilot.system import sentry
+
+from iqpilot.common.steer_delay import lateral_action_delay
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
+from iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
+from iqpilot.selfdrive.iqmodeld.config import Plan
+from iqpilot.selfdrive.iqmodeld.messaging import (
+  DrivePacketMemory,
+  pick_curvature,
+  populate_drive_messages,
+  populate_odometry_message,
+)
+from iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
+
+try:
+  from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
+except ModuleNotFoundError:
+  class WarpContext:
+    def __init__(self, *args, **kwargs):
+      raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
+
+  class RoadProjector:
+    def __init__(self, *args, **kwargs):
+      raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
+
+
+PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.daemon"
+IQP_NAV_MODEL_INFLUENCE_ENABLED = False
+TurnDirection = custom.IQTurnSignalDirection
+IQMODEL_EVAL_WARN_US = int(DT_MDL * 1_000_000)
+IQMODEL_EVAL_ERROR_US = IQMODEL_EVAL_WARN_US * 2
+_FRAME_STARVED_BACKOFF_POLLS = 5
+_FRAME_STARVED_BACKOFF_SECONDS = 0.005
+_FRAME_STARVED_LOG_EVERY = 200
+
+
+def _plan_y_std_1s(outputs: dict[str, np.ndarray]) -> float:
+  # plan_stds is (batch, IDX_N, PLAN_WIDTH); index 10 ~= 1s ahead (see ModelConstants.T_IDXS),
+  # POSITION is an (x, y, z) slice within PLAN_WIDTH so [1] picks the lateral (y) std.
+  try:
+    return float(outputs["plan_stds"][0, 10, Plan.POSITION][1])
+  except (KeyError, IndexError):
+    return 0.0
+
+
+def _model_lat_smooth_max_sec(params: Params) -> float:
+  if not params.get_bool("ModelSmoothingEnabled"):
+    return 0.0
+  try:
+    raw = params.get("ModelLatSmoothSec", return_default=True)
+    raw = 0 if raw is None else int(raw)
+  except (ValueError, TypeError):
+    raw = 0
+  return min(max(raw, 0), 30) * 0.01
+
+
+@dataclass
+class CaptureStamp:
+  frame_id: int = 0
+  timestamp_sof: int = 0
+  timestamp_eof: int = 0
+
+  @classmethod
+  def from_vipc(cls, client: VisionIpcClient) -> "CaptureStamp":
+    return cls(client.frame_id, client.timestamp_sof, client.timestamp_eof)
+
+
+@dataclass(frozen=True)
+class StreamLayout:
+  dual_camera: bool
+  main_is_wide: bool
+  primary_stream: VisionStreamType
+
+
+class ReplayLedger:
+  def __init__(self, tensor_shapes: dict[str, tuple[int, ...]], frame_inputs: list[str]):
+    self.inputs: dict[str, np.ndarray] = {}
+    self.archive: dict[str, np.ndarray] = {}
+    self.selectors: dict[str, np.ndarray] = {}
+    self._frame_inputs = set(frame_inputs)
+    self._pulse_name: str | None = None
+    self._pulse_memory: np.ndarray | None = None
+
+    feature_shape = tensor_shapes.get("features_buffer")
+    for tensor_name, tensor_shape in tensor_shapes.items():
+      if tensor_name in self._frame_inputs:
+        continue
+
+      self.inputs[tensor_name] = np.zeros(tensor_shape, dtype=np.float32)
+      if len(tensor_shape) != 3 or tensor_shape[1] <= 1:
+        continue
+
+      history_len = self._history_length(tensor_shape, feature_shape)
+      self.archive[tensor_name] = np.zeros((1, history_len, tensor_shape[2]), dtype=np.float32)
+      export_index = self._export_index(tensor_shape, history_len, feature_shape)
+      if export_index is not None:
+        self.selectors[tensor_name] = export_index
+
+      if tensor_name.startswith("desire"):
+        self._pulse_name = tensor_name
+        self._pulse_memory = np.zeros(tensor_shape[2], dtype=np.float32)
+
+  @staticmethod
+  def _history_length(tensor_shape: tuple[int, ...], feature_shape: tuple[int, ...] | None) -> int:
+    if tensor_shape[1] >= 99:
+      return tensor_shape[1]
+    if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
+      return (feature_shape[1] + 1) * 4
+    return tensor_shape[1] * 4
+
+  @staticmethod
+  def _export_index(tensor_shape: tuple[int, ...], history_len: int,
+                    feature_shape: tuple[int, ...] | None) -> np.ndarray | None:
+    if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
+      stride = int(-history_len / tensor_shape[1])
+      return np.arange(stride, stride * (tensor_shape[1] + 1), stride)[::-1]
+    if tensor_shape[1] == 25:
+      skip = history_len // tensor_shape[1]
+      return np.arange(history_len)[-1 - (skip * (tensor_shape[1] - 1))::skip]
+    if tensor_shape[1] >= 99:
+      return np.arange(tensor_shape[1])
+    return None
+
+  @property
+  def pulse_name(self) -> str:
+    if self._pulse_name is None:
+      raise KeyError("No desire-like pulse input present in model inputs")
+    return self._pulse_name
+
+  def _shift_archive(self, tensor_name: str) -> np.ndarray:
+    history = self.archive[tensor_name]
+    history[0, :-1] = history[0, 1:]
+    return history
+
+  def inject_pulse(self, pulse_values: np.ndarray) -> None:
+    pulse = pulse_values.copy()
+    pulse[0] = 0
+    assert self._pulse_memory is not None
+    rising = np.where(pulse - self._pulse_memory > 0.99, pulse, 0)
+    self._pulse_memory[:] = pulse
+
+    history = self._shift_archive(self.pulse_name)
+    history[0, -1] = rising
+    exported_shape = self.inputs[self.pulse_name].shape
+    if history.shape[1] > exported_shape[1]:
+      stride = history.shape[1] // exported_shape[1]
+      self.inputs[self.pulse_name][:] = history[0].reshape(
+        exported_shape[0], exported_shape[1], stride, -1
+      ).max(axis=2)
+      return
+    self.inputs[self.pulse_name][:] = history[0, self.selectors[self.pulse_name]]
+
+  def merge_inputs(self, fresh_inputs: dict[str, np.ndarray]) -> None:
+    pulse_name = self.pulse_name
+    for tensor_name, tensor_value in fresh_inputs.items():
+      if tensor_name in self.inputs and tensor_name != pulse_name:
+        self.inputs[tensor_name][:] = tensor_value
+
+  def note_hidden_state(self, hidden_state: np.ndarray) -> None:
+    if "features_buffer" not in self.archive:
+      return
+    history = self._shift_archive("features_buffer")
+    history[0, -1] = hidden_state[0]
+    self.inputs["features_buffer"][:] = history[0, self.selectors["features_buffer"]]
+
+  def note_feedback(self, tensor_name: str, values: np.ndarray, zero_export: bool = False) -> None:
+    if tensor_name not in self.archive:
+      return
+    history = self._shift_archive(tensor_name)
+    history[0, -1, :] = values[0]
+    exported = history[0, self.selectors[tensor_name]]
+    self.inputs[tensor_name][:] = 0 * exported if zero_export else exported
+
+
+def _planplus_gain(vehicle_speed: float) -> float:
+  return 0.75 if vehicle_speed >= 25.0 else 1.0
+
+
+def _merged_plan(runtime_state: "NeuralEngineState", outputs: dict[str, np.ndarray], vehicle_speed: float) -> np.ndarray:
+  base_plan = outputs["plan"][0]
+  if "planplus" not in outputs:
+    return base_plan
+  return base_plan + (runtime_state.PLANPLUS_CONTROL * _planplus_gain(vehicle_speed)) * outputs["planplus"][0]
+
+
+class NeuralEngineState(InferenceStateBase):
+  frames: dict[str, RoadProjector]
+
+  def __init__(self, gpu_context: WarpContext):
+    super().__init__()
+    runner = get_model_runner()
+    bundle = get_active_bundle()
+
+    self.model_runner = runner
+    self.constants = runner.constants
+    self.generation = bundle.generation if bundle is not None else None
+
+    knob_values = {entry.key: entry.value for entry in bundle.overrides} if bundle is not None else {}
+    self.LAT_SMOOTH_SECONDS = float(knob_values.get("lat", ".0"))
+    self.LONG_SMOOTH_SECONDS = float(knob_values.get("long", ".0"))
+    self.MIN_LAT_CONTROL_SPEED = 0.3
+    self.PLANPLUS_CONTROL = 1.0
+    self.model_smoothing_max_extra_sec = 0.0
+
+    context_depth = 5 if runner.is_20hz else 2
+    self.frames = {
+      stream_name: RoadProjector(gpu_context, context_depth)
+      for stream_name in runner.vision_input_names
+    }
+
+    self._ledger = ReplayLedger(runner.input_shapes, runner.vision_input_names)
+    self.numpy_inputs = self._ledger.inputs
+    self.temporal_buffers = self._ledger.archive
+    self.temporal_idxs_map = self._ledger.selectors
+
+  @property
+  def mlsim(self) -> bool:
+    return bool(self.generation is not None and self.generation >= 11)
+
+  @property
+  def desire_key(self) -> str:
+    return self._ledger.pulse_name
+
+  def _warp_frames(self, vision_bufs: dict[str, VisionBuf],
+                   transform_map: dict[str, np.ndarray]) -> dict[str, Any]:
+    return {
+      stream_name: self.frames[stream_name].stage(vision_bufs[stream_name], transform_map[stream_name].flatten())
+      for stream_name in self.model_runner.vision_input_names
+    }
+
+  def _run_split_model(self) -> dict[str, np.ndarray]:
+    if hasattr(self.model_runner, "run_vision"):
+      vision_packet = self.model_runner.run_vision()
+      self._ledger.note_hidden_state(vision_packet["hidden_state"])
+      self.model_runner.refresh_policy_features(self.numpy_inputs["features_buffer"])
+      return {**vision_packet, **self.model_runner.run_policy()}
+
+    result = self.model_runner.run_model()
+    if "hidden_state" in result:
+      self._ledger.note_hidden_state(result["hidden_state"])
+    return result
+
+  def _write_curvature_memory(self, outputs: dict[str, np.ndarray]) -> None:
+    if "desired_curvature" not in outputs:
+      return
+
+    feedback_slot = None
+    if "prev_desired_curvs" in self.numpy_inputs:
+      feedback_slot = "prev_desired_curvs"
+    elif "prev_desired_curv" in self.numpy_inputs:
+      feedback_slot = "prev_desired_curv"
+
+    if feedback_slot is not None:
+      self._ledger.note_feedback(feedback_slot, outputs["desired_curvature"], zero_export=self.mlsim)
+
+  def run(self, vision_bufs: dict[str, VisionBuf], transform_map: dict[str, np.ndarray],
+          fresh_inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
+    if not getattr(self.model_runner, "uses_opencl_warp", True):
+      return self.model_runner.run_fused(vision_bufs, transform_map, fresh_inputs)
+
+    self._ledger.inject_pulse(fresh_inputs[self.desire_key])
+    self._ledger.merge_inputs(fresh_inputs)
+    warped_frames = self._warp_frames(vision_bufs, transform_map)
+    self.model_runner.prepare_inputs(warped_frames, self.numpy_inputs, self.frames)
+
+    outputs = self._run_split_model()
+    self._write_curvature_memory(outputs)
+    return outputs
+
+  def get_action_from_model(self, outputs: dict[str, np.ndarray], previous_action: log.ModelDataV2.Action,
+                            lat_action_t: float, long_action_t: float, vehicle_speed: float,
+                            lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
+    if lat_smooth_seconds is None:
+      lat_smooth_seconds = self.LAT_SMOOTH_SECONDS
+
+    if "action" in outputs:
+      curvature_cmd = outputs["action"][0, 0] / (max(1.0, vehicle_speed)) ** 2
+      accel_cmd = outputs["action"][0, 1]
+      should_stop = bool(vehicle_speed < 0.3 and accel_cmd < 0.1)
+
+      accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
+      if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
+        curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
+      else:
+        curvature_cmd = previous_action.desiredCurvature
+
+      return log.ModelDataV2.Action(
+        desiredCurvature=float(curvature_cmd),
+        desiredAcceleration=float(accel_cmd),
+        shouldStop=should_stop,
+      )
+
+    plan_rows = _merged_plan(self, outputs, vehicle_speed)
+    accel_cmd, should_stop = get_accel_from_plan(
+      plan_rows[:, Plan.VELOCITY][:, 0],
+      plan_rows[:, Plan.ACCELERATION][:, 0],
+      self.constants.T_IDXS,
+      action_t=long_action_t,
+    )
+    accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
+
+    curvature_cmd = pick_curvature(outputs, plan_rows, vehicle_speed, lat_action_t, self.mlsim)
+    if self.generation is not None and self.generation >= 10:
+      if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
+        curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
+      else:
+        curvature_cmd = previous_action.desiredCurvature
+
+    return log.ModelDataV2.Action(
+      desiredCurvature=float(curvature_cmd),
+      desiredAcceleration=float(accel_cmd),
+      shouldStop=bool(should_stop),
+    )
+
+
+class CameraIngress:
+  def __init__(self, gpu_context: WarpContext):
+    self.layout = self._discover_layout()
+    self._primary = VisionIpcClient("camerad", self.layout.primary_stream, True, gpu_context)
+    self._secondary = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, gpu_context)
+
+    while not self._primary.connect(False):
+      time.sleep(0.1)
+    while self.layout.dual_camera and not self._secondary.connect(False):
+      time.sleep(0.1)
+
+    cloudlog.warning(
+      f"connected main cam with buffer size: {self._primary.buffer_len} ({self._primary.width} x {self._primary.height})"
+    )
+    if self.layout.dual_camera:
+      cloudlog.warning(
+        f"connected extra cam with buffer size: {self._secondary.buffer_len} ({self._secondary.width} x {self._secondary.height})"
+      )
+
+  @staticmethod
+  def _discover_layout() -> StreamLayout:
+    while True:
+      available = VisionIpcClient.available_streams("camerad", block=False)
+      if available:
+        dual_camera = (
+          VisionStreamType.VISION_STREAM_WIDE_ROAD in available
+          and VisionStreamType.VISION_STREAM_ROAD in available
+        )
+        main_is_wide = VisionStreamType.VISION_STREAM_ROAD not in available
+        primary_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_is_wide else VisionStreamType.VISION_STREAM_ROAD
+        cloudlog.warning(
+          f"vision stream set up, main_wide_camera: {main_is_wide}, use_extra_client: {dual_camera}"
+        )
+        return StreamLayout(dual_camera=dual_camera, main_is_wide=main_is_wide, primary_stream=primary_stream)
+      time.sleep(0.1)
+
+  def pull(self) -> tuple[VisionBuf, VisionBuf, CaptureStamp, CaptureStamp] | None:
+    main_buf = None
+    wide_buf = None
+    main_stamp = CaptureStamp()
+    wide_stamp = CaptureStamp()
+
+    while main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
+      main_buf = self._primary.recv()
+      main_stamp = CaptureStamp.from_vipc(self._primary)
+      if main_buf is None:
+        return None
+
+    if not self.layout.dual_camera:
+      return main_buf, main_buf, main_stamp, main_stamp
+
+    while True:
+      wide_buf = self._secondary.recv()
+      wide_stamp = CaptureStamp.from_vipc(self._secondary)
+      if wide_buf is None or main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
+        break
+
+    if wide_buf is None:
+      return None
+
+    if abs(main_stamp.timestamp_sof - wide_stamp.timestamp_sof) > 10000000:
+      cloudlog.error(
+        f"frames out of sync! main: {main_stamp.frame_id} ({main_stamp.timestamp_sof / 1e9:.5f}),"
+        f" extra: {wide_stamp.frame_id} ({wide_stamp.timestamp_sof / 1e9:.5f})"
+      )
+    return main_buf, wide_buf, main_stamp, wide_stamp
+
+
+class CalibrationAtlas:
+  def __init__(self):
+    self.main_warp = np.zeros((3, 3), dtype=np.float32)
+    self.extra_warp = np.zeros((3, 3), dtype=np.float32)
+    self.ready = False
+    self._offset_tuner = CameraOffsetHelper()
+
+  def set_offset(self, offset_value: Any) -> None:
+    self._offset_tuner.set_offset(offset_value)
+
+  def refresh(self, sm: SubMaster, main_is_wide: bool, dual_camera: bool) -> tuple[np.ndarray, np.ndarray, bool]:
+    if not (sm.seen["extrinsicsCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
+      return self.main_warp, self.extra_warp, self.ready
+
+    rpy = get_calibrated_rpy(sm["extrinsicsCalibration"])
+    if rpy is None:
+      live_calib = sm["extrinsicsCalibration"]
+      if len(live_calib.rpyCalib) == 3:
+        rpy = np.array(live_calib.rpyCalib, dtype=np.float32)
+      else:
+        rpy = np.zeros(3, dtype=np.float32)
+
+    device_key = (str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))
+    device_camera = DEVICE_CAMERAS[device_key]
+    main_intrinsics = device_camera.ecam.intrinsics if main_is_wide else device_camera.fcam.intrinsics
+    extra_uses_wide_camera = dual_camera or main_is_wide
+    extra_intrinsics = device_camera.ecam.intrinsics if extra_uses_wide_camera else device_camera.fcam.intrinsics
+    self.main_warp = get_warp_matrix(rpy, main_intrinsics, False).astype(np.float32)
+    self.extra_warp = get_warp_matrix(rpy, extra_intrinsics, True).astype(np.float32)
+    self.main_warp, self.extra_warp = self._offset_tuner.update(
+      self.main_warp, self.extra_warp, sm, main_is_wide, extra_uses_wide_camera
+    )
+    self.ready = True
+    return self.main_warp, self.extra_warp, self.ready
+
+
+class FrameDropMeter:
+  def __init__(self, model_freq: float):
+    self._smoother = FirstOrderFilter(0.0, 10.0, 1.0 / model_freq)
+    self._warm_frames = 0
+    self._last_frame_id = 0
+
+  def sample(self, frame_id: int) -> tuple[int, float, bool]:
+    dropped = max(0, frame_id - self._last_frame_id - 1)
+    smooth = self._smoother.update(min(dropped, 10))
+    if self._warm_frames < 10:
+      self._smoother.x = 0.0
+      smooth = 0.0
+    self._warm_frames += 1
+    return dropped, smooth / (1 + smooth), dropped > 0
+
+  def commit(self, frame_id: int) -> None:
+    self._last_frame_id = frame_id
+
+
+class InferenceDaemon:
+  def __init__(self, demo: bool = False, channel_path: str | None = None):
+    cloudlog.warning("iqmodeld init")
+    sentry.set_tag("daemon", PROCESS_NAME)
+    cloudlog.bind(daemon=PROCESS_NAME)
+    setproctitle(PROCESS_NAME)
+    config_realtime_process(7, 54)
+
+    cloudlog.warning("setting up CL context")
+    self._gpu = WarpContext()
+    cloudlog.warning("CL context ready; loading model")
+    self._runtime = NeuralEngineState(self._gpu)
+    self._meta_layout = select_meta_layout()
+    cloudlog.warning("models loaded, iqmodeld starting")
+
+    self._channel = None
+    if channel_path is not None:
+      from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel
+      self._channel = ModelChannel(channel_path, create=True)
+
+    self._cameras = CameraIngress(self._gpu)
+    pub_services = ["iqPerfTrace"] if self._channel is not None else [
+      "modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"]
+    self._pub = PubMaster(pub_services)
+    self._sub = SubMaster([
+      "deviceState", "carState", "roadCameraState", "extrinsicsCalibration",
+      "driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState",
+    ])
+    self._message_memory = DrivePacketMemory()
+    self._params = Params()
+    self._frame_meter = FrameDropMeter(self._runtime.constants.MODEL_FREQ)
+    self._warps = CalibrationAtlas()
+    self._perf = PerfTraceEmitter("iqmodeld", pubmaster=self._pub)
+    self._perf_ring = PerfTraceRing()
+
+    self._car_params = self._load_car_params(demo)
+    self._long_action_delay = self._car_params.longitudinalActuatorDelay + self._runtime.LONG_SMOOTH_SECONDS
+    self._previous_action = log.ModelDataV2.Action()
+    self._desire_logic = DesireHelper()
+    self._lat_smooth_extra_sec = 0.0
+
+  def _load_car_params(self, demo: bool):
+    car_params = get_demo_car_params() if demo else messaging.log_from_bytes(
+      self._params.get("CarParams", block=True), car.CarParams)
+    cloudlog.info("iqmodeld got CarParams: %s", car_params.brand)
+    return car_params
+
+  def _refresh_tunables(self, tick: int) -> None:
+    if tick % 60 != 0:
+      return
+    from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
+    big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params)
+    if big_enabled != (self._channel is not None):
+      # publish mode is fixed at startup: staying up would fight the selector for modelV2
+      cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode")
+      sys.exit(0)
+    self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
+    self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
+    self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
+    self._warps.set_offset(self._params.get("CameraOffset", return_default=True))
+
+  def _traffic_side(self) -> np.ndarray:
+    traffic = np.zeros(2, dtype=np.float32)
+    traffic[int(self._sub["driverMonitoringState"].isRHD)] = 1
+    return traffic
+
+  def _desire_pulse(self) -> np.ndarray:
+    pulse = np.zeros(self._runtime.constants.DESIRE_LEN, dtype=np.float32)
+    desire_idx = self._desire_logic.desire
+    if 0 <= desire_idx < self._runtime.constants.DESIRE_LEN:
+      pulse[desire_idx] = 1
+    return pulse
+
+  def _compose_inputs(self, vehicle_speed: float, lat_horizon: float, long_horizon: float) -> dict[str, np.ndarray]:
+    inputs: dict[str, np.ndarray] = {
+      self._runtime.desire_key: self._desire_pulse(),
+      "traffic_convention": self._traffic_side(),
+    }
+    if "lateral_control_params" in self._runtime.numpy_inputs:
+      inputs["lateral_control_params"] = np.array([vehicle_speed, lat_horizon], dtype=np.float32)
+    if "action_t" in self._runtime.numpy_inputs:
+      inputs["action_t"] = np.array([lat_horizon, long_horizon], dtype=np.float32)
+    return inputs
+
+  def _publish(self, outputs: dict[str, np.ndarray], main_stamp: CaptureStamp, extra_stamp: CaptureStamp,
+               road_frame_id: int, frame_drop_ratio: float, dropped_frames: int,
+               execution_time: float, live_calib_seen: bool,
+               lat_horizon: float, long_horizon: float, vehicle_speed: float) -> None:
+    model_msg = messaging.new_message("modelV2")
+    driving_msg = messaging.new_message("drivingModelData")
+    pose_msg = messaging.new_message("cameraOdometry")
+    iq_msg = messaging.new_message("iqDriveModelData")
+
+    self._lat_smooth_extra_sec = dynamic_lat_smooth_extra_seconds(
+      _plan_y_std_1s(outputs), self._runtime.model_smoothing_max_extra_sec
+    )
+    lat_smooth_total_sec = min(self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec, MODEL_SMOOTHING_MAX_TOTAL_SEC)
+    action = self._runtime.get_action_from_model(
+      outputs, self._previous_action, lat_horizon, long_horizon, vehicle_speed, lat_smooth_total_sec
+    )
+    self._previous_action = action
+
+    populate_drive_messages(
+      driving_msg,
+      model_msg,
+      outputs,
+      action,
+      self._message_memory,
+      main_stamp.frame_id,
+      extra_stamp.frame_id,
+      road_frame_id,
+      frame_drop_ratio,
+      main_stamp.timestamp_eof,
+      execution_time,
+      live_calib_seen,
+      self._meta_layout,
+    )
+
+    desire_state = model_msg.modelV2.meta.desireState
+    lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight]
+    self._desire_logic.update(
+      self._sub["carState"],
+      self._sub["carControl"].latActive,
+      lane_change_prob,
+      self._sub["iqNavState"],
+      model_msg.modelV2,
+      self._sub["radarState"],
+    )
+    model_msg.modelV2.meta.laneChangeState = self._desire_logic.lane_change_state
+    model_msg.modelV2.meta.laneChangeDirection = self._desire_logic.lane_change_direction
+    driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state
+    driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction
+    iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction
+    iq_msg.iqDriveModelData.lateralEdgeBlock = self._desire_logic.lateral_edge_block
+
+    populate_odometry_message(
+      pose_msg,
+      outputs,
+      main_stamp.frame_id,
+      dropped_frames,
+      main_stamp.timestamp_eof,
+      live_calib_seen,
+    )
+
+    if self._channel is not None:
+      self._channel.write(main_stamp.frame_id, {
+        "source": "small",
+        "frame_id": main_stamp.frame_id,
+        "timestamp_sof": int(main_stamp.timestamp_sof),
+        "live_calib_seen": bool(live_calib_seen),
+        "model_execution_time": float(execution_time),
+        "msgs": {
+          "modelV2": model_msg.to_bytes(),
+          "drivingModelData": driving_msg.to_bytes(),
+          "cameraOdometry": pose_msg.to_bytes(),
+          "iqDriveModelData": iq_msg.to_bytes(),
+        },
+      })
+      return
+
+    self._pub.send("modelV2", model_msg)
+    self._pub.send("drivingModelData", driving_msg)
+    self._pub.send("cameraOdometry", pose_msg)
+    self._pub.send("iqDriveModelData", iq_msg)
+
+  def serve(self) -> None:
+    tick = 0
+    starved_polls = 0
+    while True:
+      frame_pair = self._cameras.pull()
+      if frame_pair is None:
+        starved_polls += 1
+        if starved_polls >= _FRAME_STARVED_BACKOFF_POLLS:
+          time.sleep(_FRAME_STARVED_BACKOFF_SECONDS)
+        if starved_polls % _FRAME_STARVED_LOG_EVERY == 0:
+          cloudlog.error(f"visionipc delivered no frames for {starved_polls} polls; model is not running")
+        continue
+
+      if starved_polls:
+        cloudlog.warning(f"visionipc recovered after {starved_polls} frameless polls")
+        starved_polls = 0
+
+      main_buf, extra_buf, main_stamp, extra_stamp = frame_pair
+      self._sub.update(0)
+      self._refresh_tunables(tick)
+
+      vehicle_speed = max(self._sub["carState"].vEgo, 0.0)
+      lat_horizon = self._runtime.lat_delay + self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec + DT_MDL
+      long_horizon = self._long_action_delay + DT_MDL
+
+      main_warp, extra_warp, live_calib_seen = self._warps.refresh(
+        self._sub, self._cameras.layout.main_is_wide, self._cameras.layout.dual_camera
+      )
+      dropped_frames, frame_drop_ratio, prepare_only = self._frame_meter.sample(main_stamp.frame_id)
+
+      vision_bufs = {
+        stream_name: extra_buf if "big" in stream_name else main_buf
+        for stream_name in self._runtime.model_runner.vision_input_names
+      }
+      warp_map = {
+        stream_name: extra_warp if "big" in stream_name else main_warp
+        for stream_name in self._runtime.model_runner.vision_input_names
+      }
+      fresh_inputs = self._compose_inputs(vehicle_speed, lat_horizon, long_horizon)
+
+      started_at = time.perf_counter()
+      outputs = self._runtime.run(vision_bufs, warp_map, fresh_inputs)
+      execution_time = time.perf_counter() - started_at
+      execution_us = int(execution_time * 1_000_000)
+
+      sample = PerfSample(
+        frame_id=main_stamp.frame_id,
+        model_eval_us=execution_us,
+        model_dropped_frames=dropped_frames,
+        model_backlog=max(0, dropped_frames),
+      )
+      self._perf_ring.push(sample)
+      if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_WARN_US:
+        severity = "warning"
+        if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_ERROR_US:
+          severity = "error"
+        self._perf.emit(
+          "iqmodeld_dropped_frames" if dropped_frames > 0 else "iqmodeld_slow_eval",
+          severity=severity,
+          frame_id=main_stamp.frame_id,
+          total_time_us=execution_us,
+          dropped_frames=dropped_frames,
+          backlog=max(0, dropped_frames),
+          samples=self._perf_ring.snapshot(),
+          detail=(
+            f"model_eval_us={execution_us} dropped_frames={dropped_frames} prepare_only={int(prepare_only)} "
+            f"road_frame_id={self._sub['roadCameraState'].frameId}"
+          ),
+          min_interval_s=0.25,
+        )
+
+      if outputs is not None:
+        self._publish(
+          outputs,
+          main_stamp,
+          extra_stamp,
+          self._sub["roadCameraState"].frameId,
+          frame_drop_ratio,
+          dropped_frames,
+          execution_time,
+          live_calib_seen,
+          lat_horizon,
+          long_horizon,
+          vehicle_speed,
+        )
+
+      self._frame_meter.commit(main_stamp.frame_id)
+      tick += 1
+
+
+def main(demo: bool = False, channel_path: str | None = "auto"):
+  if channel_path == "auto":
+    channel_path = None
+    params = Params()
+    from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
+    if params.get_bool("IQEmacEnabled") or egpu_selected(params):
+      from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL
+      channel_path = SMALL_CHANNEL
+  InferenceDaemon(demo=demo, channel_path=channel_path).serve()
+
+
+__all__ = [
+  "PROCESS_NAME",
+  "IQP_NAV_MODEL_INFLUENCE_ENABLED",
+  "TurnDirection",
+  "CaptureStamp",
+  "ReplayLedger",
+  "NeuralEngineState",
+  "CameraIngress",
+  "CalibrationAtlas",
+  "FrameDropMeter",
+  "InferenceDaemon",
+  "main",
+]
+
+
+if __name__ == "__main__":
+  try:
+    import argparse
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--demo", action="store_true", help="Run iqmodeld in demo mode.")
+    args = parser.parse_args()
+    main(demo=args.demo)
+  except KeyboardInterrupt:
+    cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
+  except Exception:
+    sentry.capture_exception()
+    raise

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py

@@ -0,0 +1,46 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import numpy as np
+
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import smooth_value
+
+LAT_SMOOTH_SECONDS = 0.0
+LONG_SMOOTH_SECONDS = 0.3
+MIN_LAT_CONTROL_SPEED = 0.3
+DESIRE_LEN = 8
+
+
+def get_action_from_model(outputs: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
+                          v_ego: float, lat_action_t: float, long_action_t: float,
+                          lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
+  if "action" in outputs:
+    desired_accel = float(outputs["action"][0, 1])
+    desired_curvature = float(outputs["action"][0, 0]) / (max(1.0, v_ego)) ** 2
+    should_stop = bool(v_ego < 0.3 and desired_accel < 0.1)
+  else:
+    from iqpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan
+    from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
+    plan = outputs["plan"][0]
+    desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0],
+                                                     plan[:, Plan.ACCELERATION][:, 0],
+                                                     ModelConstants.T_IDXS,
+                                                     action_t=long_action_t)
+    desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2],
+                                                plan[:, Plan.ORIENTATION_RATE][:, 2],
+                                                ModelConstants.T_IDXS, v_ego, lat_action_t)
+    desired_accel, should_stop = float(desired_accel), bool(should_stop)
+    desired_curvature = float(desired_curvature)
+  desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
+  if v_ego > MIN_LAT_CONTROL_SPEED:
+    lat_smooth = LAT_SMOOTH_SECONDS if lat_smooth_seconds is None else lat_smooth_seconds
+    desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth)
+  else:
+    desired_curvature = prev_action.desiredCurvature
+  return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
+                                desiredAcceleration=float(desired_accel),
+                                shouldStop=should_stop)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py

@@ -0,0 +1,222 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import hashlib
+import json
+import os
+
+from iqpilot.common.swaglog import cloudlog
+import urllib.request
+from pathlib import Path
+
+from iqpilot.system.hardware.usb import egpu_dock_ready
+
+USB_SYSFS_ROOT = "/sys/bus/usb/devices"
+FIRMWARE_MIRROR = os.getenv("IQ_EGPU_FIRMWARE_MIRROR", "/data/firmware/tinygrad")
+TINYGRAD_CACHE = "/data/.cache"
+
+COMMA_LFS_BATCH_URL = "https://gitlab.com/commaai/openpilot-lfs.git/info/lfs/objects/batch"
+
+DOWNLOAD_CHUNK = 4 * 1024 * 1024
+
+
+def usbgpu_present(sysfs_root: str = USB_SYSFS_ROOT) -> bool:
+  return egpu_dock_ready(Path(sysfs_root))
+
+
+def egpu_present_consented(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
+  try:
+    if params is not None and params.get_bool("IQEgpuDisabled"):
+      return False
+  except Exception:
+    pass
+  return usbgpu_present(sysfs_root)
+
+
+def egpu_selected(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
+  try:
+    if params is not None and params.get_bool("IQEgpuDisabled"):
+      return False
+    if params is not None and params.get_bool("IQEgpuEnabled"):
+      return True
+  except Exception:
+    pass
+  return usbgpu_present(sysfs_root)
+
+
+def resolve_backend(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> str | None:
+  if egpu_present:
+    return "egpu"
+  if emac_enabled:
+    return "emac"
+  if egpu_enabled:
+    return "egpu"
+  return None
+
+
+def egpu_pkl_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_tinygrad.pkl")
+
+
+def egpu_policy_pkl_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy.pkl")
+
+
+def egpu_oob_pkl_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy_oob.pkl")
+
+
+def egpu_model_oob_pkl_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_model_oob.pkl")
+
+
+def onnx_cache_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx")
+
+
+def _sha256_file(path: str) -> str:
+  digest = hashlib.sha256()
+  with open(path, "rb") as f:
+    while chunk := f.read(DOWNLOAD_CHUNK):
+      digest.update(chunk)
+  return digest.hexdigest()
+
+
+def quarantine_artifact(path: str, why: str) -> None:
+  try:
+    if os.path.isfile(path):
+      os.replace(path, path + ".unusable")
+  except OSError:
+    try:
+      os.remove(path)
+    except OSError:
+      pass
+
+
+def local_onnx(meta: dict) -> str | None:
+  path = onnx_cache_path(meta)
+  if not os.path.isfile(path):
+    return None
+  size = int(meta.get("download", {}).get("size", 0))
+  if size and os.path.getsize(path) != size:
+    quarantine_artifact(path, "onnx size mismatch")
+    return None
+  if _sha256_file(path) != meta["sha256"]:
+    quarantine_artifact(path, "onnx sha256 mismatch")
+    return None
+  return path
+
+
+def resolve_download_url(download_url: str, sha256: str, size: int, timeout: float = 30.0) -> str:
+  if download_url.startswith("commalfs:"):
+    oid = download_url.split(":", 1)[1]
+    body = json.dumps({"operation": "download", "transfers": ["basic"],
+                       "objects": [{"oid": oid, "size": size}]}).encode()
+    req = urllib.request.Request(COMMA_LFS_BATCH_URL, data=body, headers={
+      "Accept": "application/vnd.git-lfs+json", "Content-Type": "application/vnd.git-lfs+json"})
+    with urllib.request.urlopen(req, timeout=timeout) as r:
+      d = json.load(r)
+    return d["objects"][0]["actions"]["download"]["href"]
+  return download_url
+
+
+def download_onnx(meta: dict, progress_cb=None) -> str:
+  from iqpilot.selfdrive.iqmodeld.egpu_model import download_descriptor
+  download_url, size = download_descriptor(meta)
+  if not download_url:
+    raise RuntimeError(f"model {meta['key']} has no download source; stage the onnx at {onnx_cache_path(meta)}")
+
+  path = onnx_cache_path(meta)
+  os.makedirs(os.path.dirname(path), exist_ok=True)
+  try:
+    from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file
+    return download_hf_file(f"onnx/{meta['sha256']}.onnx", path, meta["sha256"], int(size or 0), progress_cb=progress_cb)
+  except Exception as e:
+    cloudlog.warning(f"onnx {meta['key']} unavailable from HF ({e}); falling back to {download_url.split(':', 1)[0]}")
+  url = resolve_download_url(download_url, meta["sha256"], size)
+  tmp = path + ".part"
+  digest = hashlib.sha256()
+  got = 0
+  with urllib.request.urlopen(url, timeout=60) as r, open(tmp, "wb") as f:
+    while chunk := r.read(DOWNLOAD_CHUNK):
+      f.write(chunk)
+      digest.update(chunk)
+      got += len(chunk)
+      if progress_cb is not None and size:
+        progress_cb(got / size)
+  if size and got != size:
+    os.remove(tmp)
+    raise RuntimeError(f"onnx download truncated: {got}/{size} bytes")
+  if digest.hexdigest() != meta["sha256"]:
+    os.remove(tmp)
+    raise RuntimeError(f"onnx sha256 mismatch for {meta['key']}")
+  os.replace(tmp, path)
+  return path
+
+
+ARTIFACT_PATHS = {
+  "egpu_model_oob_artifact": egpu_model_oob_pkl_path,
+  "egpu_oob_artifact": egpu_oob_pkl_path,
+  "egpu_policy_artifact": egpu_policy_pkl_path,
+  "egpu_artifact": egpu_pkl_path,
+}
+
+
+def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False, field: str | None = None) -> str | None:
+  field = field or ("egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact")
+  art = meta.get(field)
+  if not art or not (art.get("objects") or art.get("hf_path")):
+    return None
+  from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file, download_lfs_bundle
+  dest = ARTIFACT_PATHS[field](meta)
+  if art.get("hf_path"):
+    try:
+      return download_hf_file(art["hf_path"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb)
+    except Exception as e:
+      cloudlog.warning(f"precompiled {meta['key']} unavailable from HF ({e}); trying LFS")
+      if not art.get("objects"):
+        raise
+  return download_lfs_bundle(art["objects"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb)
+
+
+def patch_tinygrad_fetch_fw() -> None:
+  import pathlib
+
+  import zstandard
+  from tinygrad import helpers
+  if getattr(helpers.fetch_fw, "_iq_patched", False):
+    return
+  _orig = helpers.fetch_fw
+
+  def fetch_fw(path, name, sha256):
+    mirror = pathlib.Path(FIRMWARE_MIRROR) / path / name
+    if mirror.is_file():
+      blob = mirror.read_bytes()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    blob = _orig(path, name, sha256)
+    # The dock's GPU firmware otherwise lives only in tinygrad's per-user download cache, which is
+    # a network fetch the first time a new HOME sees it; onroad the car is usually offline.
+    try:
+      mirror.parent.mkdir(parents=True, exist_ok=True)
+      tmp = mirror.with_suffix(mirror.suffix + ".part")
+      tmp.write_bytes(blob)
+      os.replace(tmp, mirror)
+    except OSError:
+      pass
+    return blob
+
+  fetch_fw._iq_patched = True
+  helpers.fetch_fw = fetch_fw

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py

@@ -0,0 +1,9 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
+
+try:
+  load_private_module(__name__, "iqpilot_private.models.egpu_model")
+except ProprietaryModuleMissing:
+  from iqpilot.models_private_src.egpu_model import *

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py

@@ -0,0 +1,9 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
+
+try:
+  load_private_module(__name__, "iqpilot_private.models.emac_model_meta")
+except ProprietaryModuleMissing:
+  from iqpilot.models_private_src.emac_model_meta import *

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py

@@ -0,0 +1,272 @@
+from __future__ import annotations
+
+import os
+from dataclasses import dataclass, field
+
+import capnp
+import numpy as np
+
+from iqpilot.cereal import log
+from iqpilot.selfdrive.iqmodeld.models.helpers import plan_x_idxs_helper
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
+from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
+
+SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
+ConfidenceClass = log.ModelDataV2.ConfidenceClass
+
+
+def pick_curvature(outputs: dict[str, np.ndarray], plan_rows: np.ndarray, vehicle_speed: float,
+                   action_horizon: float, synthetic_lane_logic: bool) -> float:
+  direct_signal = None if synthetic_lane_logic else outputs.get("desired_curvature")
+  if direct_signal is not None:
+    return float(direct_signal[0, 0])
+
+  yaw_track = plan_rows[:, Plan.T_FROM_CURRENT_EULER][:, 2]
+  yaw_rate_track = plan_rows[:, Plan.ORIENTATION_RATE][:, 2]
+  return float(get_curvature_from_plan(yaw_track, yaw_rate_track, ModelConstants.T_IDXS, vehicle_speed, action_horizon))
+
+
+@dataclass
+class DrivePacketMemory:
+  disengage_rollup: np.ndarray = field(default_factory=lambda: np.zeros(
+    ModelConstants.CONFIDENCE_BUFFER_LEN * ModelConstants.DISENGAGE_WIDTH, dtype=np.float32))
+  brake_watch_5: np.ndarray = field(default_factory=lambda: np.zeros(
+    ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32))
+  brake_watch_3: np.ndarray = field(default_factory=lambda: np.zeros(
+    ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32))
+
+
+def _assign_xyz(builder, t_points, x_track, y_track, z_track,
+                x_std=None, y_std=None, z_std=None) -> None:
+  builder.t = t_points
+  builder.x = x_track.tolist()
+  builder.y = y_track.tolist()
+  builder.z = z_track.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if z_std is not None:
+    builder.zStd = z_std.tolist()
+
+
+def _assign_xyva(builder, t_points, x_track, y_track, v_track, a_track,
+                 x_std=None, y_std=None, v_std=None, a_std=None) -> None:
+  builder.t = t_points
+  builder.x = x_track.tolist()
+  builder.y = y_track.tolist()
+  builder.v = v_track.tolist()
+  builder.a = a_track.tolist()
+  if x_std is not None:
+    builder.xStd = x_std.tolist()
+  if y_std is not None:
+    builder.yStd = y_std.tolist()
+  if v_std is not None:
+    builder.vStd = v_std.tolist()
+  if a_std is not None:
+    builder.aStd = a_std.tolist()
+
+
+def fill_xyz_poly(builder, degree: int, x_track: np.ndarray, y_track: np.ndarray, z_track: np.ndarray) -> None:
+  stacked = np.stack([x_track, y_track, z_track], axis=1)
+  coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, stacked, deg=degree)
+  builder.xCoefficients = coeffs[:, 0].tolist()
+  builder.yCoefficients = coeffs[:, 1].tolist()
+  builder.zCoefficients = coeffs[:, 2].tolist()
+
+
+def fill_lane_line_meta(builder, lane_lines, lane_probs: list[float]) -> None:
+  builder.leftY = lane_lines[1].y[0]
+  builder.leftProb = lane_probs[1]
+  builder.rightY = lane_lines[2].y[0]
+  builder.rightProb = lane_probs[2]
+
+
+def _roll_brake_watch(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout) -> bool:
+  memory.brake_watch_5[:-1] = memory.brake_watch_5[1:]
+  memory.brake_watch_5[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_5][0]
+  memory.brake_watch_3[:-1] = memory.brake_watch_3[1:]
+  memory.brake_watch_3[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_3][0]
+  return bool(
+    (memory.brake_watch_5 > ModelConstants.FCW_THRESHOLDS_5MS2).all()
+    and (memory.brake_watch_3 > ModelConstants.FCW_THRESHOLDS_3MS2).all()
+  )
+
+
+def _confidence_bucket(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int):
+  width = ModelConstants.DISENGAGE_WIDTH
+  if frame_id % (2 * ModelConstants.MODEL_FREQ) == 0:
+    brake_probs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE]
+    gas_probs = outputs["meta"][0, meta_layout.GAS_DISENGAGE]
+    steer_probs = outputs["meta"][0, meta_layout.STEER_OVERRIDE]
+    takeover_curve = 1 - ((1 - brake_probs) * (1 - gas_probs) * (1 - steer_probs))
+    independent = np.r_[takeover_curve[0], np.diff(takeover_curve) / (1 - takeover_curve[:-1])]
+    memory.disengage_rollup[:-width] = memory.disengage_rollup[width:]
+    memory.disengage_rollup[-width:] = independent
+
+  score = 0.0
+  for idx in range(width):
+    score += memory.disengage_rollup[idx * width + width - 1 - idx].item() / width
+
+  if score < ModelConstants.RYG_GREEN:
+    return ConfidenceClass.green
+  if score < ModelConstants.RYG_YELLOW:
+    return ConfidenceClass.yellow
+  return ConfidenceClass.red
+
+
+def _write_plan_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
+  plan_rows = outputs["plan"][0]
+  plan_stds = outputs["plan_stds"][0]
+  _assign_xyz(model_packet.position, ModelConstants.T_IDXS, *plan_rows[:, Plan.POSITION].T, *plan_stds[:, Plan.POSITION].T)
+  _assign_xyz(model_packet.velocity, ModelConstants.T_IDXS, *plan_rows[:, Plan.VELOCITY].T)
+  _assign_xyz(model_packet.acceleration, ModelConstants.T_IDXS, *plan_rows[:, Plan.ACCELERATION].T)
+  _assign_xyz(model_packet.orientation, ModelConstants.T_IDXS, *plan_rows[:, Plan.T_FROM_CURRENT_EULER].T)
+  _assign_xyz(model_packet.orientationRate, ModelConstants.T_IDXS, *plan_rows[:, Plan.ORIENTATION_RATE].T)
+  fill_xyz_poly(driving_packet.path, ModelConstants.POLY_PATH_DEGREE, *plan_rows[:, Plan.POSITION].T)
+
+
+def _write_temporal_pose(model_packet, outputs: dict[str, np.ndarray]) -> None:
+  pose_packet = model_packet.temporalPoseDEPRECATED
+  if "sim_pose" in outputs:
+    half_width = ModelConstants.POSE_WIDTH // 2
+    pose_packet.trans = outputs["sim_pose"][0, :half_width].tolist()
+    pose_packet.transStd = outputs["sim_pose_stds"][0, :half_width].tolist()
+    pose_packet.rot = outputs["sim_pose"][0, half_width:].tolist()
+    pose_packet.rotStd = outputs["sim_pose_stds"][0, half_width:].tolist()
+    return
+
+  pose_packet.trans = outputs["plan"][0, 0, Plan.VELOCITY].tolist()
+  pose_packet.transStd = outputs["plan_stds"][0, 0, Plan.VELOCITY].tolist()
+  pose_packet.rot = outputs["plan"][0, 0, Plan.ORIENTATION_RATE].tolist()
+  pose_packet.rotStd = outputs["plan_stds"][0, 0, Plan.ORIENTATION_RATE].tolist()
+
+
+def _write_lane_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
+  time_axis = plan_x_idxs_helper(ModelConstants, Plan, outputs)
+  model_packet.init("laneLines", 4)
+  for lane_idx in range(4):
+    lane_builder = model_packet.laneLines[lane_idx]
+    _assign_xyz(
+      lane_builder,
+      time_axis,
+      np.array(ModelConstants.X_IDXS),
+      outputs["lane_lines"][0, lane_idx, :, 0],
+      outputs["lane_lines"][0, lane_idx, :, 1],
+    )
+  model_packet.laneLineStds = outputs["lane_lines_stds"][0, :, 0, 0].tolist()
+  model_packet.laneLineProbs = outputs["lane_lines_prob"][0, 1::2].tolist()
+  fill_lane_line_meta(driving_packet.laneLineMeta, model_packet.laneLines, model_packet.laneLineProbs)
+
+  model_packet.init("roadEdges", 2)
+  for edge_idx in range(2):
+    edge_builder = model_packet.roadEdges[edge_idx]
+    _assign_xyz(
+      edge_builder,
+      time_axis,
+      np.array(ModelConstants.X_IDXS),
+      outputs["road_edges"][0, edge_idx, :, 0],
+      outputs["road_edges"][0, edge_idx, :, 1],
+    )
+  model_packet.roadEdgeStds = outputs["road_edges_stds"][0, :, 0, 0].tolist()
+
+
+def _write_leads(model_packet, outputs: dict[str, np.ndarray]) -> None:
+  model_packet.init("leadsV3", 3)
+  for lead_idx in range(3):
+    lead_builder = model_packet.leadsV3[lead_idx]
+    _assign_xyva(
+      lead_builder,
+      ModelConstants.LEAD_T_IDXS,
+      *outputs["lead"][0, lead_idx].T,
+      *outputs["lead_stds"][0, lead_idx].T,
+    )
+    lead_builder.prob = outputs["lead_prob"][0, lead_idx].tolist()
+    lead_builder.probTime = ModelConstants.LEAD_T_OFFSETS[lead_idx]
+
+
+def _write_meta(model_packet, outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int) -> None:
+  meta = model_packet.meta
+  meta.desireState = outputs["desire_state"][0].reshape(-1).tolist()
+  meta.desirePrediction = outputs["desire_pred"][0].reshape(-1).tolist()
+  meta.engagedProb = outputs["meta"][0, meta_layout.ENGAGED].item()
+  meta.init("disengagePredictions")
+
+  pred = meta.disengagePredictions
+  pred.t = ModelConstants.META_T_IDXS
+  pred.brakeDisengageProbs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE].tolist()
+  pred.gasDisengageProbs = outputs["meta"][0, meta_layout.GAS_DISENGAGE].tolist()
+  pred.steerOverrideProbs = outputs["meta"][0, meta_layout.STEER_OVERRIDE].tolist()
+  pred.brake3MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_3].tolist()
+  pred.brake4MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_4].tolist()
+  pred.brake5MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_5].tolist()
+
+  if hasattr(meta_layout, "GAS_PRESS") and hasattr(meta_layout, "BRAKE_PRESS"):
+    pred.gasPressProbs = outputs["meta"][0, meta_layout.GAS_PRESS].tolist()
+    pred.brakePressProbs = outputs["meta"][0, meta_layout.BRAKE_PRESS].tolist()
+
+  meta.hardBrakePredicted = _roll_brake_watch(outputs, memory, meta_layout)
+  model_packet.confidence = _confidence_bucket(outputs, memory, meta_layout, frame_id)
+
+
+def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
+                            outputs: dict[str, np.ndarray], action: log.ModelDataV2.Action,
+                            memory: DrivePacketMemory, vipc_frame_id: int, vipc_frame_id_extra: int,
+                            frame_id: int, frame_drop: float, timestamp_eof: int,
+                            model_execution_time: float, valid: bool, meta_layout) -> None:
+  frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
+  frame_drop_percent = frame_drop * 100
+  primary_msg.valid = valid
+  extended_msg.valid = valid
+
+  driving_packet = primary_msg.drivingModelData
+  driving_packet.frameId = vipc_frame_id
+  driving_packet.frameIdExtra = vipc_frame_id_extra
+  driving_packet.frameDropPerc = frame_drop_percent
+  driving_packet.modelExecutionTime = model_execution_time
+  driving_packet.action = action
+
+  model_packet = extended_msg.modelV2
+  model_packet.frameId = vipc_frame_id
+  model_packet.frameIdExtra = vipc_frame_id_extra
+  model_packet.frameAge = frame_age
+  model_packet.frameDropPerc = frame_drop_percent
+  model_packet.timestampEof = timestamp_eof
+  model_packet.modelExecutionTime = model_execution_time
+  model_packet.action = action
+
+  _write_plan_family(model_packet, driving_packet, outputs)
+  _write_temporal_pose(model_packet, outputs)
+  _write_lane_family(model_packet, driving_packet, outputs)
+  _write_leads(model_packet, outputs)
+  _write_meta(model_packet, outputs, memory, meta_layout, vipc_frame_id)
+
+  if SEND_RAW_PRED:
+    model_packet.rawPredictions = outputs["raw_pred"].tobytes()
+
+
+def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray],
+                              vipc_frame_id: int, vipc_dropped_frames: int,
+                              timestamp_eof: int, live_calib_seen: bool) -> None:
+  pose = outputs["pose"][0, :6]
+  pose_stds = outputs["pose_stds"][0, :6]
+  pose_finite = bool(np.isfinite(pose).all() and np.isfinite(pose_stds).all())
+  msg.valid = live_calib_seen & (vipc_dropped_frames < 1) & pose_finite
+  odo = msg.cameraOdometry
+  odo.frameId = vipc_frame_id
+  odo.timestampEof = timestamp_eof
+  odo.trans = pose[:3].tolist()
+  odo.rot = pose[3:6].tolist()
+  odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist()
+  odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist()
+  odo.transStd = pose_stds[:3].tolist()
+  odo.rotStd = pose_stds[3:6].tolist()
+  odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist()
+  odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist()
+
+__all__ = [
+  "DrivePacketMemory",
+  "pick_curvature",
+  "populate_drive_messages",
+  "populate_odometry_message",
+]

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py

@@ -0,0 +1,96 @@
+#!/usr/bin/env python3
+import codecs
+import pathlib
+import pickle
+import sys
+from collections.abc import Iterable
+from typing import Any
+
+from iqpilot.cereal import custom
+from tinygrad.nn.onnx import OnnxPBParser
+
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+from iqpilot.selfdrive.iqmodeld.config import Meta
+
+
+ModelBundle = custom.IQModelManager.ModelBundle
+
+
+def _blank_proto_doc() -> dict[str, Any]:
+  return {"graph": {"input": [], "output": []}, "metadata_props": []}
+
+
+class TelemetryEnvelopeParser(OnnxPBParser):
+  def _parse_ModelProto(self) -> dict:
+    envelope = _blank_proto_doc()
+    for fid, wire_type in self._parse_message(self.reader.len):
+      if fid == 7:
+        envelope["graph"] = self._parse_GraphProto()
+      elif fid == 14:
+        envelope["metadata_props"].append(self._parse_StringStringEntryProto())
+      else:
+        self.reader.skip_field(wire_type)
+    return envelope
+
+
+def _shape_fingerprint(value_info: dict[str, Any]) -> tuple[str, tuple[int, ...]]:
+  resolved = []
+  for axis in value_info["parsed_type"].shape:
+    resolved.append(int(axis) if isinstance(axis, int) else 0)
+  return value_info["name"], tuple(resolved)
+
+
+def _lookup_metadata(props: Iterable[dict[str, Any]], wanted_key: str) -> str | Any:
+  for entry in props:
+    if entry["key"] == wanted_key:
+      return entry["value"]
+  return None
+
+
+class Meta20hz(Meta):
+  ENGAGED = slice(0, 1)
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)
+
+
+def select_meta_layout():
+  active_bundle = get_active_bundle()
+  return Meta20hz if active_bundle is not None and active_bundle.is20hz else Meta
+
+
+def _decoded_slices(props: Iterable[dict[str, Any]]):
+  encoded = _lookup_metadata(props, "output_slices")
+  assert encoded is not None, "output_slices not found in metadata"
+  return pickle.loads(codecs.decode(encoded.encode(), "base64"))
+
+
+def _graph_shape_table(graph_doc: dict[str, Any], field_name: str) -> dict[str, tuple[int, ...]]:
+  return dict(_shape_fingerprint(item) for item in graph_doc[field_name])
+
+
+def build_metadata_record(model_path):
+  parsed = TelemetryEnvelopeParser(model_path).parse()
+  props = parsed["metadata_props"]
+  graph = parsed["graph"]
+  return {
+    "model_checkpoint": _lookup_metadata(props, "model_checkpoint"),
+    "output_slices": _decoded_slices(props),
+    "input_shapes": _graph_shape_table(graph, "input"),
+    "output_shapes": _graph_shape_table(graph, "output"),
+  }
+
+
+if __name__ == "__main__":
+  model_path = pathlib.Path(sys.argv[1])
+  metadata_path = model_path.parent / f"{model_path.stem}_metadata.pkl"
+  with open(metadata_path, "wb") as handle:
+    pickle.dump(build_metadata_record(model_path), handle)
+  print(f"saved metadata to {metadata_path}")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_bundle_downloader.py

@@ -0,0 +1,211 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import hashlib
+import json
+import os
+
+MODELS_BASE_URLS = (
+  "https://git.konn3kt.com/teal/IQModels/raw/branch/main",
+  "https://gitlvb.teallvbs.xyz/teal/IQModels/raw/branch/main",
+)
+CHUNK = 4 * 1024 * 1024
+HTTP_TIMEOUT_S = 60.0
+STREAM_RETRIES = 6
+
+
+def _requests_auth():
+  import importlib
+  for mod in ("iqpilot_private.models.git_auth", "iqpilot.models_private_src.git_auth",
+              "iqpilot.selfdrive.iqmodeld.models.git_auth"):
+    try:
+      return importlib.import_module(mod).get_requests_auth()
+    except Exception:
+      continue
+  return None
+
+
+def _hf():
+  import importlib
+  for mod in ("iqpilot_private.models.git_auth", "iqpilot.selfdrive.iqmodeld.models.git_auth"):
+    try:
+      m = importlib.import_module(mod)
+      return m.get_hf_headers(), m.hf_resolve_url
+    except Exception:
+      continue
+  return None, None
+
+
+def download_hf_file(hf_path: str, dst: str, sha256: str, size: int, progress_cb=None) -> str:
+  import requests
+  headers, resolve = _hf()
+  if resolve is None:
+    raise RuntimeError("no HF credentials available")
+  url = resolve(hf_path)
+  os.makedirs(os.path.dirname(dst), exist_ok=True)
+  tmp = dst + ".hfpart"
+  last_error: Exception | None = None
+  for _attempt in range(STREAM_RETRIES):
+    try:
+      have = os.path.getsize(tmp) if os.path.isfile(tmp) else 0
+      if size and have > size:
+        os.remove(tmp)
+        have = 0
+      if not size or have < size:
+        req_headers = dict(headers)
+        if have:
+          req_headers["Range"] = f"bytes={have}-"
+        with requests.get(url, headers=req_headers, stream=True, timeout=HTTP_TIMEOUT_S, allow_redirects=True) as r:
+          r.raise_for_status()
+          if have and r.status_code != 206:
+            have = 0
+          with open(tmp, "ab" if have else "wb") as f:
+            got = have
+            for chunk in r.iter_content(CHUNK):
+              f.write(chunk)
+              got += len(chunk)
+              if progress_cb is not None and size:
+                progress_cb(min(1.0, got / size))
+      digest = hashlib.sha256()
+      with open(tmp, "rb") as f:
+        for chunk in iter(lambda: f.read(CHUNK), b""):
+          digest.update(chunk)
+      if size and os.path.getsize(tmp) != size:
+        raise RuntimeError(f"size mismatch: {os.path.getsize(tmp)}/{size} bytes")
+      if sha256 and digest.hexdigest() != sha256:
+        os.remove(tmp)
+        raise RuntimeError("sha256 mismatch")
+      os.replace(tmp, dst)
+      return dst
+    except Exception as e:
+      last_error = e
+  raise RuntimeError(f"HF download failed: {last_error}")
+
+
+def _lfs_endpoint(base_url: str) -> str:
+  return base_url.split("/raw/", 1)[0] + ".git/info/lfs"
+
+
+def _resolve_oid(session, base_url: str, oid: str, size: int, auth):
+  import requests
+  batch = session.post(f"{_lfs_endpoint(base_url)}/objects/batch",
+                       data=json.dumps({"operation": "download", "transfers": ["basic"],
+                                        "objects": [{"oid": oid, "size": size}]}),
+                       headers={"Content-Type": "application/vnd.git-lfs+json",
+                                "Accept": "application/vnd.git-lfs+json"},
+                       auth=auth, timeout=HTTP_TIMEOUT_S)
+  batch.raise_for_status()
+  entry = batch.json()["objects"][0]
+  if "actions" not in entry:
+    raise requests.RequestException(f"LFS object unavailable: {entry.get('error', oid)}")
+  action = entry["actions"]["download"]
+  return action["href"], action.get("header", {})
+
+
+def _part_path(dst: str, oid: str) -> str:
+  return os.path.join(dst + ".parts", oid)
+
+
+def _part_complete(path: str, oid: str, size: int) -> bool:
+  if not os.path.isfile(path) or os.path.getsize(path) != size:
+    return False
+  digest = hashlib.sha256()
+  with open(path, "rb") as f:
+    for chunk in iter(lambda: f.read(CHUNK), b""):
+      digest.update(chunk)
+  return digest.hexdigest() == oid
+
+
+def _fetch_part(session, base_url: str, obj: dict, path: str, auth, progress) -> None:
+  size = int(obj["size"])
+  have = os.path.getsize(path) if os.path.isfile(path) else 0
+  if have > size:
+    os.remove(path)
+    have = 0
+  href, headers = _resolve_oid(session, base_url, obj["oid"], size, auth)
+  obj_auth = None if headers.get("Authorization") else auth
+  # LFS parts are content-addressed (oid == sha256), so a half-written part can be resumed with a
+  # Range request and verified afterwards instead of being thrown away on every restart.
+  if have:
+    headers = {**headers, "Range": f"bytes={have}-"}
+  with session.get(href, headers=headers, stream=True, timeout=HTTP_TIMEOUT_S, auth=obj_auth) as r:
+    r.raise_for_status()
+    if have and r.status_code != 206:
+      have = 0
+    with open(path, "ab" if have else "wb") as f:
+      for chunk in r.iter_content(CHUNK):
+        f.write(chunk)
+        progress(len(chunk))
+
+
+def download_lfs_bundle(objects: list, dst: str, sha256: str, size: int, progress_cb=None) -> str:
+  import requests
+  auth = _requests_auth()
+  session = requests.Session()
+  os.makedirs(dst + ".parts", exist_ok=True)
+  total = int(size) or sum(int(o["size"]) for o in objects)
+  done_bytes = sum(int(o["size"]) for o in objects if _part_complete(_part_path(dst, o["oid"]), o["oid"], int(o["size"])))
+  got = [done_bytes]
+
+  def progress(n: int) -> None:
+    got[0] += n
+    if progress_cb is not None and total:
+      progress_cb(min(1.0, got[0] / total))
+
+  last_error: Exception | None = None
+  for base_url in MODELS_BASE_URLS:
+    for _attempt in range(STREAM_RETRIES):
+      try:
+        for obj in objects:
+          path = _part_path(dst, obj["oid"])
+          if _part_complete(path, obj["oid"], int(obj["size"])):
+            continue
+          got[0] = done_bytes
+          _fetch_part(session, base_url, obj, path, auth, progress)
+          if not _part_complete(path, obj["oid"], int(obj["size"])):
+            if os.path.getsize(path) >= int(obj["size"]):
+              os.remove(path)
+            raise RuntimeError(f"part {obj['oid'][:12]} incomplete or failed verification")
+          done_bytes += int(obj["size"])
+          got[0] = done_bytes
+        break
+      except Exception as e:
+        last_error = e
+    else:
+      continue
+    break
+  else:
+    raise RuntimeError(f"model bundle download failed: {last_error}")
+
+  tmp = dst + ".part"
+  digest = hashlib.sha256()
+  with open(tmp, "wb") as out:
+    for obj in objects:
+      with open(_part_path(dst, obj["oid"]), "rb") as f:
+        for chunk in iter(lambda: f.read(CHUNK), b""):
+          out.write(chunk)
+          digest.update(chunk)
+  if total and os.path.getsize(tmp) != total:
+    os.remove(tmp)
+    raise RuntimeError(f"size mismatch: {os.path.getsize(tmp) if os.path.exists(tmp) else 0}/{total} bytes")
+  if sha256 and digest.hexdigest() != sha256:
+    os.remove(tmp)
+    for obj in objects:
+      try:
+        os.remove(_part_path(dst, obj["oid"]))
+      except OSError:
+        pass
+    raise RuntimeError("sha256 mismatch")
+  os.replace(tmp, dst)
+  for obj in objects:
+    try:
+      os.remove(_part_path(dst, obj["oid"]))
+    except OSError:
+      pass
+  try:
+    os.rmdir(dst + ".parts")
+  except OSError:
+    pass
+  return dst

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py

@@ -0,0 +1,58 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import mmap
+import os
+import pickle
+import struct
+
+from iqpilot.common.swaglog import cloudlog
+
+SMALL_CHANNEL = "/dev/shm/iqpilot_smallmodel"
+BIG_CHANNEL = "/dev/shm/iqpilot_bigmodel"
+SHM_SIZE = 8 * 1024 * 1024
+HEADER = struct.Struct("<QqQ")
+
+
+class ModelChannel:
+  def __init__(self, path: str, create: bool):
+    if create:
+      fd = os.open(path, os.O_CREAT | os.O_RDWR, 0o600)
+      os.ftruncate(fd, SHM_SIZE)
+    else:
+      fd = os.open(path, os.O_RDWR)
+    self.mm = mmap.mmap(fd, SHM_SIZE)
+    os.close(fd)
+    if create:
+      self.mm[:HEADER.size] = HEADER.pack(0, -1, 0)
+
+  def write(self, frame_id: int, payload: dict) -> None:
+    data = pickle.dumps(payload, protocol=pickle.HIGHEST_PROTOCOL)
+    if HEADER.size + len(data) > SHM_SIZE:
+      cloudlog.error(f"model payload {len(data)} bytes exceeds shm {SHM_SIZE}, dropping frame {frame_id}")
+      return
+    seq = HEADER.unpack(self.mm[:HEADER.size])[0]
+    HEADER.pack_into(self.mm, 0, seq + 1, frame_id, len(data))
+    self.mm[HEADER.size:HEADER.size + len(data)] = data
+    HEADER.pack_into(self.mm, 0, seq + 2, frame_id, len(data))
+
+  def peek_frame_id(self) -> int | None:
+    seq, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
+    if seq == 0 or seq % 2 != 0 or length == 0:
+      return None
+    return frame_id
+
+  def read(self) -> tuple[int, dict] | None:
+    seq1, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
+    if seq1 == 0 or seq1 % 2 != 0 or length == 0:
+      return None
+    data = bytes(self.mm[HEADER.size:HEADER.size + length])
+    seq2 = HEADER.unpack(self.mm[:HEADER.size])[0]
+    if seq1 != seq2:
+      return None
+    try:
+      return frame_id, pickle.loads(data)
+    except Exception:
+      return None

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py

@@ -0,0 +1,80 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import os
+import pickle
+
+import numpy as np
+
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.system.hardware.hw import Paths
+
+
+def _load_bundle(pkl_path: str, cam_w: int, cam_h: int, frame_skip: int) -> dict:
+  with open(pkl_path, "rb") as f:
+    bundle = pickle.load(f)
+  if bundle.get("frame_skip") != frame_skip:
+    raise RuntimeError(f"frame_skip {bundle.get('frame_skip')} != {frame_skip}")
+  if (cam_w, cam_h) not in bundle:
+    raise RuntimeError(f"missing {cam_w}x{cam_h}; has {[k for k in bundle if isinstance(k, tuple)]}")
+  _verify_selftest(bundle, cam_w, cam_h)
+  return bundle
+
+
+def _verify_selftest(bundle: dict, cam_w: int, cam_h: int) -> None:
+  want = bundle.get("selftest")
+  if not want:
+    raise RuntimeError("warp artifact predates the self-test; recompiling")
+  from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from iqpilot.selfdrive.iqmodeld.tools.compile_warp import selftest_digest
+  nv12_size = get_nv12_info(cam_w, cam_h)[3]
+  got = selftest_digest(bundle[(cam_w, cam_h)], cam_w, cam_h, nv12_size)
+  if got != want:
+    raise RuntimeError(f"warp self-test {got[:12]} != {want[:12]}; artifact computes differently here")
+
+
+class FrameWarp:
+
+  def __init__(self, cam_w: int, cam_h: int, frame_skip: int):
+    from tinygrad.tensor import Tensor
+
+    pkl_path = os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
+    bundle = None
+    if os.path.isfile(pkl_path):
+      try:
+        bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
+      except Exception as e:
+        cloudlog.warning(f"warp artifact unusable ({e}); discarding and recompiling")
+        os.remove(pkl_path)
+    if bundle is None:
+      cloudlog.warning(f"warp artifact missing; compiling for {cam_w}x{cam_h} (one-time)")
+      from iqpilot.selfdrive.iqmodeld.tools.compile_warp import compile_warp
+      compile_warp(cam_w, cam_h, pkl_path, frame_skip=frame_skip)
+      cloudlog.warning(f"warp compiled -> {pkl_path}")
+      bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
+    self._jit = bundle[(cam_w, cam_h)]
+
+    self._npy = {"tfm": np.zeros((3, 3), dtype=np.float32), "big_tfm": np.zeros((3, 3), dtype=np.float32)}
+    self._tensors = {k: Tensor(v, device="NPY").realize() for k, v in self._npy.items()}
+    self._blob_cache: dict[tuple[str, int], object] = {}
+    self._Tensor = Tensor
+
+  def _frame_tensor(self, key: str, buf):
+    from tinygrad.device import Device
+    arr = np.frombuffer(buf.data, dtype=np.uint8)
+    ck = (key, arr.ctypes.data)
+    t = self._blob_cache.get(ck)
+    if t is None:
+      t = self._Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype="uint8", device=Device.DEFAULT)
+      self._blob_cache[ck] = t
+    return t
+
+  def run(self, main_buf, extra_buf, main_tfm: np.ndarray, extra_tfm: np.ndarray) -> np.ndarray:
+    self._npy["tfm"][:] = main_tfm
+    self._npy["big_tfm"][:] = extra_tfm
+    warped = self._jit(tfm=self._tensors["tfm"], big_tfm=self._tensors["big_tfm"],
+                       frame=self._frame_tensor("img", main_buf),
+                       big_frame=self._frame_tensor("big_img", extra_buf))
+    return warped.numpy().astype(np.uint8, copy=False)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py

@@ -0,0 +1,100 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import os
+import re
+from pathlib import Path
+
+from iqpilot.system.hardware.hw import Paths
+
+
+_MODEL_ROOT = Path(Paths.model_root())
+_OVERRIDE_KEYS = (
+  "combinedRuntimeArtifact",
+  "combinedSplitArtifact",
+  "iqCombinedArtifact",
+)
+_SPLIT_ROLE_PATTERN = re.compile(r"^driving_(vision|policy|off_policy|on_policy)_(.+)_tinygrad\.pkl$")
+
+
+def _bundle_models(bundle) -> list:
+  models = getattr(bundle, "models", None)
+  return list(models) if models is not None else []
+
+
+def _bundle_override_map(bundle) -> dict[str, str]:
+  result: dict[str, str] = {}
+  for override in getattr(bundle, "overrides", None) or []:
+    key = getattr(override, "key", None)
+    value = getattr(override, "value", None)
+    if key and value:
+      result[str(key)] = str(value)
+  return result
+
+
+def _artifact_name(model) -> str:
+  return getattr(getattr(model, "artifact", None), "fileName", "") or ""
+
+
+def _split_suffixes(bundle) -> list[str]:
+  suffixes: list[str] = []
+  for model in _bundle_models(bundle):
+    match = _SPLIT_ROLE_PATTERN.match(_artifact_name(model))
+    if match:
+      suffixes.append(match.group(2))
+  return suffixes
+
+
+def _derived_candidates(bundle) -> list[str]:
+  seen: set[str] = set()
+  candidates: list[str] = []
+
+  for suffix in _split_suffixes(bundle):
+    for candidate in (
+      f"driving_combined_{suffix}.pkl",
+      f"iqmodeld_combined_{suffix}.pkl",
+    ):
+      if candidate not in seen:
+        seen.add(candidate)
+        candidates.append(candidate)
+
+  ref = getattr(bundle, "ref", None)
+  if ref:
+    short_ref = str(ref)[:8]
+    for candidate in (
+      f"driving_combined_{short_ref}.pkl",
+      f"iqmodeld_combined_{short_ref}.pkl",
+    ):
+      if candidate not in seen:
+        seen.add(candidate)
+        candidates.append(candidate)
+
+  return candidates
+
+
+def combined_split_artifact_candidates(bundle) -> list[Path]:
+  explicit_env = os.getenv("IQMODEL_COMBINED_PKL")
+  if explicit_env:
+    explicit_path = Path(explicit_env)
+    return [explicit_path if explicit_path.is_absolute() else _MODEL_ROOT / explicit_path]
+
+  overrides = _bundle_override_map(bundle)
+  explicit_names = [overrides[key] for key in _OVERRIDE_KEYS if key in overrides]
+  if explicit_names:
+    return [_MODEL_ROOT / name for name in explicit_names]
+
+  return [_MODEL_ROOT / name for name in _derived_candidates(bundle)]
+
+
+def resolve_combined_split_artifact(bundle) -> Path | None:
+  for candidate in combined_split_artifact_candidates(bundle):
+    if candidate.is_file():
+      return candidate
+  return None
+
+
+def has_combined_split_artifact(bundle) -> bool:
+  return resolve_combined_split_artifact(bundle) is not None

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
+
+try:
+  load_private_module(__name__, "iqpilot_private.models.fetcher")
+except ProprietaryModuleMissing:
+  from iqpilot.models_private_src.fetcher import *  # noqa: F403

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py

@@ -0,0 +1,313 @@
+#!/usr/bin/env python3
+"""
+Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import json
+import os
+import shutil
+from pathlib import Path
+
+from iqpilot.cereal import custom
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
+from iqpilot.system.hardware.hw import Paths
+
+try:
+  load_private_module(__name__, "iqpilot_private.models.helpers")
+except ProprietaryModuleMissing:
+  try:
+    from iqpilot.models_private_src.helpers import *  # noqa: F403
+  except ImportError:
+    pass
+
+
+ModelBundle = custom.IQModelManager.ModelBundle
+Runner = custom.IQModelManager.Runner
+_MODEL_ROOT = Path(Paths.model_root())
+_ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle"
+_MODELS_CACHE_KEY = "ModelManager_ModelsCache"
+_RUNNER_CACHE_KEY = "ModelRunnerTypeCache"
+_DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex"
+_PENDING_INDEX_KEY = "ModelManager_PendingIndex"
+_PENDING_MODEL_RESTORE_FILE = "/data/k3_pending_model_restore"
+_STOCK_RUNNER = int(Runner.stock)
+_TINYGRAD_RUNNER = int(Runner.tinygrad)
+_SNPE_RUNNER = int(Runner.snpe)
+
+_DEFAULT_MODEL_DIR = Path(__file__).resolve().parents[1] / "default_model"
+_DEFAULT_BUNDLE_JSON = _DEFAULT_MODEL_DIR / "bundle.json"
+_DEFAULT_BUNDLE_REF = "default"
+
+
+def get_default_model_bundle(_bundles):
+  return None
+
+
+def _coerce_runner_value(value) -> int | None:
+  raw = getattr(value, "raw", value)
+  try:
+    return int(raw)
+  except (TypeError, ValueError):
+    return None
+
+
+def _bundle_models(bundle) -> list:
+  models = getattr(bundle, "models", None)
+  return list(models) if models is not None else []
+
+
+def _bundle_needs_runtime_upgrade(bundle) -> bool:
+  if bundle is None:
+    return False
+
+  if _coerce_runner_value(getattr(bundle, "runner", None)) == _SNPE_RUNNER:
+    return True
+
+  for model in _bundle_models(bundle):
+    file_name = getattr(getattr(model, "artifact", None), "fileName", "") or ""
+    if file_name.endswith(".thneed"):
+      return True
+
+  return False
+
+
+def _load_cached_manifest_bundles(params: Params):
+  cached = params.get(_MODELS_CACHE_KEY) or {}
+  bundles = []
+  for raw_bundle in cached.get("bundles", []):
+    try:
+      min_selector_version = int(raw_bundle.get("minimumSelectorVersion", raw_bundle.get("minimum_selector_version", 0)))
+      compatibility_view = dict(raw_bundle)
+      compatibility_view["minimumSelectorVersion"] = min_selector_version
+      is_compatible = globals().get("is_bundle_version_compatible")
+      if is_compatible is not None and not is_compatible(compatibility_view):
+        continue
+
+      if "short_name" in raw_bundle:
+        from iqpilot.selfdrive.iqmodeld.models.fetcher import ManifestDecoder
+        bundles.append(ManifestDecoder._decode_bundle(raw_bundle))
+        continue
+
+      if "internalName" in raw_bundle:
+        bundles.append(ModelBundle(**raw_bundle))
+        continue
+
+      bundle = ModelBundle()
+      bundle.index = int(raw_bundle["index"])
+      bundle.internalName = raw_bundle.get("short_name")
+      bundle.displayName = raw_bundle.get("display_name")
+      bundle.status = 0
+      bundle.generation = int(raw_bundle["generation"])
+      bundle.environment = raw_bundle["environment"]
+      bundle.runner = raw_bundle.get("runner", Runner.tinygrad)
+      bundle.is20hz = raw_bundle.get("is_20hz", False)
+      bundle.minimumSelectorVersion = int(min_selector_version)
+      bundle.ref = raw_bundle.get("ref")
+      bundle.overrides = []
+      for key, value in raw_bundle.get("overrides", {}).items():
+        override = custom.IQModelManager.Override()
+        override.key = key
+        override.value = value
+        bundle.overrides.append(override)
+
+      bundle.models = []
+      for raw_model in raw_bundle.get("models", []):
+        model = custom.IQModelManager.Model()
+        model.type = raw_model.get("type")
+        for attr_name in ("artifact", "metadata"):
+          raw_artifact = raw_model.get(attr_name)
+          if not raw_artifact:
+            continue
+          artifact = custom.IQModelManager.Artifact()
+          artifact.fileName = raw_artifact.get("file_name")
+          download_uri = custom.IQModelManager.DownloadUri()
+          download_uri.uri = raw_artifact.get("download_uri", {}).get("url")
+          download_uri.sha256 = raw_artifact.get("download_uri", {}).get("sha256")
+          artifact.downloadUri = download_uri
+          setattr(model, attr_name, artifact)
+        bundle.models.append(model)
+
+      bundles.append(bundle)
+    except Exception:
+      continue
+  return bundles
+
+
+def _bundle_match_key(bundle) -> tuple[str | None, str | None, str | None]:
+  return (
+    getattr(bundle, "ref", None),
+    getattr(bundle, "internalName", None),
+    getattr(bundle, "displayName", None),
+  )
+
+
+def _find_runtime_upgrade(bundle, params: Params, available_bundles=None):
+  if not _bundle_needs_runtime_upgrade(bundle):
+    return bundle
+
+  candidate_bundles = available_bundles if available_bundles is not None else _load_cached_manifest_bundles(params)
+  ref, internal_name, display_name = _bundle_match_key(bundle)
+
+  for candidate in candidate_bundles:
+    if getattr(candidate, "ref", None) and getattr(candidate, "ref", None) == ref:
+      return candidate
+
+  for candidate in candidate_bundles:
+    if getattr(candidate, "internalName", None) == internal_name:
+      return candidate
+
+  for candidate in candidate_bundles:
+    if getattr(candidate, "displayName", None) == display_name:
+      return candidate
+
+  return None
+
+
+def bundle_files_ready(bundle) -> bool:
+  if bundle is None:
+    return False
+
+  for model in _bundle_models(bundle):
+    artifact = getattr(model, "artifact", None)
+    metadata = getattr(model, "metadata", None)
+    for file_name in (getattr(metadata, "fileName", None), getattr(artifact, "fileName", None)):
+      if file_name and not (_MODEL_ROOT / file_name).is_file():
+        return False
+  return True
+
+
+def persist_active_bundle(params: Params, bundle) -> None:
+  params.put(_ACTIVE_BUNDLE_KEY, bundle.to_dict())
+  params.remove(_RUNNER_CACHE_KEY)
+
+
+def _load_default_bundle_dict() -> dict:
+  return json.loads(_DEFAULT_BUNDLE_JSON.read_text())
+
+
+def _default_bundle_filenames(bundle_dict: dict) -> list[str]:
+  names = []
+  for model in bundle_dict.get("models", []):
+    for artifact in (model.get("metadata"), model.get("artifact")):
+      file_name = artifact.get("fileName", "") if isinstance(artifact, dict) else ""
+      if file_name:
+        names.append(file_name)
+  return names
+
+
+def is_default_bundle(bundle) -> bool:
+  return bool(bundle is not None and getattr(bundle, "ref", None) == _DEFAULT_BUNDLE_REF)
+
+
+def ensure_default_model_files(bundle_dict: dict = None) -> None:
+  bundle_dict = bundle_dict if bundle_dict is not None else _load_default_bundle_dict()
+  try:
+    _MODEL_ROOT.mkdir(parents=True, exist_ok=True)
+  except OSError as e:
+    cloudlog.exception(f"default_model: cannot create model root: {e}")
+    return
+  for file_name in _default_bundle_filenames(bundle_dict):
+    src = _DEFAULT_MODEL_DIR / file_name
+    dst = _MODEL_ROOT / file_name
+    if not src.is_file():
+      cloudlog.error(f"default_model: shipped asset missing {src}")
+      continue
+    if dst.is_file() and dst.stat().st_size == src.stat().st_size:
+      continue
+    try:
+      shutil.copy2(src, dst)
+      cloudlog.warning(f"default_model: staged {file_name} into model root")
+    except OSError as e:
+      cloudlog.exception(f"default_model: failed staging {file_name}: {e}")
+
+
+def select_default_model(params: Params = None) -> None:
+  params = Params() if params is None else params
+  bundle_dict = _load_default_bundle_dict()
+  ensure_default_model_files(bundle_dict)
+  params.remove(_DOWNLOAD_INDEX_KEY)
+  params.remove(_PENDING_INDEX_KEY)
+  params.put(_ACTIVE_BUNDLE_KEY, bundle_dict)
+  params.remove(_RUNNER_CACHE_KEY)
+  params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
+  try:
+    if os.path.isfile(_PENDING_MODEL_RESTORE_FILE):
+      os.remove(_PENDING_MODEL_RESTORE_FILE)
+  except OSError:
+    pass
+
+
+def seed_default_bundle_if_unset(params: Params = None) -> None:
+  params = Params() if params is None else params
+  if params.get(_ACTIVE_BUNDLE_KEY):
+    return
+  queued_download = params.get(_DOWNLOAD_INDEX_KEY)
+  try:
+    select_default_model(params)
+    if queued_download is not None:
+      params.put(_DOWNLOAD_INDEX_KEY, queued_download)
+    cloudlog.warning("default_model: seeded Default (CD210) as active bundle")
+  except Exception as e:
+    cloudlog.exception(f"default_model: failed to seed default bundle: {e}")
+
+
+def get_runtime_bundle_upgrade(bundle, params: Params = None, available_bundles=None):
+  params = Params() if params is None else params
+  return _find_runtime_upgrade(bundle, params, available_bundles)
+
+
+def get_active_bundle(params: Params = None):
+  params = Params() if params is None else params
+
+  try:
+    active_bundle = params.get(_ACTIVE_BUNDLE_KEY) or {}
+    if not active_bundle:
+      return None
+    is_compatible = globals().get("is_bundle_version_compatible")
+    if is_compatible is not None and not is_compatible(active_bundle):
+      return None
+    bundle = ModelBundle(**active_bundle)
+  except Exception:
+    return None
+
+  replacement = _find_runtime_upgrade(bundle, params)
+  if replacement is not None and replacement is not bundle and bundle_files_ready(replacement):
+    persist_active_bundle(params, replacement)
+    return replacement
+
+  return bundle
+
+
+def get_active_model_runner(params: Params = None, force_check=False):
+  params = Params() if params is None else params
+
+  active_bundle = get_active_bundle(params)
+  if not active_bundle:
+    seed_default_bundle_if_unset(params)
+    active_bundle = get_active_bundle(params)
+    if not active_bundle:
+      if params.get(_RUNNER_CACHE_KEY) != str(_TINYGRAD_RUNNER):
+        params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
+      return _TINYGRAD_RUNNER
+
+  cached_runner_type = params.get(_RUNNER_CACHE_KEY)
+  if cached_runner_type and not force_check and isinstance(cached_runner_type, str) and cached_runner_type.isdigit():
+    return int(cached_runner_type)
+
+  runner_type = _coerce_runner_value(active_bundle.runner)
+  if runner_type == _SNPE_RUNNER:
+    replacement = _find_runtime_upgrade(active_bundle, params)
+    if replacement is not None and replacement is not active_bundle and bundle_files_ready(replacement):
+      persist_active_bundle(params, replacement)
+      runner_type = _coerce_runner_value(replacement.runner)
+    else:
+      if replacement is not None and getattr(replacement, "index", None) is not None and params.get(_DOWNLOAD_INDEX_KEY) is None:
+        params.put(_DOWNLOAD_INDEX_KEY, int(replacement.index))
+        cloudlog.warning(f"Queued tinygrad migration for retired bundle {getattr(active_bundle, 'internalName', '<unknown>')}")
+      runner_type = _TINYGRAD_RUNNER
+
+  if cached_runner_type != runner_type:
+    params.put(_RUNNER_CACHE_KEY, int(runner_type))
+
+  return runner_type

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py

@@ -0,0 +1,9 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.common.steer_delay import cached_steer_delay
+
+
+class InferenceStateBase:
+  def __init__(self):
+    self.lat_delay = cached_steer_delay()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py

@@ -0,0 +1,231 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import os
+import pickle as _pk
+from dataclasses import dataclass, field
+from typing import TYPE_CHECKING, Any
+
+import numpy as np
+from iqpilot.cereal import custom
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.system.hardware import TICI
+from iqpilot.system.hardware.hw import Paths as _hw_paths
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle as _fetch_bundle
+from iqpilot.selfdrive.iqmodeld.models.combined_artifact import has_combined_split_artifact
+
+# ---- runtime type surface (native OpenCL/frame handles resolve to Any off-device) ----
+if TYPE_CHECKING:
+  from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot, RoadProjector
+else:
+  def _resolve_native_types() -> tuple[Any, Any]:
+    try:
+      from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot as iq_clmem
+      from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector as iq_frame
+      return iq_clmem, iq_frame
+    except (ModuleNotFoundError, ImportError):
+      return Any, Any
+
+  GpuMemorySlot, RoadProjector = _resolve_native_types()
+
+NumpyDict = dict[str, np.ndarray]
+ShapeDict = dict[str, tuple[int, ...]]
+SliceDict = dict[str, slice]
+CLMemDict = dict[str, GpuMemorySlot]
+FrameDict = dict[str, RoadProjector]
+
+ModelType = custom.IQModelManager.Model.Type
+Model = custom.IQModelManager.Model
+
+SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
+CUSTOM_MODEL_PATH = _hw_paths.model_root()
+
+_META_FIELDS = ("input_shapes", "output_slices")
+
+USBGPU = "USBGPU" in os.environ
+
+
+def _configure_accelerator():
+  """Point tinygrad at the right backend. Must run before tinygrad is imported,
+  which is why it fires at module import."""
+  backend, extra = ("QCOM" if TICI else "CPU"), {}
+  if USBGPU:
+    backend, extra = "AMD", {"AMD_IFACE": "USB"}
+  elif TICI:
+    extra = {"QCOM_PRIORITY": "8"}
+  os.environ["DEV"] = backend
+  os.environ.update(extra)
+
+
+_configure_accelerator()
+
+
+# real metadata pkls are a few KB; anything bigger is a model artifact wrongly
+# referenced as metadata (pre-fix manifests self-referenced the artifact), and
+# unpickling it here double-loads the model onto the GPU
+_META_MAX_BYTES = 1 << 20
+
+
+def load_artifact_metadata(metadata_filename):
+  """Read one artifact's metadata pkl: (input shapes, output slices)."""
+  try:
+    path = os.path.join(CUSTOM_MODEL_PATH, metadata_filename)
+    if os.path.getsize(path) > _META_MAX_BYTES:
+      cloudlog.error(f"metadata pkl {metadata_filename} is artifact-sized, refusing to unpickle it")
+      return tuple({} for _ in _META_FIELDS)
+    with open(path, 'rb') as fh:
+      blob = _pk.load(fh)
+    return tuple(blob.get(field, {}) for field in _META_FIELDS)
+  except Exception:
+    cloudlog.exception(f"unreadable metadata pkl {metadata_filename}, continuing without it")
+    return tuple({} for _ in _META_FIELDS)
+
+
+@dataclass
+class ArtifactSpec:
+  """One model of the active bundle plus its unpacked metadata."""
+  model: Any
+  metadata: Any = None
+  input_shapes: ShapeDict = field(default_factory=dict)
+  output_slices: SliceDict = field(default_factory=dict)
+
+  def __post_init__(self):
+    self.metadata = self.model.metadata
+    if self.metadata:
+      self.input_shapes, self.output_slices = load_artifact_metadata(self.metadata.fileName)
+
+
+# kept name: some runners annotate against the old alias
+ModelData = ArtifactSpec
+
+
+class RunnerRoot:
+  """Shared root of the runner hierarchy.
+
+  Both ModelRunner and the per-model parser mixins (model_types.py) inherit
+  this, so the concrete `TinygradRunner(ModelRunner, *Tinygrad)` diamond keeps
+  one consistent parser registry + slice implementation.
+  """
+
+  parser_method_dict: dict
+  _model_data: "ArtifactSpec | None"
+
+  def _slice_outputs(self, model_outputs):
+    raise NotImplementedError
+
+
+class ModelRunner(RunnerRoot):
+  """Base for the tinygrad/ONNX runners.
+
+  Owns the active bundle's ArtifactSpecs and the shared slice/parse plumbing;
+  subclasses provide input staging (prepare_inputs) and execution (_run_model).
+  """
+
+  # False for fused runners, which warp + manage temporal buffers inside the JIT
+  uses_opencl_warp = True
+
+  def __init__(self):
+    active = _fetch_bundle()
+    if not active:
+      raise ValueError("runner started without an active model bundle")
+
+    self.models = {spec.type.raw: ArtifactSpec(spec) for spec in _qcom_models(active)}
+    self.is_20hz_3d = False
+    self.is_20hz = active.is20hz
+    self.inputs = {}
+    self.parser_method_dict = {}
+    self._model_data = None            # active spec for the current operation
+    self._parser = self._constants = None
+
+  def _active_spec(self):
+    spec = self._model_data
+    if spec is None:
+      raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
+    return spec
+
+  # views proxied straight off the active artifact spec; kept out of the class
+  # body (served via __getattr__) so the read surface stays data-driven
+  _SPEC_VIEW = frozenset(("input_shapes", "output_slices"))
+
+  def __getattr__(self, name):
+    if name == "constants":
+      return self._constants
+    if name == "vision_input_names":
+      return list(self._active_spec().input_shapes)
+    if name in ModelRunner._SPEC_VIEW:
+      return getattr(self._active_spec(), name)
+    raise AttributeError(name)
+
+  def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
+    """Stage image + numpy inputs for inference; implemented per backend."""
+    raise NotImplementedError
+
+  def _run_model(self):
+    """Execute inference over the staged inputs; implemented per backend."""
+    raise NotImplementedError
+
+  def run_model(self):
+    # parsing happens inside each backend's _run_model
+    return self._run_model()
+
+  def _slice_outputs(self, model_outputs):
+    """Split the flat output vector into named views per the artifact's slice table."""
+    sliced = {}
+    for tag, span in self._active_spec().output_slices.items():
+      sliced[tag] = model_outputs[np.newaxis, span]
+    if SEND_RAW_PRED:
+      sliced["raw_pred"] = model_outputs.copy()
+    return sliced
+
+
+# ---- runner selection (which backend to build for the active bundle) ----------
+
+def _qcom_models(bundle) -> list:
+  # usbeMac artifacts ride along in a bundle for the eGPU host; they are never
+  # loaded on QCOM and must not affect runner classification
+  return [m for m in bundle.models if m.type.raw != ModelType.usbeMac]
+
+
+def _single_artifact_prefix(bundle, prefix: str) -> bool:
+  models = _qcom_models(bundle)
+  return len(models) == 1 and models[0].artifact.fileName.startswith(prefix)
+
+
+def _is_fused_bundle(bundle) -> bool:
+  return _single_artifact_prefix(bundle, "driving_fused_")
+
+
+def _is_supercombo_bundle(bundle) -> bool:
+  return _single_artifact_prefix(bundle, "driving_supercombo_")
+
+
+def _is_split_bundle(bundle) -> bool:
+  present = {m.type.raw for m in _qcom_models(bundle)}
+  split_kinds = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy}
+  return not present.isdisjoint(split_kinds)
+
+
+def get_model_runner() -> "ModelRunner":
+  """Build the runner backend that fits the active bundle (supercombo / fused /
+  combined-split / split / single). Concrete runners are imported lazily so one
+  backend failing to load can't take down the others at import time."""
+  from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner import (TinygradRunner,
+                                                                                             TinygradSplitRunner)
+  bundle = _fetch_bundle()
+  # an eMac-only bundle (no QCOM-loadable models) runs the stock default on
+  # device; the big host serves the bundle's precompiled artifact
+  if not (bundle and bundle.models and _qcom_models(bundle)):
+    return TinygradRunner(ModelType.supercombo)
+
+  if _is_supercombo_bundle(bundle):
+    from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner import TinygradSupercomboRunner
+    return TinygradSupercomboRunner()
+  if _is_fused_bundle(bundle):
+    from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner import TinygradFusedRunner
+    return TinygradFusedRunner()
+  if _is_split_bundle(bundle) and has_combined_split_artifact(bundle):
+    from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner import TinygradCombinedSplitRunner
+    return TinygradCombinedSplitRunner()
+  if _is_split_bundle(bundle):
+    return TinygradSplitRunner()
+  return TinygradRunner(_qcom_models(bundle)[0].type.raw)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py

@@ -0,0 +1,245 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import os
+import pickle
+from pathlib import Path
+from typing import Any
+
+import numpy as np
+
+from iqpilot.selfdrive.iqmodeld.models.combined_artifact import resolve_combined_split_artifact
+from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import NumpyDict, ShapeDict, SliceDict
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
+from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
+from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
+
+
+def _tinygrad_imports():
+  from tinygrad.device import Device
+  from tinygrad.tensor import Tensor
+  return Tensor, Device
+
+
+def _phase_roles(meta_by_role: dict[str, dict]) -> list[str]:
+  return [name for name in meta_by_role if name != "vision"]
+
+
+def _phase_desire_key(policy_shapes: dict[str, tuple[int, ...]]) -> str:
+  for key in policy_shapes:
+    if key.startswith("desire"):
+      return key
+  raise KeyError("No desire-like key found in policy inputs")
+
+
+def _phase_image_keys(vision_shapes: dict[str, tuple[int, ...]]) -> tuple[str, str]:
+  names = sorted(name for name in vision_shapes if "img" in name)
+  road_key = next((name for name in names if "big" not in name), None)
+  wide_key = next((name for name in names if "big" in name), None)
+  if road_key is None or wide_key is None:
+    raise ValueError(f"Unable to resolve road/wide image keys from {list(vision_shapes)}")
+  return road_key, wide_key
+
+
+def _base_policy_keys(policy_shapes: dict[str, tuple[int, ...]]) -> set[str]:
+  desired_key = _phase_desire_key(policy_shapes)
+  return {desired_key, "features_buffer", "traffic_convention", "action_t"}
+
+
+def _slice_map(raw_blob: np.ndarray, slices: dict[str, slice]) -> NumpyDict:
+  return {name: raw_blob[np.newaxis, section] for name, section in slices.items() if name != "pad"}
+
+
+class TinygradCombinedSplitRunner(ModelRunner):
+  uses_opencl_warp: bool = False
+
+  def __init__(self):
+    super().__init__()
+    self._constants = SplitModelConstants
+    self._parser = PhaseParser()
+    self._bundle = get_active_bundle()
+    self._artifact_path = resolve_combined_split_artifact(self._bundle)
+    if self._artifact_path is None:
+      raise FileNotFoundError("No IQ combined split artifact is available for the active bundle")
+
+    with open(self._artifact_path, "rb") as artifact:
+      runtime_package: dict[Any, Any] = pickle.load(artifact)
+
+    self._meta_by_role = runtime_package.get("meta_by_role", runtime_package.get("metadata", {}))
+    self._policy_roles = runtime_package.get("roles", _phase_roles(self._meta_by_role))
+    self._camera_programs = {
+      camera_key: spec
+      for camera_key, spec in runtime_package.items()
+      if isinstance(camera_key, tuple) and isinstance(spec, dict)
+    }
+    self._execute_bundle = runtime_package.get("execute_bundle", runtime_package.get("run_policy"))
+    self._frame_stride = int(runtime_package.get("frame_stride", runtime_package.get("frame_skip", 1)))
+
+    if "vision" not in self._meta_by_role:
+      raise ValueError("Combined split artifact is missing vision metadata")
+    if not self._policy_roles:
+      raise ValueError("Combined split artifact is missing policy roles")
+    if self._execute_bundle is None:
+      raise ValueError("Combined split artifact is missing execute_bundle")
+
+    self._vision_meta = self._meta_by_role["vision"]
+    self._primary_policy_meta = self._meta_by_role[self._policy_roles[0]]
+    self._desired_key = _phase_desire_key(self._primary_policy_meta["input_shapes"])
+    self._road_key, self._wide_key = _phase_image_keys(self._vision_meta["input_shapes"])
+    self._extra_policy_keys = [
+      key for key in self._primary_policy_meta["input_shapes"]
+      if key not in _base_policy_keys(self._primary_policy_meta["input_shapes"])
+    ]
+
+    self._queue_tensors: dict[str, Any] | None = None
+    self._numpy_state: dict[str, np.ndarray] | None = None
+    self._camera_shape: tuple[int, int] | None = None
+    self._blob_cache: dict[tuple[str, int], Any] = {}
+    self._last_desire = np.zeros(self._primary_policy_meta["input_shapes"][self._desired_key][2], dtype=np.float32)
+
+  @property
+  def vision_input_names(self) -> list[str]:
+    return [self._road_key, self._wide_key]
+
+  @property
+  def input_shapes(self) -> ShapeDict:
+    merged: ShapeDict = dict(self._vision_meta["input_shapes"])
+    for role in self._policy_roles:
+      merged.update(self._meta_by_role[role]["input_shapes"])
+    return merged
+
+  @property
+  def output_slices(self) -> SliceDict:
+    merged: SliceDict = dict(self._vision_meta["output_slices"])
+    for role in self._policy_roles:
+      merged.update(self._meta_by_role[role]["output_slices"])
+    return merged
+
+  def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
+    raise RuntimeError("Combined split runner manages its own warp + queue state; use run_fused()")
+
+  def _frame_blob(self, stream_name: str, buf):
+    Tensor, Device = _tinygrad_imports()
+    raw_frame = np.frombuffer(buf.data, dtype=np.uint8)
+    cache_key = (stream_name, raw_frame.ctypes.data)
+    tensor = self._blob_cache.get(cache_key)
+    if tensor is None:
+      tensor = Tensor.from_blob(raw_frame.ctypes.data, (raw_frame.size,), dtype="uint8", device=Device.DEFAULT)
+      self._blob_cache[cache_key] = tensor
+    return tensor
+
+  def _allocate_runtime_state(self, camera_width: int, camera_height: int) -> None:
+    if self._queue_tensors is not None and self._camera_shape == (camera_width, camera_height):
+      return
+    if (camera_width, camera_height) not in self._camera_programs:
+      raise RuntimeError(f"No combined split kernels available for {camera_width}x{camera_height}")
+
+    Tensor, Device = _tinygrad_imports()
+    vision_shapes = self._vision_meta["input_shapes"]
+    policy_shapes = self._primary_policy_meta["input_shapes"]
+
+    image_shape = vision_shapes[self._road_key]
+    frame_history = image_shape[1] // 6
+    queue_depth = self._frame_stride * (frame_history - 1) + 1
+    frame_queue_shape = (queue_depth, 6, image_shape[2], image_shape[3])
+
+    feature_shape = policy_shapes["features_buffer"]
+    desired_shape = policy_shapes[self._desired_key]
+    traffic_shape = policy_shapes["traffic_convention"]
+    action_shape = policy_shapes.get("action_t", traffic_shape)
+
+    numpy_state = {
+      "tfm": np.zeros((3, 3), dtype=np.float32),
+      "big_tfm": np.zeros((3, 3), dtype=np.float32),
+      "desire": np.zeros(desired_shape[2], dtype=np.float32),
+      "traffic_convention": np.zeros(traffic_shape, dtype=np.float32),
+      "action_t": np.zeros(action_shape, dtype=np.float32),
+    }
+    for key in self._extra_policy_keys:
+      numpy_state[key] = np.zeros(policy_shapes[key], dtype=np.float32)
+
+    queue_tensors = {
+      "img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
+      "big_img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
+      "feat_q": Tensor(
+        np.zeros((self._frame_stride * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), dtype=np.float32),
+        device=Device.DEFAULT,
+      ).contiguous().realize(),
+      "desire_q": Tensor(
+        np.zeros((self._frame_stride * desired_shape[1], desired_shape[0], desired_shape[2]), dtype=np.float32),
+        device=Device.DEFAULT,
+      ).contiguous().realize(),
+      **{name: Tensor(value, device="NPY").realize() for name, value in numpy_state.items()},
+    }
+
+    self._queue_tensors = queue_tensors
+    self._numpy_state = numpy_state
+    self._camera_shape = (camera_width, camera_height)
+
+  def _policy_inputs(self) -> dict[str, Any]:
+    assert self._queue_tensors is not None
+    tensor_names = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t", *self._extra_policy_keys]
+    return {name: self._queue_tensors[name] for name in tensor_names if name in self._queue_tensors}
+
+  def _merge_policy_outputs(self, raw_outputs: tuple[Any, ...]) -> NumpyDict:
+    outputs = self._parser.parse_vision_outputs(
+      _slice_map(raw_outputs[0].numpy().flatten(), self._vision_meta["output_slices"])
+    )
+
+    has_on_policy = any(role == "on_policy" for role in self._policy_roles)
+    for role_name, tensor_out in zip(self._policy_roles, raw_outputs[1:], strict=True):
+      parsed = self._parser.parse_policy_outputs(
+        _slice_map(tensor_out.numpy().flatten(), self._meta_by_role[role_name]["output_slices"])
+      )
+      if role_name == "off_policy" and has_on_policy:
+        parsed.pop("plan", None)
+      outputs.update(parsed)
+
+    if "planplus" in outputs and "plan" in outputs:
+      outputs["plan"] = outputs["plan"] + outputs["planplus"]
+    return outputs
+
+  def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
+    main_buf = bufs[self._road_key]
+    self._allocate_runtime_state(main_buf.width, main_buf.height)
+    assert self._queue_tensors is not None and self._numpy_state is not None and self._camera_shape is not None
+
+    self._numpy_state["tfm"][:] = transforms[self._road_key]
+    self._numpy_state["big_tfm"][:] = transforms[self._wide_key]
+
+    current_desire = numpy_inputs[self._desired_key].copy()
+    current_desire[0] = 0
+    self._numpy_state["desire"][:] = np.where(current_desire - self._last_desire > 0.99, current_desire, 0)
+    self._last_desire[:] = current_desire
+
+    if "traffic_convention" in numpy_inputs:
+      self._numpy_state["traffic_convention"][:] = numpy_inputs["traffic_convention"]
+    if "action_t" in numpy_inputs:
+      self._numpy_state["action_t"][:] = numpy_inputs["action_t"]
+    for key in self._extra_policy_keys:
+      if key in numpy_inputs:
+        self._numpy_state[key][:] = numpy_inputs[key]
+
+    stage_inputs = self._camera_programs[self._camera_shape].get("stage_inputs", self._camera_programs[self._camera_shape].get("warp_enqueue"))
+    if stage_inputs is None:
+      raise RuntimeError("Combined split artifact camera entry is missing stage_inputs")
+
+    staged_main, staged_wide = stage_inputs(
+      img_q=self._queue_tensors["img_q"],
+      big_img_q=self._queue_tensors["big_img_q"],
+      tfm=self._queue_tensors["tfm"],
+      big_tfm=self._queue_tensors["big_tfm"],
+      frame=self._frame_blob(self._road_key, bufs[self._road_key]),
+      big_frame=self._frame_blob(self._wide_key, bufs[self._wide_key]),
+    )
+    raw_outputs = self._execute_bundle(img=staged_main, big_img=staged_wide, **self._policy_inputs())
+    if not isinstance(raw_outputs, tuple):
+      raw_outputs = (raw_outputs,)
+    return self._merge_policy_outputs(raw_outputs)
+
+  def _run_model(self) -> NumpyDict:
+    raise RuntimeError("Combined split runner executes through run_fused()")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py

@@ -0,0 +1,178 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import os
+import pickle
+from typing import Any
+
+import numpy as np
+
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
+    CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict,
+)
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
+from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
+from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
+
+
+def _tinygrad_imports():
+    from tinygrad.tensor import Tensor
+    from tinygrad.device import Device
+    return Tensor, Device
+
+
+WARP_DEV = os.getenv('WARP_DEV')
+
+
+class TinygradFusedRunner(ModelRunner):
+    uses_opencl_warp: bool = False
+
+    def __init__(self):
+        super().__init__()
+
+        self._constants = SplitModelConstants
+        self._parser = PhaseParser()
+
+        if len(self.models) != 1:
+            raise ValueError(f"fused bundle must have exactly one artifact, got {list(self.models)}")
+        self._model_data = next(iter(self.models.values()))
+
+        pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
+        with open(pkl_path, 'rb') as f:
+            self._fused: dict[Any, Any] = pickle.load(f)
+
+        self._vision_meta = self._fused['metadata']['vision']
+        self._on_meta = self._fused['metadata']['on_policy']
+        self._off_meta = self._fused['metadata']['off_policy']
+        self._run_policy = self._fused['run_policy']
+        self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._fused.items() if isinstance(k, tuple)}
+        if not self._warp_jits:
+            raise ValueError("fused pkl has no warp JITs")
+
+        self._frame_skip: int = int(self._fused.get('frame_skip', 4))
+
+        self._queues: dict[str, Any] | None = None
+        self._npy_buffers: dict[str, np.ndarray] | None = None
+        self._cam_resolution: tuple[int, int] | None = None
+        self._blob_cache: dict[tuple[str, int], Any] = {}
+
+    def _frame_tensor(self, key, buf):
+        Tensor, Device = _tinygrad_imports()
+        arr = np.frombuffer(buf.data, dtype=np.uint8)
+        ck = (key, arr.ctypes.data)
+        t = self._blob_cache.get(ck)
+        if t is None:
+            t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
+            self._blob_cache[ck] = t
+        return t
+
+    @property
+    def vision_input_names(self) -> list[str]:
+        return ['img', 'big_img']
+
+    @property
+    def input_shapes(self) -> ShapeDict:
+        return {**self._vision_meta['input_shapes'], **self._on_meta['input_shapes']}
+
+    @property
+    def output_slices(self) -> SliceDict:
+        merged: SliceDict = {}
+        for src in (self._vision_meta['output_slices'], self._on_meta['output_slices'], self._off_meta['output_slices']):
+            merged.update({k: v for k, v in src.items() if k != 'pad'})
+        return merged
+
+    def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
+        raise RuntimeError("fused runner has no OpenCL path; use run_fused()")
+
+    def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
+        if self._queues is not None and self._cam_resolution == (cam_w, cam_h):
+            return
+        if (cam_w, cam_h) not in self._warp_jits:
+            raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
+
+        Tensor, Device = _tinygrad_imports()
+        img_shape = self._vision_meta['input_shapes']['img']
+        fb = self._on_meta['input_shapes']['features_buffer']
+        dp = self._on_meta['input_shapes']['desire_pulse']
+        n_frames = img_shape[1] // 6
+        img_buf_shape = (self._frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
+
+        zeros_u8 = lambda shp: Tensor(np.zeros(shp, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
+        zeros_f32 = lambda shp: Tensor(np.zeros(shp, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
+
+        self._queues = {
+            'img_q':     zeros_u8(img_buf_shape),
+            'big_img_q': zeros_u8(img_buf_shape),
+            'feat_q':    zeros_f32((self._frame_skip * (fb[1] - 1) + 1, fb[0], fb[2])),
+            'desire_q':  zeros_f32((self._frame_skip * dp[1], dp[0], dp[2])),
+        }
+        on_shapes = self._on_meta['input_shapes']
+        captured = self._run_policy.captured
+        jit_shapes = {
+            name: tuple(int(s) for s in view.shape)
+            for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info)
+        }
+
+        def policy_input_shape(name):
+            shape = on_shapes.get(name, jit_shapes.get(name))
+            if shape is None:
+                raise ValueError(f"fused pkl declares no shape for policy input {name}")
+            return shape
+
+        self._npy_buffers = {
+            'desire':             np.zeros(dp[2], dtype=np.float32),
+            'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32),
+            'tfm':                np.zeros((3, 3), dtype=np.float32),
+            'big_tfm':            np.zeros((3, 3), dtype=np.float32),
+        }
+        if 'action_t' in jit_shapes:
+            self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32)
+        self._cam_resolution = (cam_w, cam_h)
+
+    def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
+        Tensor, Device = _tinygrad_imports()
+
+        main_buf = bufs['img']
+        self._ensure_queues(main_buf.width, main_buf.height)
+        assert self._queues is not None and self._npy_buffers is not None
+
+        desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
+        if desire_key is not None:
+            self._npy_buffers['desire'][:] = numpy_inputs[desire_key]
+        if 'traffic_convention' in numpy_inputs:
+            self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention']
+        if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers:
+            self._npy_buffers['action_t'][:] = numpy_inputs['action_t']
+        self._npy_buffers['tfm'][:] = transforms['img']
+        self._npy_buffers['big_tfm'][:] = transforms['big_img']
+
+        npy = lambda key: Tensor(self._npy_buffers[key], device='NPY')
+
+        frame = self._frame_tensor('img', bufs['img'])
+        big_frame = self._frame_tensor('big_img', bufs['big_img'])
+
+        warp_jit = self._warp_jits[self._cam_resolution]
+        img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
+                                tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame)
+
+        policy_inputs = dict(
+            img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
+            desire=npy('desire'), traffic_convention=npy('traffic_convention'))
+        if 'action_t' in self._npy_buffers:
+            policy_inputs['action_t'] = npy('action_t')
+        vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs)
+
+        def _slice(tensor_out, meta) -> NumpyDict:
+            flat = tensor_out.numpy().flatten()
+            return {k: flat[np.newaxis, sl] for k, sl in meta['output_slices'].items() if k != 'pad'}
+
+        parsed: NumpyDict = {}
+        parsed.update(self._parser.parse_vision_outputs(_slice(vision_out_t, self._vision_meta)))
+        parsed.update(self._parser.parse_policy_outputs(_slice(off_out_t, self._off_meta)))
+        parsed.update(self._parser.parse_policy_outputs(_slice(on_out_t, self._on_meta)))
+        return parsed
+
+    def _run_model(self) -> NumpyDict:
+        raise RuntimeError("fused path goes through run_fused(), not _run_model()")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py

@@ -0,0 +1,60 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from abc import ABC
+from collections.abc import Callable
+
+import numpy as np
+
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType, NumpyDict
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import RunnerRoot
+from iqpilot.selfdrive.iqmodeld.parser import ArchiveParser, PhaseParser
+
+
+class _ParserRole(RunnerRoot, ABC):
+  def _bind_parser_role(self,
+                        selector: int,
+                        parser_builder: Callable[[], object],
+                        projector: Callable[[object, NumpyDict], NumpyDict]) -> None:
+    parser = parser_builder()
+    self.parser_method_dict[selector] = lambda model_blob: projector(parser, self._slice_outputs(model_blob))
+
+
+def _phase_policy(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
+  return parser.parse_policy_outputs(sliced_outputs)
+
+
+def _phase_vision(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
+  return parser.parse_vision_outputs(sliced_outputs)
+
+
+def _archive_combined(parser: ArchiveParser, sliced_outputs: NumpyDict) -> NumpyDict:
+  return parser.parse_outputs(sliced_outputs)
+
+
+class OffPolicyTinygrad(_ParserRole, ABC):
+  def __init__(self):
+    self._bind_parser_role(ModelType.offPolicy, PhaseParser, _phase_policy)
+
+
+class OnPolicyTinygrad(_ParserRole, ABC):
+  def __init__(self):
+    self._bind_parser_role(ModelType.onPolicy, PhaseParser, _phase_policy)
+
+
+class PolicyTinygrad(_ParserRole, ABC):
+  def __init__(self):
+    self._bind_parser_role(ModelType.policy, PhaseParser, _phase_policy)
+
+
+class VisionTinygrad(_ParserRole, ABC):
+  def __init__(self):
+    self._bind_parser_role(ModelType.vision, PhaseParser, _phase_vision)
+
+
+class SupercomboTinygrad(_ParserRole, ABC):
+  def __init__(self):
+    self._bind_parser_role(ModelType.supercombo, ArchiveParser, _archive_combined)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py

@@ -0,0 +1,339 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import hashlib
+import math
+import os
+import pickle
+import re
+from typing import Any
+
+import numpy as np
+
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
+from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
+from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
+
+
+def _tinygrad_imports():
+    from tinygrad.tensor import Tensor
+    from tinygrad.device import Device
+    return Tensor, Device
+
+
+def _captured_queue_depth(warp_jit: Any) -> int | None:
+    captured = getattr(warp_jit, "captured", None)
+    infos = getattr(captured, "expected_input_info", None)
+    if not infos or len(infos) < 2:
+        return None
+
+    view_repr = repr(infos[1][0])
+    dims = [int(val) for val in re.findall(r"arg=(\d+)", view_repr)]
+    return dims[0] if len(dims) >= 4 else None
+
+
+def _captured_devices(warp_jit: Any) -> set[str]:
+    captured = getattr(warp_jit, "captured", None)
+    infos = getattr(captured, "expected_input_info", None)
+    if not infos:
+        return set()
+
+    devices: set[str] = set()
+    for info in infos:
+        if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
+            devices.add(info[3])
+    return devices
+
+
+def _captured_expected_names(jit_obj: Any) -> list[str]:
+    captured = getattr(jit_obj, "captured", None)
+    names = getattr(captured, "expected_names", None)
+    return list(names) if names else []
+
+
+def _file_sha256(path: str) -> str:
+    digest = hashlib.sha256()
+    with open(path, "rb") as f:
+        for chunk in iter(lambda: f.read(1024 * 1024), b""):
+            digest.update(chunk)
+    return digest.hexdigest()
+
+
+def _is_jit_arg_mismatch(err: BaseException) -> bool:
+    return "args mismatch in JIT" in str(err)
+
+
+class TinygradSupercomboRunner(ModelRunner):
+    uses_opencl_warp: bool = False
+
+    def __init__(self):
+        super().__init__()
+        self._constants = SplitModelConstants
+        self._parser = PhaseParser()
+
+        if len(self.models) != 1:
+            raise ValueError(f"supercombo bundle must have exactly one artifact, got {list(self.models)}")
+        self._model_data = next(iter(self.models.values()))
+
+        pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
+        self._pkl_path = pkl_path
+        self._expected_sha256 = getattr(getattr(self._model_data.model.artifact, "downloadUri", None), "sha256", "") or ""
+        self._verify_artifact_file()
+        with open(pkl_path, 'rb') as f:
+            self._m: dict[Any, Any] = pickle.load(f)
+
+        self._meta = self._m['metadata']
+        self._ish = self._meta['input_shapes']
+        self._slices = {k: v for k, v in self._meta['output_slices'].items() if k != 'pad'}
+        self._hidden_slice = self._meta['output_slices']['hidden_state']
+        self._run_policy = self._m['run_policy']
+        self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._m.items() if isinstance(k, tuple)}
+        if not self._warp_jits:
+            raise ValueError("supercombo pkl has no warp JITs")
+        self._frame_skip = int(self._m.get('frame_skip', 4))
+        self._validate_warp_jits(pkl_path)
+        self._validate_jit_names()
+
+        self._queues: dict[str, Any] | None = None
+        self._npy: dict[str, np.ndarray] | None = None
+        self._cam: tuple[int, int] | None = None
+        self._prev_desire = np.zeros(self._ish['desire_pulse'][2], dtype=np.float32)
+        self._blob_cache: dict[tuple[str, int], Any] = {}
+
+    def _verify_artifact_file(self) -> None:
+        if not self._expected_sha256:
+            return
+
+        actual_sha256 = _file_sha256(self._pkl_path)
+        if actual_sha256 == self._expected_sha256:
+            return
+
+        try:
+            os.remove(self._pkl_path)
+        except OSError:
+            pass
+        redownload_msg = self._schedule_active_bundle_redownload()
+
+        raise RuntimeError(
+            "supercombo artifact SHA mismatch: "
+            f"expected {self._expected_sha256}, got {actual_sha256} for {self._pkl_path}. "
+            f"Deleted the stale cached file{redownload_msg}."
+        )
+
+    def _validate_warp_jits(self, pkl_path: str) -> None:
+        img = self._ish['img']
+        n_frames = img[1] // 6
+        expected_depth = self._frame_skip * (n_frames - 1) + 1
+        expected_device = os.getenv('DEV')
+
+        mismatches: list[str] = []
+        for cam, warp_jit in sorted(self._warp_jits.items()):
+            captured_depth = _captured_queue_depth(warp_jit)
+            captured_devices = _captured_devices(warp_jit)
+            if captured_depth is not None and captured_depth != expected_depth:
+                mismatches.append(
+                    f"{cam[0]}x{cam[1]} queue-depth captured={captured_depth} expected={expected_depth}"
+                )
+            if expected_device and captured_devices and expected_device not in captured_devices:
+                mismatches.append(
+                    f"{cam[0]}x{cam[1]} device captured={sorted(captured_devices)} expected={expected_device}"
+                )
+
+        if mismatches:
+            details = "; ".join(mismatches)
+            raise RuntimeError(
+                "supercombo warp JIT compatibility mismatch: "
+                f"{details}. Bundle {pkl_path} was compiled with the wrong backend, frame_skip, or queue shape; "
+                "re-download or rebuild this model artifact."
+            )
+
+    def _validate_jit_names(self) -> None:
+        expected_warp_names = ['big_frame', 'big_tfm', 'frame', 'tfm']
+        expected_policy_names = ['big_img_q', 'desire_q', 'feat_q', 'img_q', 'packed_npy_inputs', 'warped']
+
+        mismatches: list[str] = []
+
+        policy_names = sorted(_captured_expected_names(self._run_policy))
+        if policy_names and policy_names != expected_policy_names:
+            mismatches.append(f"run_policy captured={policy_names} expected={expected_policy_names}")
+
+        for cam, warp_jit in sorted(self._warp_jits.items()):
+            warp_names = sorted(_captured_expected_names(warp_jit))
+            if warp_names and warp_names != expected_warp_names:
+                mismatches.append(f"{cam[0]}x{cam[1]} warp captured={warp_names} expected={expected_warp_names}")
+
+        if mismatches:
+            details = "; ".join(mismatches)
+            actual_sha = None
+            try:
+                actual_sha = _file_sha256(self._pkl_path)
+            except OSError:
+                pass
+
+            if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
+                try:
+                    os.remove(self._pkl_path)
+                except OSError:
+                    pass
+                redownload_msg = self._schedule_active_bundle_redownload()
+                raise RuntimeError(
+                    "supercombo artifact contract mismatch with stale cached SHA: "
+                    f"{details}. Expected SHA {self._expected_sha256}, got {actual_sha}. "
+                    f"Deleted the stale cached file{redownload_msg}."
+                )
+
+            raise RuntimeError(
+                "supercombo artifact JIT argument mismatch: "
+                f"{details}. This model file does not match the current IQPilot runtime contract. "
+                "Re-download or rebuild this model artifact."
+            )
+
+    def _handle_runtime_jit_mismatch(self, err: BaseException) -> None:
+        if not _is_jit_arg_mismatch(err):
+            raise err
+
+        actual_sha = None
+        try:
+            actual_sha = _file_sha256(self._pkl_path)
+        except OSError:
+            pass
+
+        if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
+            try:
+                os.remove(self._pkl_path)
+            except OSError:
+                pass
+            redownload_msg = self._schedule_active_bundle_redownload()
+            raise RuntimeError(
+                "supercombo artifact runtime JIT mismatch with stale cached SHA: "
+                f"expected {self._expected_sha256}, got {actual_sha} for {self._pkl_path}. "
+                f"Deleted the stale cached file{redownload_msg}."
+            ) from err
+
+        raise RuntimeError(
+            "supercombo artifact runtime JIT mismatch: "
+            f"{err}. This model file does not match the current IQPilot runtime contract. "
+            "Re-download or rebuild this model artifact."
+        ) from err
+
+    def _schedule_active_bundle_redownload(self) -> str:
+        try:
+            params = Params()
+            active_bundle = params.get("ModelManager_ActiveBundle") or {}
+            index = active_bundle.get("index") if isinstance(active_bundle, dict) else None
+            if isinstance(index, str) and index.isdigit():
+                index = int(index)
+            if isinstance(index, int) and index >= 0:
+                params.put("ModelManager_DownloadIndex", str(index))
+                params.remove("ModelRunnerTypeCache")
+                return "; scheduled automatic re-download of the active model"
+        except Exception:
+            pass
+
+        return "; unable to schedule automatic re-download"
+
+    def _frame_tensor(self, key: str, buf):
+        Tensor, Device = _tinygrad_imports()
+        arr = np.frombuffer(buf.data, dtype=np.uint8)
+        ck = (key, arr.ctypes.data)
+        t = self._blob_cache.get(ck)
+        if t is None:
+            t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
+            self._blob_cache[ck] = t
+        return t
+
+    @property
+    def vision_input_names(self) -> list[str]:
+        return ['img', 'big_img']
+
+    @property
+    def input_shapes(self) -> ShapeDict:
+        return dict(self._ish)
+
+    @property
+    def output_slices(self) -> SliceDict:
+        return dict(self._slices)
+
+    def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
+        raise RuntimeError("supercombo runner has no OpenCL path; use run_fused()")
+
+    def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
+        if self._queues is not None and self._cam == (cam_w, cam_h):
+            return
+        if (cam_w, cam_h) not in self._warp_jits:
+            raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
+
+        Tensor, Device = _tinygrad_imports()
+        fs = self._frame_skip
+        img = self._ish['img']
+        n_frames = img[1] // 6
+        img_buf = (fs * (n_frames - 1) + 1, 6, img[2], img[3])
+        fb = self._ish['features_buffer']
+        dp = self._ish['desire_pulse']
+        tc = self._ish['traffic_convention']
+        at = self._ish['action_t']
+
+        zeros_u8 = lambda s: Tensor(np.zeros(s, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
+        zeros_f32 = lambda s: Tensor(np.zeros(s, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
+
+        shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
+        sizes = [math.prod(s) for s in shapes.values()]
+        packed = np.zeros(sum(sizes), dtype=np.float32)
+        views = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed, np.cumsum(sizes[:-1])), strict=True)}
+
+        self._npy = {'tfm': np.zeros((3, 3), dtype=np.float32), 'big_tfm': np.zeros((3, 3), dtype=np.float32), **views}
+        self._queues = {
+            'img_q':     zeros_u8(img_buf),
+            'big_img_q': zeros_u8(img_buf),
+            'feat_q':    zeros_f32((fs * fb[1], fb[0], fb[2])),
+            'desire_q':  zeros_f32((fs * dp[1], dp[0], dp[2])),
+            'tfm':       Tensor(self._npy['tfm'], device='NPY'),
+            'big_tfm':   Tensor(self._npy['big_tfm'], device='NPY'),
+            'packed_npy_inputs': Tensor(packed, device='NPY'),
+        }
+        self._cam = (cam_w, cam_h)
+
+    def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
+        Tensor, Device = _tinygrad_imports()
+        main_buf = bufs['img']
+        self._ensure_queues(main_buf.width, main_buf.height)
+        assert self._queues is not None and self._npy is not None
+
+        self._npy['tfm'][:] = transforms['img']
+        self._npy['big_tfm'][:] = transforms['big_img']
+
+        desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
+        cur = numpy_inputs[desire_key].copy() if desire_key is not None else np.zeros_like(self._prev_desire)
+        cur[0] = 0
+        self._npy['desire'][:] = np.where(cur - self._prev_desire > .99, cur, 0)
+        self._prev_desire[:] = cur
+        if 'traffic_convention' in numpy_inputs:
+            self._npy['traffic_convention'][:] = numpy_inputs['traffic_convention']
+        if 'action_t' in numpy_inputs:
+            self._npy['action_t'][:] = numpy_inputs['action_t']
+
+        frame = self._frame_tensor('img', bufs['img'])
+        big_frame = self._frame_tensor('big_img', bufs['big_img'])
+
+        warp = self._warp_jits[self._cam]
+        try:
+            warped = warp(tfm=self._queues['tfm'], big_tfm=self._queues['big_tfm'], frame=frame, big_frame=big_frame)
+            out, = self._run_policy(warped=warped, img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
+                                    feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
+                                    packed_npy_inputs=self._queues['packed_npy_inputs'])
+        except Exception as err:
+            self._handle_runtime_jit_mismatch(err)
+            raise
+        flat = out.numpy().flatten()
+
+        self._npy['prev_feat'][:] = flat[self._hidden_slice].reshape(self._npy['prev_feat'].shape)
+
+        sliced = {k: flat[np.newaxis, sl] for k, sl in self._slices.items()}
+        return self._parser.parse_vision_outputs(sliced)
+
+    def _run_model(self) -> NumpyDict:
+        raise RuntimeError("supercombo path goes through run_fused(), not _run_model()")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py

@@ -0,0 +1,190 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import pickle
+from dataclasses import dataclass
+
+import numpy as np
+from tinygrad.dtype import dtypes
+from tinygrad.tensor import Tensor
+
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
+  CLMemDict,
+  CUSTOM_MODEL_PATH,
+  FrameDict,
+  ModelType,
+  NumpyDict,
+  ShapeDict,
+  SliceDict,
+)
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
+from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types import (
+  OffPolicyTinygrad,
+  OnPolicyTinygrad,
+  PolicyTinygrad,
+  SupercomboTinygrad,
+  VisionTinygrad,
+)
+from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.iqmodeld.runtime.tinygrad import qcom_tensor_from_opencl_address
+from iqpilot.system.hardware import TICI
+
+
+@dataclass(frozen=True)
+class _TensorShapePlan:
+  dtype: object
+  device: str
+
+
+def _artifact_path(filename: str) -> str:
+  return f"{CUSTOM_MODEL_PATH}/{filename}"
+
+
+def _load_program_blob(filename: str):
+  with open(_artifact_path(filename), "rb") as artifact:
+    try:
+      return pickle.load(artifact)
+    except FileNotFoundError as exc:
+      assert "/dev/kgsl-3d0" not in str(exc), "Model was built on C3 or C3X, but is being loaded on PC"
+      raise
+
+
+def _compile_input_plan(captured) -> dict[str, _TensorShapePlan]:
+  plan: dict[str, _TensorShapePlan] = {}
+  for name, info in zip(captured.expected_names, captured.expected_input_info, strict=True):
+    plan[name] = _TensorShapePlan(dtype=info[2], device=info[3])
+  return plan
+
+
+def _merge_step_outputs(output_groups: list[NumpyDict]) -> NumpyDict:
+  stitched: NumpyDict = {}
+  for payload in output_groups:
+    stitched.update(payload)
+  if "planplus" in stitched and "plan" in stitched:
+    stitched["plan"] = stitched["plan"] + stitched["planplus"]
+  return stitched
+
+
+class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
+  def __init__(self, model_type: int = ModelType.supercombo):
+    ModelRunner.__init__(self)
+    for initializer in (SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
+      initializer.__init__(self)
+
+    self._constants = ModelConstants
+    self._model_data = self.models.get(model_type)
+    if self._model_data is None or self._model_data.model is None:
+      raise ValueError(f"Model data for type {model_type} not available.")
+
+    asset_name = self._model_data.model.artifact.fileName
+    assert asset_name.endswith("_tinygrad.pkl"), f"Invalid model file {asset_name} for TinygradRunner"
+
+    self.model_run = _load_program_blob(asset_name)
+    self._input_plan = _compile_input_plan(self.model_run.captured)
+    for name, spec in self._input_plan.items():
+      if "img" in name and spec.dtype is not dtypes.uint8:
+        raise ValueError(f"{asset_name}: image input {name} expects {spec.dtype}, incompatible with uint8 warp buffer")
+    self.input_to_dtype = {name: spec.dtype for name, spec in self._input_plan.items()}
+    self.input_to_device = {name: spec.device for name, spec in self._input_plan.items()}
+
+  @property
+  def vision_input_names(self) -> list[str]:
+    return [stream_name for stream_name in self.input_shapes if "img" in stream_name]
+
+  def _attach_vision_tensor(self, stream_name: str, frame_buffers: CLMemDict, frame_views: FrameDict) -> None:
+    spec = self._input_plan[stream_name]
+    frame_buffer = frame_buffers[stream_name]
+    if TICI:
+      self.inputs[stream_name] = qcom_tensor_from_opencl_address(frame_buffer.mem_address,
+                                                                 self.input_shapes[stream_name],
+                                                                 dtype=spec.dtype)
+      return
+
+    mirrored = frame_views[stream_name].as_numpy(frame_buffer).reshape(self.input_shapes[stream_name])
+    self.inputs[stream_name] = Tensor(mirrored, device=spec.device, dtype=spec.dtype).realize()
+
+  def _attach_state_tensor(self, tensor_name: str, tensor_value: np.ndarray) -> None:
+    spec = self._input_plan[tensor_name]
+    self.inputs[tensor_name] = Tensor(tensor_value, device=spec.device, dtype=spec.dtype).realize()
+
+  def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict):
+    for stream_name in imgs_cl:
+      if stream_name not in self.inputs or not TICI:
+        self._attach_vision_tensor(stream_name, imgs_cl, frames)
+
+  def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
+    for tensor_name, tensor_value in numpy_inputs.items():
+      self._attach_state_tensor(tensor_name, tensor_value)
+
+  def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
+    self.prepare_vision_inputs(imgs_cl, frames)
+    self.prepare_policy_inputs(numpy_inputs)
+    return self.inputs
+
+  def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
+    if self._model_data is None:
+      raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
+    return self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
+
+  def _run_model(self) -> NumpyDict:
+    raw_output = self.model_run(**self.inputs).numpy().reshape(-1)
+    return self._parse_outputs(raw_output)
+
+
+class TinygradSplitRunner(ModelRunner):
+  def __init__(self):
+    super().__init__()
+    self.is_20hz_3d = True
+    self._constants = SplitModelConstants
+    self.vision_runner = TinygradRunner(ModelType.vision)
+    self.policy_runner = TinygradRunner(ModelType.policy) if self.models.get(ModelType.policy) else None
+    self.off_policy_runner = TinygradRunner(ModelType.offPolicy) if self.models.get(ModelType.offPolicy) else None
+    self.on_policy_runner = TinygradRunner(ModelType.onPolicy) if self.models.get(ModelType.onPolicy) else None
+
+  def _policy_units(self) -> list[TinygradRunner]:
+    return [runner for runner in (self.policy_runner, self.off_policy_runner, self.on_policy_runner) if runner is not None]
+
+  def run_vision(self) -> NumpyDict:
+    return self.vision_runner.run_model()
+
+  def run_policy(self) -> NumpyDict:
+    return _merge_step_outputs([runner.run_model() for runner in self._policy_units()])
+
+  def refresh_policy_features(self, features_buffer: np.ndarray) -> None:
+    for runner in self._policy_units():
+      if "features_buffer" in runner._input_plan:
+        runner._attach_state_tensor("features_buffer", features_buffer)
+
+  def _run_model(self) -> NumpyDict:
+    return _merge_step_outputs([self.run_vision(), self.run_policy()])
+
+  @property
+  def vision_input_names(self) -> list[str]:
+    return list(self.vision_runner.vision_input_names)
+
+  @property
+  def input_shapes(self) -> ShapeDict:
+    composite: ShapeDict = dict(self.vision_runner.input_shapes)
+    for runner in self._policy_units():
+      composite.update(runner.input_shapes)
+    return composite
+
+  @property
+  def output_slices(self) -> SliceDict:
+    composite: SliceDict = dict(self.vision_runner.output_slices)
+    for runner in self._policy_units():
+      composite.update(runner.output_slices)
+    return composite
+
+  def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
+    self.vision_runner.prepare_vision_inputs(imgs_cl, frames)
+    assembled_inputs = dict(self.vision_runner.inputs)
+    for runner in self._policy_units():
+      runner.prepare_policy_inputs(numpy_inputs)
+      assembled_inputs.update(runner.inputs)
+    self.inputs = assembled_inputs
+    return assembled_inputs

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py

@@ -0,0 +1,89 @@
+import numpy as np
+
+
+def index_function(idx, max_val=192, max_idx=32):
+  return max_val * ((idx/max_idx)**2)
+
+
+class SplitModelConstants:
+  IDX_N = 33
+  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
+  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
+  LEAD_T_OFFSETS = [0., 2., 4.]
+  META_T_IDXS = [2., 4., 6., 8., 10.]
+
+  MODEL_FREQ = 20
+  HISTORY_FREQ = 5
+  HISTORY_LEN_SECONDS = 5
+  TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
+  FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
+  INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
+
+  FEATURE_LEN = 512
+
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
+  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+  DESIRED_CURV_WIDTH = 1
+  ACTION_WIDTH = 2
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+
+  POLY_PATH_DEGREE = 4
+
+
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+
+class Meta:
+  ENGAGED = slice(0, 1)
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py

@@ -0,0 +1,219 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+def safe_exp(values, out=None):
+  return np.exp(np.clip(values, -np.inf, 11), out=out)
+
+
+def sigmoid(values):
+  return 1.0 / (1.0 + safe_exp(-values))
+
+
+def _softmax_last(values, axis=-1):
+  values -= np.max(values, axis=axis, keepdims=True)
+  if values.dtype in (np.float32, np.float64):
+    safe_exp(values, out=values)
+  else:
+    values = safe_exp(values)
+  values /= np.sum(values, axis=axis, keepdims=True)
+  return values
+
+
+@dataclass(frozen=True)
+class _MixtureRecipe:
+  input_heads: int
+  output_heads: int
+  final_shape: tuple[int, ...]
+
+
+class _TensorKitchen:
+  def __init__(self, ignore_missing: bool = False):
+    self.ignore_missing = ignore_missing
+
+  def _grab(self, outputs: dict[str, np.ndarray], tensor_name: str) -> np.ndarray | None:
+    if tensor_name not in outputs:
+      if not self.ignore_missing:
+        raise ValueError(f"Missing output {tensor_name}")
+      return
+    return outputs[tensor_name]
+
+  def categorical(self, outputs: dict[str, np.ndarray], tensor_name: str, shape=None) -> None:
+    raw = self._grab(outputs, tensor_name)
+    if raw is None:
+      return
+    if shape is not None:
+      raw = raw.reshape((raw.shape[0],) + shape)
+    outputs[tensor_name] = _softmax_last(raw, axis=-1)
+
+  def binary(self, outputs: dict[str, np.ndarray], tensor_name: str) -> None:
+    raw = self._grab(outputs, tensor_name)
+    if raw is None:
+      return
+    outputs[tensor_name] = sigmoid(raw)
+
+  def mixture(self, outputs: dict[str, np.ndarray], tensor_name: str, recipe: _MixtureRecipe) -> None:
+    raw = self._grab(outputs, tensor_name)
+    if raw is None:
+      return
+
+    reshaped = raw.reshape((raw.shape[0], max(recipe.input_heads, 1), -1))
+    value_count = (reshaped.shape[2] - recipe.output_heads) // 2
+    means = reshaped[:, :, :value_count]
+    stds = safe_exp(reshaped[:, :, value_count:2 * value_count])
+
+    if recipe.input_heads > 1:
+      weights = np.zeros((reshaped.shape[0], recipe.input_heads, recipe.output_heads), dtype=reshaped.dtype)
+      for output_idx in range(recipe.output_heads):
+        weights[:, :, output_idx - recipe.output_heads] = _softmax_last(
+          reshaped[:, :, output_idx - recipe.output_heads], axis=-1
+        )
+
+      if recipe.output_heads == 1:
+        for batch_idx in range(weights.shape[0]):
+          order = np.argsort(weights[batch_idx][:, 0])[::-1]
+          weights[batch_idx] = weights[batch_idx][order]
+          means[batch_idx] = means[batch_idx][order]
+          stds[batch_idx] = stds[batch_idx][order]
+
+      hypothesis_shape = (reshaped.shape[0], recipe.input_heads, *recipe.final_shape)
+      outputs[f"{tensor_name}_weights"] = weights
+      outputs[f"{tensor_name}_hypotheses"] = means.reshape(hypothesis_shape)
+      outputs[f"{tensor_name}_stds_hypotheses"] = stds.reshape(hypothesis_shape)
+
+      picked_means = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
+      picked_stds = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
+      for batch_idx in range(weights.shape[0]):
+        for output_idx in range(recipe.output_heads):
+          order = np.argsort(weights[batch_idx, :, output_idx])[::-1]
+          picked_means[batch_idx, output_idx] = means[batch_idx, order[0]]
+          picked_stds[batch_idx, output_idx] = stds[batch_idx, order[0]]
+    else:
+      picked_means = means
+      picked_stds = stds
+
+    final_shape = ((reshaped.shape[0], recipe.output_heads, *recipe.final_shape)
+                   if recipe.output_heads > 1 else (reshaped.shape[0], *recipe.final_shape))
+    outputs[tensor_name] = picked_means.reshape(final_shape)
+    outputs[f"{tensor_name}_stds"] = picked_stds.reshape(final_shape)
+
+
+class ArchiveParser(_TensorKitchen):
+  def __init__(self, ignore_missing: bool = False):
+    super().__init__(ignore_missing=ignore_missing)
+    self._c = ModelConstants
+
+  def _recipes(self) -> list[tuple[str, _MixtureRecipe]]:
+    c = self._c
+    return [
+      ("plan", _MixtureRecipe(c.PLAN_MHP_N, c.PLAN_MHP_SELECTION, (c.IDX_N, c.PLAN_WIDTH))),
+      ("lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH))),
+      ("road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH))),
+      ("pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
+      ("road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
+      ("wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,))),
+      ("lead", _MixtureRecipe(c.LEAD_MHP_N, c.LEAD_MHP_SELECTION, (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH))),
+    ]
+
+  def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    c = self._c
+    for tensor_name, recipe in self._recipes():
+      self.mixture(outputs, tensor_name, recipe)
+    if "sim_pose" in outputs:
+      self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
+    if "lat_planner_solution" in outputs:
+      self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
+    if "desired_curvature" in outputs:
+      self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
+    for name in ("lead_prob", "lane_lines_prob", "meta"):
+      self.binary(outputs, name)
+    self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
+    self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
+    return outputs
+
+
+class PhaseParser(_TensorKitchen):
+  def __init__(self, ignore_missing: bool = False):
+    super().__init__(ignore_missing=ignore_missing)
+    self._c = SplitModelConstants
+
+  def _has_mixture_heads(self, outputs: dict[str, np.ndarray], tensor_name: str, flat_width: int) -> bool:
+    raw = self._grab(outputs, tensor_name)
+    if raw is None:
+      return False
+    return raw.shape[1] != 2 * flat_width
+
+  def _decode_dynamic_family(self, outputs: dict[str, np.ndarray]) -> None:
+    c = self._c
+    if "lead" in outputs:
+      uses_heads = self._has_mixture_heads(outputs, "lead", c.LEAD_MHP_SELECTION * c.LEAD_TRAJ_LEN * c.LEAD_WIDTH)
+      self.mixture(outputs, "lead", _MixtureRecipe(
+        c.LEAD_MHP_N if uses_heads else 0,
+        c.LEAD_MHP_SELECTION if uses_heads else 0,
+        (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH) if uses_heads else (c.LEAD_MHP_SELECTION, c.LEAD_TRAJ_LEN, c.LEAD_WIDTH),
+      ))
+
+    if "plan" in outputs:
+      uses_heads = self._has_mixture_heads(outputs, "plan", c.IDX_N * c.PLAN_WIDTH)
+      self.mixture(outputs, "plan", _MixtureRecipe(
+        c.PLAN_MHP_N if uses_heads else 0,
+        c.PLAN_MHP_SELECTION if uses_heads else 0,
+        (c.IDX_N, c.PLAN_WIDTH),
+      ))
+
+    if "planplus" in outputs:
+      self.mixture(outputs, "planplus", _MixtureRecipe(0, 0, (c.IDX_N, c.PLAN_WIDTH)))
+
+  def _decode_policy_family(self, outputs: dict[str, np.ndarray]) -> None:
+    c = self._c
+    if "action" in outputs:
+      self.mixture(outputs, "action", _MixtureRecipe(0, 0, (c.ACTION_WIDTH,)))
+    if "desired_curvature" in outputs:
+      self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
+    if "desire_pred" in outputs:
+      self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
+    if "desire_state" in outputs:
+      self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
+    if "lane_lines" in outputs:
+      self.mixture(outputs, "lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH)))
+    if "lane_lines_prob" in outputs:
+      self.binary(outputs, "lane_lines_prob")
+    if "lead_prob" in outputs:
+      self.binary(outputs, "lead_prob")
+    if "lat_planner_solution" in outputs:
+      self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
+    if "meta" in outputs:
+      self.binary(outputs, "meta")
+    if "road_edges" in outputs:
+      self.mixture(outputs, "road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH)))
+    if "sim_pose" in outputs:
+      self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
+
+  def parse_vision_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    c = self._c
+    self.mixture(outputs, "pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
+    self.mixture(outputs, "wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,)))
+    self.mixture(outputs, "road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
+    self._decode_dynamic_family(outputs)
+    self._decode_policy_family(outputs)
+    return outputs
+
+  def parse_policy_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self._decode_dynamic_family(outputs)
+    self._decode_policy_family(outputs)
+    return outputs
+
+  def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    return self.parse_policy_outputs(self.parse_vision_outputs(outputs))
+
+
+__all__ = [
+  "ArchiveParser",
+  "PhaseParser",
+]

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py

@@ -0,0 +1,23 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import to_mv
+
+_PTR_STRIDE = 8
+_RAW_GPU_PTR_SLOT = 20
+_RAW_GPU_PTR_VIEW_BYTES = 0x100
+
+
+def _descriptor_pointer(opencl_address: int) -> int:
+  return to_mv(opencl_address, _PTR_STRIDE).cast("Q")[0]
+
+
+def _raw_gpu_pointer(descriptor_pointer: int) -> int:
+  return to_mv(descriptor_pointer, _RAW_GPU_PTR_VIEW_BYTES).cast("Q")[_RAW_GPU_PTR_SLOT]
+
+
+def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
+  descriptor_pointer = _descriptor_pointer(opencl_address)
+  device_pointer = _raw_gpu_pointer(descriptor_pointer)
+  return Tensor.from_blob(device_pointer, shape, dtype=dtype, device="QCOM")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py

@@ -0,0 +1,131 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+
+DEFAULT_FRAME_SKIP = 4
+
+MODEL_INPUT_SPEC: dict[str, tuple[tuple[int, ...], str]] = {
+  "img": ((1, 12, 128, 256), "uint8"),
+  "big_img": ((1, 12, 128, 256), "uint8"),
+  "desire_pulse": ((1, 25, 8), "float32"),
+  "traffic_convention": ((1, 2), "float32"),
+  "features_buffer": ((1, 24, 512), "float32"),
+  "action_t": ((1, 2), "float32"),
+}
+
+
+def spec_from_meta(meta: dict) -> dict[str, tuple[tuple[int, ...], str]] | None:
+  shapes = meta.get("input_shapes")
+  if not shapes:
+    return None
+  return {name: (tuple(shape), "uint8" if name in ("img", "big_img") else "float32")
+          for name, shape in shapes.items()}
+
+
+class TemporalInputState:
+  def __init__(self, frame_skip: int, spec: dict[str, tuple[tuple[int, ...], str]] = MODEL_INPUT_SPEC):
+    self.frame_skip = frame_skip
+    img = spec["img"][0]
+    fb = spec["features_buffer"][0]
+    dp = spec["desire_pulse"][0]
+
+    self.n_frames = img[1] // 6
+    img_q_shape = (frame_skip * (self.n_frames - 1) + 1, 6, img[2], img[3])
+    self._img_shape = img
+    self._fb_shape = fb
+    self._dp_shape = dp
+    feat_dim = math.prod(fb[2:])
+
+    self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
+    self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
+    self.feat_q = np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32)
+    self.desire_q = np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32)
+    self.prev_desire = np.zeros(dp[2], dtype=np.float32)
+    self.prev_feat = np.zeros((fb[0], feat_dim), dtype=np.float32)
+
+  @staticmethod
+  def _shift_append(q: np.ndarray, new_val: np.ndarray) -> None:
+    q[:-1] = q[1:]
+    q[-1] = new_val
+
+  def push_and_materialize(self, warped: np.ndarray, desire_pulse: np.ndarray,
+                           traffic_convention: np.ndarray, action_t: np.ndarray,
+                           ) -> dict[str, np.ndarray]:
+    fs = self.frame_skip
+
+    cur = desire_pulse.astype(np.float32).copy()
+    cur[0] = 0
+    pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
+    self.prev_desire[:] = cur
+
+    self._shift_append(self.img_q, warped[0])
+    self._shift_append(self.big_img_q, warped[1])
+    self._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
+    self._shift_append(self.feat_q, self.prev_feat)
+
+    dp = self._dp_shape
+    return {
+      "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
+      "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
+      "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
+      "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
+      "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
+      "action_t": action_t.astype(np.float32).reshape(1, -1),
+    }
+
+  def note_hidden_state(self, model_output: np.ndarray, hidden_slice: slice) -> None:
+    self.prev_feat[:] = model_output[hidden_slice].reshape(self.prev_feat.shape)
+
+
+class SplitTemporalState:
+
+  def __init__(self, frame_skip: int, img_shape: tuple[int, ...],
+               feature_shape: tuple[int, ...], desire_shape: tuple[int, ...]):
+    self.frame_skip = frame_skip
+    self._img_shape = tuple(img_shape)
+    self._fb_shape = tuple(feature_shape)
+    self._dp_shape = tuple(desire_shape)
+
+    n_frames = img_shape[1] // 6
+    img_q_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
+    self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
+    self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
+    self.feat_q = np.zeros((frame_skip * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]),
+                           dtype=np.float32)
+    self.desire_q = np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]), dtype=np.float32)
+    self.prev_desire = np.zeros(desire_shape[2], dtype=np.float32)
+
+  def materialize_vision(self, warped: np.ndarray, desire: np.ndarray) -> dict[str, np.ndarray]:
+    fs = self.frame_skip
+    cur = desire.astype(np.float32).copy()
+    cur[0] = 0
+    pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
+    self.prev_desire[:] = cur
+
+    TemporalInputState._shift_append(self.img_q, warped[0])
+    TemporalInputState._shift_append(self.big_img_q, warped[1])
+    TemporalInputState._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
+    return {
+      "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
+      "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
+    }
+
+  def materialize_policy(self, vision_feature: np.ndarray, traffic_convention: np.ndarray,
+                         action_t: np.ndarray | None = None) -> dict[str, np.ndarray]:
+    fs = self.frame_skip
+    TemporalInputState._shift_append(self.feat_q, vision_feature.reshape(self._fb_shape[0], self._fb_shape[2]))
+    dp = self._dp_shape
+    out = {
+      "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
+      "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
+      "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
+    }
+    if action_t is not None:
+      out["action_t"] = action_t.astype(np.float32).reshape(1, -1)
+    return out

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py

@@ -0,0 +1,422 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+import argparse
+import atexit
+import math
+import os
+import pickle
+import re
+import tempfile
+import time
+from functools import partial
+from collections import namedtuple
+
+import numpy as np
+
+def _patch_tinygrad_fetch_fw():
+  import hashlib
+  import pathlib
+  import zstandard
+  from tinygrad import helpers
+  _orig = helpers.fetch_fw
+  def fetch_fw(path, name, sha256):
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
+      if hashlib.sha256(blob).hexdigest() == sha256:
+        return blob
+    return _orig(path, name, sha256)
+  helpers.fetch_fw = fetch_fw
+_patch_tinygrad_fetch_fw()
+
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import Context
+from tinygrad.device import Device
+from tinygrad.engine.jit import TinyJit
+
+
+NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
+WARP_INPUTS = ['tfm', 'big_tfm']
+POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
+
+UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
+UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
+
+WARP_DEV = os.getenv('WARP_DEV')
+
+
+def make_random_images(keys, shape, device=None):
+  return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
+
+
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
+  w_dst, h_dst = dst_shape
+  h_src, w_src = src_shape
+
+  x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
+
+  src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
+  src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
+  src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
+
+  src_x = src_x / src_w
+  src_y = src_y / src_w
+
+  x_round = Tensor.round(src_x)
+  y_round = Tensor.round(src_y)
+  x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
+  y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
+  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
+  sampled = src_flat[idx]
+
+  if border_fill_val is None:
+    return sampled
+
+  in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
+               (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
+  return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
+
+
+def frames_to_tensor(frames):
+  H = (frames.shape[0] * 2) // 3
+  W = frames.shape[1]
+  in_img1 = Tensor.cat(frames[0:H:2, 0::2],
+                       frames[1:H:2, 0::2],
+                       frames[0:H:2, 1::2],
+                       frames[1:H:2, 1::2],
+                       frames[H:H+H//4].reshape((H//2, W//2)),
+                       frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2))
+  return in_img1
+
+
+def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
+  cam_w, cam_h, stride, y_height, uv_height, _ = nv12
+  uv_offset = stride * y_height
+  stride_pad = stride - cam_w
+
+  def frame_prepare_tinygrad(input_frame, M_inv):
+    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
+    uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
+    with Context(SPLIT_REDUCEOP=0):
+      y = warp_perspective_tinygrad(input_frame[:cam_h*stride],
+                                    M_inv, (model_w, model_h),
+                                    (cam_h, cam_w), stride_pad).realize()
+      u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(),
+                                    M_inv_uv, (model_w//2, model_h//2),
+                                    (cam_h//2, cam_w//2), 0).realize()
+      v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(),
+                                    M_inv_uv, (model_w//2, model_h//2),
+                                    (cam_h//2, cam_w//2), 0).realize()
+    yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
+    tensor = frames_to_tensor(yuv)
+    return tensor
+  return frame_prepare_tinygrad
+
+
+def make_warp_input_queues(vision_input_shapes, frame_skip, device):
+  img = vision_input_shapes['img']  # (1, 12, 128, 256)
+  n_frames = img[1] // 6
+  img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
+
+  npy = {
+    'tfm': np.zeros((3, 3), dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32),
+  }
+  input_queues = {
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
+  }
+  return input_queues, npy
+
+
+def get_policy_npy_shapes(input_shapes):
+  dp = input_shapes['desire_pulse']  # (1, 25, 8)
+  tc = input_shapes['traffic_convention']  # (1, 2)
+  at = input_shapes['action_t']  # (1, 2)
+  fb = input_shapes['features_buffer']  # (1, 24, 512)
+  shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
+  return shapes, [math.prod(s) for s in shapes.values()]
+
+
+def make_input_queues(input_shapes, frame_skip, device):
+  input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
+
+  fb = input_shapes['features_buffer']  # (1, 24, 512), past features only; the model appends the current frame's feature
+  dp = input_shapes['desire_pulse']  # (1, 25, 8)
+
+  shapes, sizes = get_policy_npy_shapes(input_shapes)
+  packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
+  npy.update({k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)})
+  input_queues.update({
+    'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
+    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
+    'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
+  })
+  return input_queues, npy
+
+
+def shift_and_sample(buf, new_val, sample_fn):
+  buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
+  return sample_fn(buf)
+
+
+def sample_skip(buf, frame_skip):
+  return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
+
+
+def sample_desire(buf, frame_skip):
+  return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
+
+
+def make_warp(nv12, model_w, model_h, frame_skip):
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+
+  def warp(tfm, big_tfm, frame, big_frame):
+    tfm = tfm.to(WARP_DEV)
+    big_tfm = big_tfm.to(WARP_DEV)
+    Tensor.realize(tfm, big_tfm)
+
+    warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
+    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
+    return Tensor.cat(warped_frame, warped_big_frame)
+
+  return warp
+
+
+def make_run_policy(model_runner, model_metadata, frame_skip):
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
+
+  def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
+    packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
+    warped = warped.to(Device.DEFAULT)
+    Tensor.realize(packed_npy_inputs, warped)
+
+    img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn)
+
+    desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True))
+    desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
+    feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn)
+
+    inputs = {
+      'img': img,
+      'big_img': big_img,
+      'features_buffer': feat_buf,
+      'desire_pulse': desire_buf,
+      'traffic_convention': traffic_convention,
+      'action_t': action_t,
+    }
+    out = next(iter(model_runner(inputs).values())).cast('float32')
+    return out,
+  return run_policy
+
+
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
+  SEED = 42
+  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
+    input_queues, npy = make_queues(Device.DEFAULT)
+    np.random.seed(seed)
+    Tensor.manual_seed(seed)
+
+    testing = test_val is not None or test_buffers is not None
+    n_runs = 1 if testing else 3
+
+    for i in range(n_runs):
+      for v in npy.values():
+        v[:] = np.random.randn(*v.shape).astype(v.dtype)
+      Device.default.synchronize()
+      random_inputs = make_random_inputs()
+      st = time.perf_counter()
+      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
+      mt = time.perf_counter()
+      Device.default.synchronize()
+      et = time.perf_counter()
+      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
+
+      if i == 0:
+        val = [np.copy(v.numpy()) for v in outs]
+        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
+
+    if test_val is not None:
+      match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
+      assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
+    if test_buffers is not None:
+      match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
+      assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
+    return val, buffers
+
+  print('capture + replay')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
+  jit = pickle.loads(pickle.dumps(jit))
+  random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
+  random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
+  return jit
+
+
+def _captured_devices(jit) -> set[str]:
+  captured = getattr(jit, 'captured', None)
+  infos = getattr(captured, 'expected_input_info', None)
+  if not infos:
+    return set()
+
+  devices: set[str] = set()
+  for info in infos:
+    if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
+      devices.add(info[3])
+  return devices
+
+
+def _slice_outputs(model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
+  return {name: model_outputs[np.newaxis, tensor_slice] for name, tensor_slice in output_slices.items() if name != 'pad'}
+
+
+def _validate_pose_outputs(parsed_outputs: dict[str, np.ndarray]) -> None:
+  from iqpilot.selfdrive.locationd.locationd import MIN_STD_SANITY_CHECK, ROTATION_SANITY_CHECK, TRANS_SANITY_CHECK
+
+  required = (
+    'pose', 'pose_stds', 'wide_from_device_euler', 'wide_from_device_euler_stds',
+    'road_transform', 'road_transform_stds',
+  )
+  missing = [name for name in required if name not in parsed_outputs]
+  if missing:
+    raise AssertionError(f"parsed supercombo outputs missing required odometry tensors: {missing}")
+
+  for name in required:
+    values = parsed_outputs[name]
+    if not np.isfinite(values).all():
+      raise AssertionError(f"parsed supercombo output {name} contains non-finite values")
+
+  pose = parsed_outputs['pose'][0]
+  pose_stds = parsed_outputs['pose_stds'][0]
+  road_transform_stds = parsed_outputs['road_transform_stds'][0]
+  wide_stds = parsed_outputs['wide_from_device_euler_stds'][0]
+
+  if pose_stds.min() <= MIN_STD_SANITY_CHECK:
+    raise AssertionError(f"pose_stds min {pose_stds.min()} <= {MIN_STD_SANITY_CHECK}")
+  if road_transform_stds.min() <= MIN_STD_SANITY_CHECK:
+    raise AssertionError(f"road_transform_stds min {road_transform_stds.min()} <= {MIN_STD_SANITY_CHECK}")
+  if wide_stds.min() <= MIN_STD_SANITY_CHECK:
+    raise AssertionError(f"wide_from_device_euler_stds min {wide_stds.min()} <= {MIN_STD_SANITY_CHECK}")
+
+  if np.linalg.norm(pose[:3]) > TRANS_SANITY_CHECK:
+    raise AssertionError(f"pose translation norm {np.linalg.norm(pose[:3])} exceeds {TRANS_SANITY_CHECK}")
+  if np.linalg.norm(pose[3:]) > ROTATION_SANITY_CHECK:
+    raise AssertionError(f"pose rotation norm {np.linalg.norm(pose[3:])} exceeds {ROTATION_SANITY_CHECK}")
+  if np.linalg.norm(pose_stds[:3]) > 10 * TRANS_SANITY_CHECK:
+    raise AssertionError(
+      f"pose translation std norm {np.linalg.norm(pose_stds[:3])} exceeds {10 * TRANS_SANITY_CHECK}"
+    )
+  if np.linalg.norm(pose_stds[3:]) > 10 * ROTATION_SANITY_CHECK:
+    raise AssertionError(
+      f"pose rotation std norm {np.linalg.norm(pose_stds[3:])} exceeds {10 * ROTATION_SANITY_CHECK}"
+    )
+
+
+def validate_supercombo_release(run_policy_jit, model_runner, model_metadata, frame_skip, expected_device: str) -> None:
+  from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
+
+  direct_fn = make_run_policy(model_runner, model_metadata, frame_skip)
+  parser = PhaseParser()
+  queue_factory = partial(make_input_queues, model_metadata['input_shapes'], frame_skip)
+  image_shape = model_metadata['input_shapes']['img']
+
+  jit_queues, jit_npy = queue_factory(Device.DEFAULT)
+  direct_queues, direct_npy = queue_factory(Device.DEFAULT)
+
+  for payload in (jit_npy, direct_npy):
+    for name, value in payload.items():
+      value[:] = 0 if value.dtype.kind in ('i', 'u') else 0.0
+
+  zero_inputs = {
+    'warped': Tensor(np.zeros((2, 6, *image_shape[2:]), dtype=np.uint8), device=Device.DEFAULT).realize(),
+  }
+
+  direct_outs, = direct_fn(**{k: direct_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
+  jit_outs, = run_policy_jit(**{k: jit_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
+
+  direct_flat = direct_outs.numpy().astype(np.float32).reshape(-1)
+  jit_flat = jit_outs.numpy().astype(np.float32).reshape(-1)
+
+  if not np.allclose(direct_flat, jit_flat, atol=1e-4, rtol=1e-4):
+    max_delta = float(np.max(np.abs(direct_flat - jit_flat)))
+    raise AssertionError(f"JIT supercombo output diverges from direct ONNX execution; max abs delta {max_delta}")
+
+  parsed = parser.parse_vision_outputs(_slice_outputs(jit_flat, model_metadata['output_slices']))
+  _validate_pose_outputs(parsed)
+
+  captured_devices = _captured_devices(run_policy_jit)
+  if expected_device and captured_devices and expected_device not in captured_devices:
+    raise AssertionError(
+      f"compiled run_policy backend mismatch: captured {sorted(captured_devices)} expected {expected_device}"
+    )
+
+
+def _parse_size(s):
+  w, h = s.lower().split('x')
+  return int(w), int(h)
+
+
+def read_file_chunked_to_shm(path):
+  from iqpilot.common.file_chunker import read_file_chunked
+  from iqpilot.system.hardware.hw import Paths
+  with tempfile.NamedTemporaryFile(prefix='compile_modeld_', dir=Paths.shm_path(), delete=False) as f:
+    f.write(read_file_chunked(path))
+    tmp_path = f.name
+  atexit.register(lambda: os.path.exists(tmp_path) and os.remove(tmp_path))
+  return tmp_path
+
+
+if __name__ == "__main__":
+  from tinygrad.nn.onnx import OnnxRunner
+  from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from iqpilot.selfdrive.iqmodeld.metadata import build_metadata_record
+  p = argparse.ArgumentParser()
+  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True,
+                 help='camera resolutions WxH (one or more)')
+  p.add_argument('--onnx', required=True)
+  p.add_argument('--output', required=True)
+  p.add_argument('--frame-skip', type=int, required=True)
+  p.add_argument('--expected-device', default='QCOM', help='expected tinygrad backend baked into the JIT')
+  args = p.parse_args()
+
+  model_path = read_file_chunked_to_shm(args.onnx)
+  model_w, model_h = args.model_size
+
+  model_runner = OnnxRunner(model_path)
+  out = {
+    'metadata': build_metadata_record(model_path),
+    'frame_skip': args.frame_skip,
+  }
+
+  run_policy_jit = TinyJit(make_run_policy(model_runner, out['metadata'], args.frame_skip), prune=True)
+
+  make_policy_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip)
+  make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, *out['metadata']['input_shapes']['img'][2:]))
+  out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
+                                  make_policy_queues)
+  validate_supercombo_release(out['run_policy'], model_runner, out['metadata'], args.frame_skip, args.expected_device)
+
+  for cam_w, cam_h in args.camera_resolutions:
+    nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+    make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
+    warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
+    make_warp_queues = partial(make_warp_input_queues, out['metadata']['input_shapes'], args.frame_skip)
+    out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
+    captured_devices = _captured_devices(out[(cam_w,cam_h)])
+    if args.expected_device and captured_devices and args.expected_device not in captured_devices:
+      raise AssertionError(
+        f"compiled warp backend mismatch for {cam_w}x{cam_h}: captured {sorted(captured_devices)} expected {args.expected_device}"
+      )
+
+  with open(args.output, "wb") as f:
+    pickle.dump(out, f)
+  print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+Compile the backend-neutral warp-only artifact: NV12 camera frames + 3x3
+transforms -> (2, 6, model_h/2, model_w/2) uint8 warped tensor, on the device
+GPU (QCOM). maciqmodeld runs this locally
+and feed the output to their backend, so the big model's image pipeline is
+bit-identical to comma's fused pkl warp stage.
+
+Run ON the device (needs the QCOM backend):
+  cd /data/openpilot && DEV=QCOM WARP_DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 \
+    python3 iqpilot/selfdrive/iqmodeld/tools/compile_warp.py \
+    --camera-resolutions 1928x1208 --output /data/models/emac_warp.pkl
+The artifact is then split per-resolution into Paths.model_root().
+"""
+from __future__ import annotations
+
+import argparse
+import hashlib
+import os
+import pickle
+from functools import partial
+
+import numpy as np
+
+SELFTEST_SEED = 20260817
+
+from iqpilot.selfdrive.iqmodeld.temporal_state import DEFAULT_FRAME_SKIP, MODEL_INPUT_SPEC
+from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import (
+  NV12Frame, WARP_INPUTS, compile_jit, make_random_images, make_warp, make_warp_input_queues,
+)
+
+MODEL_SIZE = (MODEL_INPUT_SPEC["img"][0][3] * 2, MODEL_INPUT_SPEC["img"][0][2] * 2)  # (512, 256)
+
+
+def _parse_size(s: str) -> tuple[int, int]:
+  w, h = s.lower().split("x")
+  return int(w), int(h)
+
+
+def compile_warp(cam_w: int, cam_h: int, out_path: str | None = None,
+                 frame_skip: int = DEFAULT_FRAME_SKIP) -> str:
+  """Compile the warp-only QCOM JIT for one camera resolution and write the pkl.
+  Returns the artifact path. Callable from the workers so a fresh device
+  self-provisions the warp instead of erroring — needs the QCOM backend."""
+  # the QCOM warp env must be set before tinygrad is imported here
+  os.environ.setdefault("DEV", "QCOM")
+  os.environ.setdefault("WARP_DEV", "QCOM")
+  os.environ.setdefault("IMAGE", "1")
+  os.environ.setdefault("FLOAT16", "1")
+  os.environ.setdefault("NOLOCALS", "1")
+  os.environ.setdefault("JIT_BATCH_SIZE", "0")
+  from tinygrad.engine.jit import TinyJit
+  from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from iqpilot.system.hardware.hw import Paths
+
+  model_w, model_h = MODEL_SIZE
+  input_shapes = {name: shape for name, (shape, _) in MODEL_INPUT_SPEC.items()}
+  nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+  make_random_warp_inputs = partial(make_random_images, keys=["frame", "big_frame"],
+                                    shape=nv12.size, device=os.getenv("WARP_DEV"))
+  warp_jit = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True)
+  make_warp_queues = partial(make_warp_input_queues, input_shapes, frame_skip)
+  compiled = compile_jit(warp_jit, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
+
+  # historical artifact name: already-provisioned devices keep their warp
+  out_path = out_path or os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
+  os.makedirs(os.path.dirname(out_path), exist_ok=True)
+  tmp = out_path + ".part"
+  bundle = {(cam_w, cam_h): compiled, "frame_skip": frame_skip, "model_size": MODEL_SIZE}
+  bundle["selftest"] = selftest_digest(compiled, cam_w, cam_h, nv12.size)
+  with open(tmp, "wb") as f:
+    pickle.dump(bundle, f)
+  os.replace(tmp, out_path)  # atomic: a reader never sees a half-written pkl
+  return out_path
+
+
+def main() -> None:
+  p = argparse.ArgumentParser()
+  p.add_argument("--camera-resolutions", type=_parse_size, nargs="+", default=[(1928, 1208)])
+  p.add_argument("--output", default=None)
+  p.add_argument("--frame-skip", type=int, default=DEFAULT_FRAME_SKIP)
+  args = p.parse_args()
+  for cam_w, cam_h in args.camera_resolutions:
+    out = compile_warp(cam_w, cam_h, args.output, frame_skip=args.frame_skip)
+    print(f"saved warp JIT to {out} ({os.path.getsize(out) / 1e6:.2f} MB)")
+
+
+if __name__ == "__main__":
+  main()
+
+
+def selftest_inputs(cam_w: int, cam_h: int, nv12_size: int):
+  """A fixed synthetic frame pair and pair of matrices. Deterministic so the
+  digest is reproducible on the device that compiled the artifact."""
+  rng = np.random.default_rng(SELFTEST_SEED)
+  frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
+  big_frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
+  tfm = np.array([[0.7, 0.02, 300.0], [0.01, 0.7, 240.0], [0.0, 0.0, 1.0]], dtype=np.float32)
+  big_tfm = np.array([[0.5, 0.01, 380.0], [0.02, 0.5, 300.0], [0.0, 0.0, 1.0]], dtype=np.float32)
+  return frame, big_frame, tfm, big_tfm
+
+
+def selftest_digest(compiled, cam_w: int, cam_h: int, nv12_size: int) -> str:
+  """Hash the warp's output for a fixed input.
+
+  A warp artifact pinned to one tinygrad can still unpickle under another and
+  then compute silently wrong, which reaches the model as a garbage image and
+  looks like a bad model rather than a stale artifact. A version string cannot
+  see that; running it can."""
+  from tinygrad.tensor import Tensor
+  frame, big_frame, tfm, big_tfm = selftest_inputs(cam_w, cam_h, nv12_size)
+  dev = os.getenv("WARP_DEV") or "QCOM"
+  out = compiled(tfm=Tensor(tfm, device="NPY").realize(),
+                 big_tfm=Tensor(big_tfm, device="NPY").realize(),
+                 frame=Tensor(frame, device=dev).realize(),
+                 big_frame=Tensor(big_frame, device=dev).realize())
+  return hashlib.sha256(out.numpy().astype(np.uint8).tobytes()).hexdigest()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py

@@ -0,0 +1,32 @@
+import math
+
+import numpy as np
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
+
+
+def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None:
+  if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
+    return None
+
+  if len(live_calib.rpyCalib) != 3:
+    return None
+
+  calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
+  return calib_rpy if np.isfinite(calib_rpy).all() else None
+
+
+def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float:
+  if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
+    return float(HEIGHT_INIT[0])
+
+  if len(live_calib.height) != 1:
+    return float(HEIGHT_INIT[0])
+
+  height = float(live_calib.height[0])
+  if not math.isfinite(height):
+    return float(HEIGHT_INIT[0])
+  if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX):
+    return float(HEIGHT_INIT[0])
+  return height

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py

@@ -0,0 +1,367 @@
+#!/usr/bin/env python3
+'''
+This process finds calibration values. More info on what these calibration values
+are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
+While the roll calibration is a real value that can be estimated, here we assume it's zero,
+and the image input into the neural network is not corrected for roll.
+'''
+
+import os
+import capnp
+import numpy as np
+from typing import NoReturn
+
+from iqpilot.cereal import log, car
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.issue_debug import log_issue_limited
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.system.hardware import HARDWARE
+
+MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
+MAX_VEL_ANGLE_STD = np.radians(0.25)
+MAX_YAW_RATE_FILTER = np.radians(2)  # per second
+
+MAX_HEIGHT_STD = np.exp(-3.5)
+
+# This is at model frequency, blocks needed for efficiency
+SMOOTH_CYCLES = 10
+BLOCK_SIZE = 100
+INPUTS_NEEDED = 5   # Minimum blocks needed for valid calibration
+INPUTS_WANTED = 50   # We want a little bit more than we need for stability
+MAX_ALLOWED_YAW_SPREAD = np.radians(2)
+MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
+TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3)
+RPY_INIT = np.array([0.0,0.0,0.0])
+WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
+HEIGHT_INIT = np.array([1.22])
+HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0
+DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi")
+
+# These values are needed to accommodate the model frame in the narrow cam
+if HARDWARE.get_device_type() == 'mici':
+  PITCH_LIMITS = np.array([-0.143101, 0.22235988])
+else:
+  PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
+YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
+DEBUG = os.getenv("DEBUG") is not None
+
+def is_calibration_valid(rpy: np.ndarray) -> bool:
+  return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
+
+
+def sanity_clip(rpy: np.ndarray) -> np.ndarray:
+  if np.isnan(rpy).any():
+    rpy = RPY_INIT
+  return np.array([rpy[0],
+                   np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
+                   np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
+
+def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
+  return (idx*prev_mean + (block_size - idx) * new_val) / block_size
+
+class Calibrator:
+  def __init__(self, param_put: bool = False):
+    self.param_put = param_put
+
+    self.not_car = False
+    self.stable_rpy = RPY_INIT.copy()
+    self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
+    self.stable_height = HEIGHT_INIT.copy()
+    self.has_stable_snapshot = False
+
+    # Read saved calibration
+    self.params = Params()
+    calibration_params = self.params.get("CalibrationParams")
+    rpy_init = RPY_INIT
+    wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
+    height = HEIGHT_INIT
+    valid_blocks = 0
+    self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
+
+    if param_put and calibration_params:
+      try:
+        with log.Event.from_bytes(calibration_params) as msg:
+          rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
+          valid_blocks = msg.extrinsicsCalibration.validBlocks
+          wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
+          height = np.array(msg.extrinsicsCalibration.height)
+      except Exception:
+        cloudlog.exception("Error reading cached CalibrationParams")
+
+    self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
+    self.update_status()
+
+    # If saved calibration is immediately invalid (e.g. bad params from a previous
+    # bootstrap bug or device remount), auto-clear it so we recalibrate from scratch
+    # instead of getting permanently stuck in the "Calibration Invalid" state.
+    if self.cal_status == log.ExtrinsicsCalibration.Status.invalid:
+      cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh")
+      if param_put:
+        self.params.remove("CalibrationParams")
+      self.reset()
+      self.update_status()
+
+  def _remember_stable_solution(self) -> None:
+    self.stable_rpy = self.rpy.copy()
+    self.stable_wide_from_device_euler = self.wide_from_device_euler.copy()
+    self.stable_height = self.height.copy()
+    self.has_stable_snapshot = True
+
+  def reset(self, rpy_init: np.ndarray = RPY_INIT,
+                  valid_blocks: int = 0,
+                  wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
+                  height_init: np.ndarray = HEIGHT_INIT,
+                  smooth_from: np.ndarray | None = None) -> None:
+    if not np.isfinite(rpy_init).all():
+      self.rpy = RPY_INIT.copy()
+    else:
+      self.rpy = rpy_init.copy()
+
+    if not np.isfinite(height_init).all() or len(height_init) != 1:
+      self.height = HEIGHT_INIT.copy()
+    else:
+      self.height = height_init.copy()
+
+    if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
+      self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
+    else:
+      self.wide_from_device_euler = wide_from_device_euler_init.copy()
+
+    if not np.isfinite(valid_blocks) or valid_blocks < 0:
+      self.valid_blocks = 0
+    else:
+      self.valid_blocks = valid_blocks
+
+    self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
+    self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
+    self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
+
+    self.idx = 0
+    self.block_idx = 0
+    self.v_ego = 0.0
+
+    if smooth_from is None:
+      self.old_rpy = RPY_INIT
+      self.old_rpy_weight = 0.0
+    else:
+      self.old_rpy = smooth_from
+      self.old_rpy_weight = 1.0
+
+  def get_valid_idxs(self) -> list[int]:
+    # exclude current block_idx from validity window
+    before_current = list(range(self.block_idx))
+    after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
+    return before_current + after_current
+
+  def update_status(self) -> None:
+    valid_idxs = self.get_valid_idxs()
+    if valid_idxs:
+      self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
+      self.height = np.mean(self.heights[valid_idxs], axis=0)
+      rpys = self.rpys[valid_idxs]
+      self.rpy = np.mean(rpys, axis=0)
+      max_rpy_calib = np.array(np.max(rpys, axis=0))
+      min_rpy_calib = np.array(np.min(rpys, axis=0))
+      self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
+    else:
+      self.calib_spread = np.zeros(3)
+
+    if self.valid_blocks < INPUTS_NEEDED:
+      if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
+        self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
+      else:
+        self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
+    elif is_calibration_valid(self.rpy):
+      self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
+    else:
+      self.cal_status = log.ExtrinsicsCalibration.Status.invalid
+
+    # If spread is too high, assume mounting was changed and reset to last block.
+    # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
+    # TODO: add height spread check with smooth transition too
+    pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD
+    spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
+    if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high:
+      self._remember_stable_solution()
+
+    if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
+      use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot
+      if use_stable_snapshot:
+        reset_rpy = self.stable_rpy
+        reset_wide = self.stable_wide_from_device_euler
+        reset_height = self.stable_height
+      else:
+        reset_rpy = self.rpys[self.block_idx - 1]
+        reset_wide = self.wide_from_device_eulers[self.block_idx - 1]
+        reset_height = self.heights[self.block_idx - 1]
+
+      log_issue_limited(
+        "calibrationd_reset_spread",
+        "calibration",
+        f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} "
+        f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} "
+        f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}",
+        interval_sec=0.5,
+      )
+      self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide,
+                 height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy)
+      self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
+
+    write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
+    if self.param_put and write_this_cycle:
+      self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
+
+  def handle_v_ego(self, v_ego: float) -> None:
+    self.v_ego = v_ego
+
+  def get_smooth_rpy(self) -> np.ndarray:
+    if self.old_rpy_weight > 0:
+      return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
+    else:
+      return self.rpy
+
+  def handle_cam_odom(self, trans: list[float],
+                            rot: list[float],
+                            wide_from_device_euler: list[float],
+                            trans_std: list[float],
+                            road_transform_trans: list[float],
+                            road_transform_trans_std: list[float]) -> np.ndarray | None:
+    self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
+
+    fast_enough = self.v_ego > MIN_SPEED_FILTER
+    motion_speed = max(float(self.v_ego), float(trans[0]))
+    cam_fast_enough = motion_speed > MIN_SPEED_FILTER
+    yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER
+    straight_and_fast = fast_enough and cam_fast_enough and yaw_ok
+    angle_std_threshold = MAX_VEL_ANGLE_STD
+    height_std_threshold = MAX_HEIGHT_STD
+    rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold
+    if len(road_transform_trans_std) == 3:
+      height_certain = road_transform_trans_std[2] < height_std_threshold
+    else:
+      height_certain = True
+
+    certain_if_calib = rpy_certain
+    if not (straight_and_fast and certain_if_calib):
+      log_issue_limited(
+        "calibrationd_rejected_sample",
+        "calibration",
+        f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} "
+        f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} "
+        f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}",
+        interval_sec=1.0,
+      )
+      return None
+
+    observed_rpy = np.array([0,
+                             -np.arctan2(trans[2], trans[0]),
+                             np.arctan2(trans[1], trans[0])])
+    new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
+    new_rpy = sanity_clip(new_rpy)
+
+    if len(wide_from_device_euler) == 3:
+      new_wide_from_device_euler = np.array(wide_from_device_euler)
+    else:
+      new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
+
+    if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX:
+      new_height = np.array([road_transform_trans[2]])
+    else:
+      new_height = HEIGHT_INIT
+
+    self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
+    self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
+                                                                                new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
+    self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
+
+    self.idx = (self.idx + 1) % BLOCK_SIZE
+    if self.idx == 0:
+      self.block_idx += 1
+      self.valid_blocks = max(self.block_idx, self.valid_blocks)
+      self.block_idx = self.block_idx % INPUTS_WANTED
+
+    self.update_status()
+
+    if self.idx == 0:
+      log_issue_limited(
+        "calibrationd_progress_block",
+        "calibration",
+        f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} "
+        f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} "
+        f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}",
+        interval_sec=0.5,
+      )
+
+    return new_rpy
+
+  def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
+    smooth_rpy = self.get_smooth_rpy()
+
+    msg = messaging.new_message('extrinsicsCalibration')
+    msg.valid = valid
+
+    extrinsicsCalibration = msg.extrinsicsCalibration
+    extrinsicsCalibration.validBlocks = self.valid_blocks
+    extrinsicsCalibration.calStatus = self.cal_status
+    extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
+    extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
+    extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
+    extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
+    extrinsicsCalibration.height = self.height.tolist()
+
+    return msg
+
+  def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
+    pm.send('extrinsicsCalibration', self.get_msg(valid))
+
+
+def main() -> NoReturn:
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  pm = messaging.PubMaster(['extrinsicsCalibration'])
+  sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
+
+  params_reader = Params()
+  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
+
+  calibrator = Calibrator(param_put=True)
+  calibrator.not_car = CP.notCar
+
+  while 1:
+    timeout = 0 if sm.frame == -1 else 100
+    sm.update(timeout)
+
+    if sm.updated['cameraOdometry']:
+      calibrator.handle_v_ego(sm['carState'].vEgo)
+      new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
+                                           sm['cameraOdometry'].rot,
+                                           sm['cameraOdometry'].wideFromDeviceEuler,
+                                           sm['cameraOdometry'].transStd,
+                                           sm['cameraOdometry'].roadTransformTrans,
+                                           sm['cameraOdometry'].roadTransformTransStd)
+
+      if DEBUG and new_rpy is not None:
+        print('got new rpy', new_rpy)
+
+    # 4Hz driven by cameraOdometry
+    if sm.frame % 5 == 0:
+      checks_ok = sm.all_checks()
+      if not checks_ok:
+        ft = sm.freq_tracker
+        recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services}
+        log_issue_limited(
+          "calibrationd_checks_failed",
+          "calibration",
+          f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} "
+          f"seen={sm.seen} recv_hz={recv_hz}",
+          interval_sec=5.0,
+        )
+      calibrator.send_data(pm, checks_ok)
+
+
+if __name__ == "__main__":
+  main()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py

@@ -0,0 +1,188 @@
+import numpy as np
+from typing import Any
+from functools import cache
+
+from iqpilot.cereal import log
+from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+
+
+@cache
+def fft_next_good_size(n: int) -> int:
+    """
+    smallest composite of 2, 3, 5, 7, 11 that is >= n
+    inspired by pocketfft
+    """
+    if n <= 6:
+      return n
+    best, f2 = 2 * n, 1
+    while f2 < best:
+        f23 = f2
+        while f23 < best:
+            f235 = f23
+            while f235 < best:
+                f2357 = f235
+                while f2357 < best:
+                    f235711 = f2357
+                    while f235711 < best:
+                        best = f235711 if f235711 >= n else best
+                        f235711 *= 11
+                    f2357 *= 7
+                f235 *= 5
+            f23 *= 3
+        f2 *= 2
+    return best
+
+
+def parabolic_peak_interp(R, max_index):
+  if max_index == 0 or max_index == len(R) - 1:
+    return max_index
+
+  y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1]
+  offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1)
+
+  return max_index + offset
+
+
+def rotate_cov(rot_matrix, cov_in):
+  return rot_matrix @ cov_in @ rot_matrix.T
+
+
+def rotate_std(rot_matrix, std_in):
+  return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2))))
+
+
+class NPQueue:
+  def __init__(self, maxlen: int, rowsize: int) -> None:
+    self.maxlen = maxlen
+    self.arr = np.empty((0, rowsize))
+
+  def __len__(self) -> int:
+    return len(self.arr)
+
+  def append(self, pt: list[float]) -> None:
+    if len(self.arr) < self.maxlen:
+      self.arr = np.append(self.arr, [pt], axis=0)
+    else:
+      self.arr[:-1] = self.arr[1:]
+      self.arr[-1] = pt
+
+
+class PointBuckets:
+  def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
+    self.x_bounds = x_bounds
+    self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
+    self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
+    self.min_points_total = min_points_total
+
+  def __len__(self) -> int:
+    return sum([len(v) for v in self.buckets.values()])
+
+  def is_valid(self) -> bool:
+    individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
+    total_points_valid = self.__len__() >= self.min_points_total
+    return individual_buckets_valid and total_points_valid
+
+  def get_valid_percent(self) -> int:
+    total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
+    individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
+                                      zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
+    return int((total_points_perc + individual_buckets_perc) / 2)
+
+  def is_calculable(self) -> bool:
+    return all(len(v) > 0 for v in self.buckets.values())
+
+  def add_point(self, x: float, y: float) -> None:
+    raise NotImplementedError
+
+  def get_points(self, num_points: int | None = None) -> Any:
+    points = np.vstack([x.arr for x in self.buckets.values()])
+    if num_points is None:
+      return points
+    return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
+
+  def load_points(self, points: list[list[float]]) -> None:
+    for point in points:
+      self.add_point(*point)
+
+
+class ParameterEstimator:
+  """ Base class for parameter estimators """
+  def reset(self) -> None:
+    raise NotImplementedError
+
+  def handle_log(self, t: int, which: str, msg: log.Event) -> None:
+    raise NotImplementedError
+
+  def get_msg(self, valid: bool, with_points: bool) -> log.Event:
+    raise NotImplementedError
+
+
+class Measurement:
+  x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
+  x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
+  roll, pitch, yaw = x, y, z
+  roll_std, pitch_std, yaw_std = x_std, y_std, z_std
+
+  def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
+    self.xyz: np.ndarray = xyz
+    self.xyz_std: np.ndarray = xyz_std
+
+  @classmethod
+  def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
+    return cls(
+      xyz=np.array([measurement.x, measurement.y, measurement.z]),
+      xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
+    )
+
+
+class Pose:
+  def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
+    self.orientation = orientation
+    self.velocity = velocity
+    self.acceleration = acceleration
+    self.angular_velocity = angular_velocity
+
+  @classmethod
+  def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose':
+    return Pose(
+      orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
+      velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
+      acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
+      angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
+    )
+
+
+class PoseCalibrator:
+  def __init__(self):
+    self.calib_valid = False
+    self.calib_from_device = np.eye(3)
+
+  def _transform_calib_from_device(self, meas: Measurement):
+    new_xyz = self.calib_from_device @ meas.xyz
+    new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
+    return Measurement(new_xyz, new_xyz_std)
+
+  def _ned_from_calib(self, orientation: Measurement):
+    ned_from_device = rot_from_euler(orientation.xyz)
+    ned_from_calib = ned_from_device @ self.calib_from_device.T
+    ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
+    return ned_from_calib_euler_meas
+
+  def build_calibrated_pose(self, pose: Pose) -> Pose:
+    ned_from_calib_euler = self._ned_from_calib(pose.orientation)
+    angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
+    acceleration_calib = self._transform_calib_from_device(pose.acceleration)
+    velocity_calib = self._transform_calib_from_device(pose.velocity)
+
+    return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
+
+  def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration):
+    calib_rpy = get_calibrated_rpy(live_calib)
+    if calib_rpy is not None:
+      self.calib_from_device = rot_from_euler(calib_rpy).T
+      self.calib_valid = True
+    else:
+      if not self.calib_valid:
+        self.calib_from_device = np.eye(3)
+      self.calib_valid = False

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py

@@ -0,0 +1,336 @@
+#!/usr/bin/env python3
+import os
+import time
+import capnp
+import numpy as np
+from enum import Enum
+from collections import defaultdict
+
+from iqpilot.cereal import log, messaging
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.common.transformations.orientation import rot_from_euler
+from iqpilot.common.realtime import config_realtime_process
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from iqpilot.selfdrive.locationd.helpers import rotate_std
+from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+
+ACCEL_SANITY_CHECK = 100.0  # m/s^2
+ROTATION_SANITY_CHECK = 10.0  # rad/s
+TRANS_SANITY_CHECK = 200.0  # m/s
+CALIB_RPY_SANITY_CHECK = 0.5  # rad (+- 30 deg)
+MIN_STD_SANITY_CHECK = 1e-5  # m or rad
+MAX_FILTER_REWIND_TIME = 0.8  # s
+MAX_SENSOR_TIME_DIFF = 0.1  # s
+YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
+INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
+INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
+POSENET_STD_INITIAL_VALUE = 10.0
+POSENET_STD_HIST_HALF = 20
+
+
+def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
+  return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
+
+
+def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
+  assert len(values) == len(stds) == 3
+  measurement.x, measurement.y, measurement.z = map(float, values)
+  measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
+  measurement.valid = valid
+
+
+class HandleLogResult(Enum):
+  SUCCESS = 0
+  TIMING_INVALID = 1
+  INPUT_INVALID = 2
+  SENSOR_SOURCE_INVALID = 3
+
+
+class LocationEstimator:
+  def __init__(self, debug: bool):
+    self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
+
+    self.debug = debug
+
+    self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
+    self.car_speed = 0.0
+    self.camodo_yawrate_distribution = np.array([0.0, 10.0])  # mean, std
+    self.device_from_calib = np.eye(3)
+
+    obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
+    self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
+    self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
+
+  def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
+    self.kf.init_state(x_initial, covs=P_initial, filter_time=t)
+
+  def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
+    # some segments have two IMUs, ignore the second one
+    return source != log.SensorEventData.SensorSource.bmx055
+
+  def _validate_sensor_time(self, sensor_time: float, t: float):
+    # ignore empty readings
+    if sensor_time == 0:
+      return False
+
+    # sensor time and log time should be close
+    sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
+    if sensor_time_invalid:
+      cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
+    return not sensor_time_invalid
+
+  def _validate_timestamp(self, t: float):
+    kf_t = self.kf.t
+    invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
+    if invalid:
+      cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
+    return not invalid
+
+  def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
+    all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
+    if not all_finite:
+      cloudlog.error("Non-finite values detected, kalman reset")
+      self.reset(t)
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
+    new_x, new_P = None, None
+    if which == "accelerometer" and msg.which() == "acceleration":
+      sensor_time = msg.timestamp * 1e-9
+
+      if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
+        return HandleLogResult.TIMING_INVALID
+
+      if not self._validate_sensor_source(msg.source):
+        return HandleLogResult.SENSOR_SOURCE_INVALID
+
+      v = msg.acceleration.v
+      meas = np.array([-v[2], -v[1], -v[0]])
+      if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
+      if acc_res is not None:
+        _, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
+        self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
+        self.observations[ObservationKind.PHONE_ACCEL] = meas
+
+    elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
+      sensor_time = msg.timestamp * 1e-9
+
+      if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
+        return HandleLogResult.TIMING_INVALID
+
+      if not self._validate_sensor_source(msg.source):
+        return HandleLogResult.SENSOR_SOURCE_INVALID
+
+      v = msg.gyroUncalibrated.v
+      meas = np.array([-v[2], -v[1], -v[0]])
+
+      gyro_bias = self.kf.x[States.GYRO_BIAS]
+      gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
+      gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
+      gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
+
+      if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
+        return HandleLogResult.INPUT_INVALID
+
+      gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
+      if gyro_res is not None:
+        _, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
+        self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
+        self.observations[ObservationKind.PHONE_GYRO] = meas
+
+    elif which == "carState":
+      self.car_speed = abs(msg.vEgo)
+
+    elif which == "extrinsicsCalibration":
+      # Note that we use this message during calibration
+      calib = get_calibrated_rpy(msg)
+      if calib is None and len(msg.rpyCalib) > 0:
+        calib = np.array(msg.rpyCalib)
+
+      if calib is not None:
+        if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
+          return HandleLogResult.INPUT_INVALID
+
+        self.device_from_calib = rot_from_euler(calib)
+
+    elif which == "cameraOdometry":
+      if not self._validate_timestamp(t):
+        return HandleLogResult.TIMING_INVALID
+
+      rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
+      trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
+
+      if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      rot_calib_std = np.array(msg.rotStd)
+      trans_calib_std = np.array(msg.transStd)
+
+      if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
+        return HandleLogResult.INPUT_INVALID
+
+      self.posenet_stds = np.roll(self.posenet_stds, -1)
+      self.posenet_stds[-1] = trans_calib_std[0]
+
+      # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
+      rot_calib_std *= 10
+      trans_calib_std *= 2
+
+      rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
+      trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
+      rot_device_noise = rot_device_std ** 2
+      trans_device_noise = trans_device_std ** 2
+
+      cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
+      cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
+      self.camodo_yawrate_distribution =  np.array([rot_device[2], rot_device_std[2]])
+      if cam_odo_rot_res is not None:
+        _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
+        self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
+        self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
+      if cam_odo_trans_res is not None:
+        _, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
+        self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
+        self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
+
+    if new_x is not None and new_P is not None:
+      self._finite_check(t, new_x, new_P)
+    return HandleLogResult.SUCCESS
+
+  def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
+    state, cov = self.kf.x, self.kf.P
+    std = np.sqrt(np.diag(cov))
+
+    orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
+    velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
+    angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
+    acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
+
+    msg = messaging.new_message("deviceMotion")
+    msg.valid = filter_valid
+
+    deviceMotion = msg.deviceMotion
+    init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
+    init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
+    init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
+    init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
+    if self.debug:
+      deviceMotion.debugFilterState.value = state.tolist()
+      deviceMotion.debugFilterState.std = std.tolist()
+      deviceMotion.debugFilterState.valid = filter_valid
+      deviceMotion.debugFilterState.observations = [
+        {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
+        for k in self.observations.keys()
+      ]
+
+    old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
+    new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
+    std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
+
+    deviceMotion.inputsOK = inputs_valid
+    deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
+    deviceMotion.sensorsOK = sensors_valid
+
+    return msg
+
+
+def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
+  cur_time = time.monotonic()
+  for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
+    if len(msgs) > 0:
+      sensor_valid[which] = msgs[-1].valid
+      sensor_recv_time[which] = cur_time
+
+    if not simulation:
+      sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
+    else:
+      sensor_alive[which] = len(msgs) > 0
+
+  return all(sensor_alive.values()) and all(sensor_valid.values())
+
+
+def main():
+  config_realtime_process([0, 1, 2, 3], 5)
+
+  DEBUG = bool(int(os.getenv("DEBUG", "0")))
+  SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
+
+  pm = messaging.PubMaster(['deviceMotion'])
+  sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
+  # separate sensor sockets for efficiency
+  sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
+  sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
+
+  params = Params()
+
+  estimator = LocationEstimator(DEBUG)
+
+  filter_initialized = False
+  critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
+  observation_input_invalid = defaultdict(int)
+
+  input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
+  input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
+  input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
+
+  initial_pose_data = params.get("LocationFilterInitialState")
+  if initial_pose_data is not None:
+    with log.Event.from_bytes(initial_pose_data) as lp_msg:
+      filter_state = lp_msg.deviceMotion.debugFilterState
+      x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
+      P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
+      estimator.reset(None, x_initial, P_initial)
+
+  while True:
+    sm.update()
+
+    acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
+
+    if filter_initialized:
+      msgs = []
+      for msg in acc_msgs + gyro_msgs:
+        t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
+        msgs.append((t, valid, which, data))
+      for which, updated in sm.updated.items():
+        if not updated:
+          continue
+        t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
+        msgs.append((t, valid, which, data))
+
+      for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
+        if valid:
+          t = log_mono_time * 1e-9
+          res = estimator.handle_log(t, which, msg)
+          if which not in critcal_services:
+            continue
+
+          if res == HandleLogResult.TIMING_INVALID:
+            cloudlog.warning(f"Observation {which} ignored due to failed timing check")
+            observation_input_invalid[which] += 1
+          elif res == HandleLogResult.INPUT_INVALID:
+            cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
+            observation_input_invalid[which] += 1
+          elif res == HandleLogResult.SUCCESS:
+            observation_input_invalid[which] *= input_invalid_decay[which]
+    else:
+      filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
+
+    if sm.updated["cameraOdometry"]:
+      critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
+      inputs_valid = sm.all_valid() and critical_service_inputs_valid
+      sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
+
+      msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
+      pm.send("deviceMotion", msg)
+
+
+if __name__ == "__main__":
+  main()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py

@@ -0,0 +1,88 @@
+class ObservationKind:
+  UNKNOWN = 0
+  NO_OBSERVATION = 1
+  GPS_NED = 2
+  ODOMETRIC_SPEED = 3
+  PHONE_GYRO = 4
+  GPS_VEL = 5
+  PSEUDORANGE_GPS = 6
+  PSEUDORANGE_RATE_GPS = 7
+  SPEED = 8
+  NO_ROT = 9
+  PHONE_ACCEL = 10
+  ORB_POINT = 11
+  ECEF_POS = 12
+  CAMERA_ODO_TRANSLATION = 13
+  CAMERA_ODO_ROTATION = 14
+  ORB_FEATURES = 15
+  MSCKF_TEST = 16
+  FEATURE_TRACK_TEST = 17
+  LANE_PT = 18
+  IMU_FRAME = 19
+  PSEUDORANGE_GLONASS = 20
+  PSEUDORANGE_RATE_GLONASS = 21
+  PSEUDORANGE = 22
+  PSEUDORANGE_RATE = 23
+  ECEF_VEL = 35
+  ECEF_ORIENTATION_FROM_GPS = 32
+  NO_ACCEL = 33
+  ORB_FEATURES_WIDE = 34
+
+  ROAD_FRAME_XY_SPEED = 24  # (x, y) [m/s]
+  ROAD_FRAME_YAW_RATE = 25  # [rad/s]
+  STEER_ANGLE = 26  # [rad]
+  ANGLE_OFFSET_FAST = 27  # [rad]
+  STIFFNESS = 28  # [-]
+  STEER_RATIO = 29  # [-]
+  ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
+  ROAD_ROLL = 31  # [rad]
+
+  names = [
+    'Unknown',
+    'No observation',
+    'GPS NED',
+    'Odometric speed',
+    'Phone gyro',
+    'GPS velocity',
+    'GPS pseudorange',
+    'GPS pseudorange rate',
+    'Speed',
+    'No rotation',
+    'Phone acceleration',
+    'ORB point',
+    'ECEF pos',
+    'camera odometric translation',
+    'camera odometric rotation',
+    'ORB features',
+    'MSCKF test',
+    'Feature track test',
+    'Lane ecef point',
+    'imu frame eulers',
+    'GLONASS pseudorange',
+    'GLONASS pseudorange rate',
+    'pseudorange',
+    'pseudorange rate',
+
+    'Road Frame x,y speed',
+    'Road Frame yaw rate',
+    'Steer Angle',
+    'Fast Angle Offset',
+    'Stiffness',
+    'Steer Ratio',
+    'Road Frame x speed',
+    'Road Roll',
+    'ECEF orientation from GPS',
+    'NO accel',
+    'ORB features wide camera',
+    'ECEF_VEL',
+  ]
+
+  @classmethod
+  def to_string(cls, kind):
+    return cls.names[kind]
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+           ObservationKind.PSEUDORANGE_RATE_GPS,
+           ObservationKind.PSEUDORANGE_GLONASS,
+           ObservationKind.PSEUDORANGE_RATE_GLONASS]

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py

@@ -0,0 +1,67 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+
+from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler
+from iqpilot.selfdrive.locationd.models.constants import ObservationKind
+from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
+try:
+  from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update
+except ModuleNotFoundError:
+  pose_predict = None
+  pose_update = None
+
+
+EARTH_G = 9.81
+
+
+class States:
+  NED_ORIENTATION = slice(0, 3)
+  DEVICE_VELOCITY = slice(3, 6)
+  ANGULAR_VELOCITY = slice(6, 9)
+  GYRO_BIAS = slice(9, 12)
+  ACCELERATION = slice(12, 15)
+  ACCEL_BIAS = slice(15, 18)
+
+
+def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray:
+  result = state.copy()
+  result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION]
+  rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY])
+  result[States.NED_ORIENTATION] = euler_from_rot(rotation)
+  return result
+
+
+def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray:
+  device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T
+  centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY])
+  return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS]
+
+
+class PoseKalman(EstimatorModel):
+  name = "pose"
+  initial_x = np.zeros(18)
+  initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3)
+  Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3)
+  obs_noise = {
+    ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3),
+    ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3),
+    ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3),
+    ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3),
+  }
+
+  def __init__(self, max_rewind_age: float):
+    measurements = {
+      ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS],
+      ObservationKind.PHONE_ACCEL: _phone_acceleration,
+      ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY],
+      ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY],
+    }
+    def native_predict(state, covariance, dt, process_noise, _):
+      pose_predict(state, covariance, process_noise, dt)
+
+    model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise,
+                            native_predict=native_predict if pose_predict is not None else None, native_update=pose_update)
+    super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age))

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py

@@ -0,0 +1,68 @@
+import copy
+import os
+import json
+from collections import defaultdict
+from dataclasses import dataclass
+
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.selfdrived.events import Alert
+from iqpilot.common.atlas_alerts import NULL_ALERT as EmptyAlert
+
+
+with open(os.path.join(BASEDIR, "iqpilot/selfdrive/selfdrived/alerts_offroad.json")) as f:
+  OFFROAD_ALERTS = json.load(f)
+
+
+def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None:
+  if show_alert:
+    a = copy.copy(OFFROAD_ALERTS[alert])
+    a['extra'] = extra_text or ''
+    Params().put(alert, a)
+  else:
+    Params().remove(alert)
+
+
+@dataclass
+class AlertEntry:
+  alert: Alert | None = None
+  start_frame: int = -1
+  end_frame: int = -1
+  added_frame: int = -1
+
+  def active(self, frame: int) -> bool:
+    return frame <= self.end_frame
+
+  def just_added(self, frame: int) -> bool:
+    return self.active(frame) and frame == (self.added_frame + 1)
+
+class AlertManager:
+  def __init__(self):
+    self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
+    self.current_alert = EmptyAlert
+
+  def add_many(self, frame: int, alerts: list[Alert]) -> None:
+    for alert in alerts:
+      entry = self.alerts[alert.alert_type]
+      entry.alert = alert
+      if not entry.just_added(frame):
+        entry.start_frame = frame
+      min_end_frame = entry.start_frame + alert.duration
+      entry.end_frame = max(frame + 1, min_end_frame)
+      entry.added_frame = frame
+
+  def process_alerts(self, frame: int, clear_event_types: set):
+    ae = AlertEntry()
+    for v in self.alerts.values():
+      if not v.alert:
+        continue
+
+      if v.alert.event_type in clear_event_types:
+        v.end_frame = -1
+
+      # sort by priority first and then by start_frame
+      greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
+      if v.active(frame) and greater:
+        ae = v
+
+    self.current_alert = ae.alert if ae.alert is not None else EmptyAlert

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py

@@ -0,0 +1,991 @@
+#!/usr/bin/env python3
+import math
+
+from iqpilot.cereal import log, car
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.constants import CV
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER
+from iqpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER
+from iqpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION
+from iqpilot.system.hardware import HARDWARE
+
+from iqpilot.common.atlas_alerts import EventBook as EventsBase, Tier as Priority, Tags as ET, AlertCard as Alert, \
+  NoEntryCard as NoEntryAlert, GentleDisableCard as SoftDisableAlert, PendingDisableCard as UserSoftDisableAlert, \
+  HardDisableCard as ImmediateDisableAlert, ChimeCard as EngagementAlert, BannerCard as NormalPermanentAlert, \
+  BootCard as StartupAlert, AlertFactory as AlertCallbackType, car_mode_entry_alert as wrong_car_mode_alert
+
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+EventName = log.OnroadEvent.EventName
+
+
+# get event name from enum
+EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()}
+
+
+class Events(EventsBase):
+  def __init__(self):
+    super().__init__()
+    self.event_counters = dict.fromkeys(EVENTS.keys(), 0)
+
+  def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
+    return EVENTS
+
+  def get_event_name(self, event: int):
+    return EVENT_NAME[event]
+
+  def get_event_msg_type(self):
+    return log.OnroadEvent
+
+
+
+# ********** helper functions **********
+def get_display_speed(speed_ms: float, metric: bool) -> str:
+  speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)))
+  unit = 'km/h' if metric else 'mph'
+  return f"{speed} {unit}"
+
+
+# ********** alert callback functions **********
+
+
+def soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
+  def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+    if soft_disable_time < int(0.5 / DT_CTRL):
+      return ImmediateDisableAlert(alert_text_2)
+    return SoftDisableAlert(alert_text_2)
+  return func
+
+def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType:
+  def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+    if soft_disable_time < int(0.5 / DT_CTRL):
+      return ImmediateDisableAlert(alert_text_2)
+    return UserSoftDisableAlert(alert_text_2)
+  return func
+
+def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage")
+
+
+def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  return Alert(
+    f"Steer Assist Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
+    "",
+    AlertStatus.userPrompt, AlertSize.small,
+    Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4)
+
+
+def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  first_word = 'Recalibrating' if sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.recalibrating else 'Calibrating'
+  return Alert(
+    f"{first_word}: {sm['extrinsicsCalibration'].calPerc:.0f}%",
+    f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}",
+    AlertStatus.normal, AlertSize.mid,
+    Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
+
+
+def audio_feedback_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  duration = FEEDBACK_MAX_DURATION - ((sm['audioFeedback'].blockNum + 1) * SAMPLE_BUFFER / SAMPLE_RATE)
+  return NormalPermanentAlert(
+    "Recording Audio Feedback",
+    f"{round(duration)} second{'s' if round(duration) != 1 else ''} remaining. Press again to save early.",
+    priority=Priority.LOW)
+
+
+# *** debug alerts ***
+
+def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  full_perc = round(100. - sm['deviceState'].freeSpacePercent)
+  return NormalPermanentAlert("Out of Storage", f"{full_perc}% full")
+
+
+def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  if sm.frame * DT_CTRL < 10.:
+    return NoEntryAlert("IQModel is starting up", alert_text_1="Please Wait")
+  mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan
+  err = CS.vEgo - mdl
+  msg = f"Speed Error: {err:.1f} m/s"
+  return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid")
+
+
+def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning]
+  msg = ', '.join(not_running)
+  return NoEntryAlert(msg, alert_text_1="Process Not Running")
+
+
+def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])]
+  msg = ', '.join(bs[:4])  # can't fit too many on one line
+  return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes")
+
+
+def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
+  bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])]
+  return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams))
+
+
+def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  rpy = sm['extrinsicsCalibration'].rpyCalib
+  yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan)
+  pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan)
+  angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)"
+  return NormalPermanentAlert("Calibration Invalid", angles)
+
+
+def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  if not sm['vehicleParameters'].angleOffsetValid:
+    angle_offset_deg = sm['vehicleParameters'].angleOffsetDeg
+    title = "Steering misalignment detected"
+    text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)"
+  elif not sm['vehicleParameters'].steerRatioValid:
+    steer_ratio = sm['vehicleParameters'].steerRatio
+    title = "Steer ratio mismatch"
+    text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})"
+  elif not sm['vehicleParameters'].stiffnessFactorValid:
+    stiffness_factor = sm['vehicleParameters'].stiffnessFactor
+    title = "Abnormal tire stiffness"
+    text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})"
+  else:
+    return NoEntryAlert("paramsd Temporary Error")
+
+  return NoEntryAlert(alert_text_1=title, alert_text_2=text)
+
+def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  cpu = max(sm['deviceState'].cpuTempC, default=0.)
+  gpu = max(sm['deviceState'].gpuTempC, default=0.)
+  temp = max((cpu, gpu, sm['deviceState'].memoryTempC))
+  return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C")
+
+
+def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used")
+
+
+def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  x = max(sm['deviceState'].cpuUsagePercent, default=0.)
+  return NormalPermanentAlert("High CPU Usage", f"{x}% used")
+
+
+def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped")
+
+
+def _joystick_axes(sm: messaging.SubMaster) -> tuple[float, float] | None:
+  if 'testJoystick' not in sm.data or sm.recv_frame['testJoystick'] == 0:
+    return None
+
+  axes = list(sm['testJoystick'].axes)
+  if len(axes) < 2:
+    return None
+
+  return float(axes[0]), float(axes[1])
+
+
+def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  gb = sm['carControl'].actuators.accel / 4.
+  if CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+    steer = sm['carControl'].actuators.steeringAngleDeg
+    vals = f"Gas: {round(gb * 100.)}%, Angle: {round(steer, 1)}°"
+  else:
+    steer = sm['carControl'].actuators.torque
+    vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%"
+  return NormalPermanentAlert("Joystick Mode", vals)
+
+
+def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  ad = sm['alertDebug']
+  audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none
+  alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal
+  alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small
+  return Alert(ad.alertText1, ad.alertText2,
+               alert_status, alert_size,
+               Priority.LOW, VisualAlert.none, audible_alert, 0.2)
+
+
+def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  personality = str(personality).title()
+  return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5)
+
+
+def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  title = "Invalid LKAS setting"
+  text = "Toggle stock LKAS on or off to engage"
+  if CP.brand == "tesla":
+    title = "Dashcam Mode"
+    text = "FSD / Autosteer is active"
+  elif CP.brand == "mazda":
+    text = "Enable your car's LKAS to engage"
+  elif CP.brand == "nissan":
+    text = "Disable your car's stock LKAS to engage"
+  return NormalPermanentAlert(title, text)
+
+
+def invalid_lkas_setting_no_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster,
+                                        metric: bool, soft_disable_time: int, personality) -> Alert:
+  if CP.brand == "tesla":
+    return NoEntryAlert("FSD / Autosteer is active", alert_text_1="Dashcam Mode")
+  return NoEntryAlert("Invalid LKAS setting")
+
+
+
+EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
+  # ********** events with no alerts **********
+
+  EventName.stockFcw: {},
+  EventName.actuatorsApiUnavailable: {},
+
+  # ********** events only containing alerts displayed in all states **********
+
+  EventName.joystickDebug: {
+    ET.WARNING: joystick_alert,
+    ET.PERMANENT: NormalPermanentAlert("Joystick Mode"),
+  },
+
+  EventName.longitudinalManeuver: {
+    ET.WARNING: longitudinal_maneuver_alert,
+    ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode",
+                                       "Ensure road ahead is clear"),
+  },
+
+  EventName.bigModelLoading: {
+    ET.NO_ENTRY: NoEntryAlert("Big Model Loading"),
+  },
+
+  EventName.bigModelFailed: {
+    ET.SOFT_DISABLE: soft_disable_alert("Big Model Failed"),
+    ET.PERMANENT: NormalPermanentAlert("Big Model Failed ", "Restart the car to retry,\nsmall model is still available", duration=20.),
+  },
+
+  EventName.lateralManeuver: {
+    ET.WARNING: longitudinal_maneuver_alert,
+    ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode"),
+  },
+
+  EventName.selfdriveInitializing: {
+    ET.NO_ENTRY: NoEntryAlert("IQ.Pilot Initializing"),
+  },
+
+  EventName.startup: {
+    ET.PERMANENT: StartupAlert("Welcome to IQ.Pilot!")
+  },
+
+  EventName.startupMaster: {
+    ET.PERMANENT: StartupAlert("Welcome to IQ.Pilot!"),
+  },
+
+  EventName.startupNoControl: {
+    ET.PERMANENT: StartupAlert("Dashcam mode"),
+    ET.NO_ENTRY: NoEntryAlert("Dashcam mode"),
+  },
+
+  EventName.startupNoCar: {
+    ET.PERMANENT: StartupAlert("IQ.Pilot Dashcam mode: car unrecognized"),
+  },
+
+  EventName.startupNoSecOcKey: {
+    ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
+                                       "TSK Security Key Not Available",
+                                       priority=Priority.HIGH),
+  },
+
+  EventName.dashcamMode: {
+    ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
+                                       priority=Priority.LOWEST),
+  },
+
+  EventName.invalidLkasSetting: {
+    ET.PERMANENT: invalid_lkas_setting_alert,
+    ET.NO_ENTRY: invalid_lkas_setting_no_entry_alert,
+  },
+
+  EventName.cruiseMismatch: {
+    #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"),
+  },
+
+  # openpilot doesn't recognize the car. This switches openpilot into a
+  # read-only mode. This can be solved by adding your fingerprint.
+  # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information
+  EventName.carUnrecognized: {
+    ET.PERMANENT: NormalPermanentAlert("Dashcam Mode",
+                                       "Car Unrecognized",
+                                       priority=Priority.LOWEST),
+  },
+
+  EventName.aeb: {
+    ET.PERMANENT: Alert(
+      "BRAKE!",
+      "Emergency Braking: Risk of Collision",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.),
+    ET.NO_ENTRY: NoEntryAlert("AEB: Risk of Collision"),
+  },
+
+  EventName.stockAeb: {
+    ET.PERMANENT: Alert(
+      "BRAKE!",
+      "Stock AEB: Risk of Collision",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.),
+    ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"),
+  },
+
+  EventName.stockLkas: {
+    ET.PERMANENT: Alert(
+      "Stock LKAS: Lane Departure Detected",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.),
+    ET.NO_ENTRY: NoEntryAlert("Stock LKAS: Lane Departure Detected"),
+  },
+
+  EventName.fcw: {
+    ET.PERMANENT: Alert(
+      "BRAKE!",
+      "Risk of Collision",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.),
+  },
+
+  EventName.ldw: {
+    ET.PERMANENT: Alert(
+      "Lane Departure Detected",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.),
+  },
+
+  # ********** events only containing alerts that display while engaged **********
+
+  EventName.steerTempUnavailableSilent: {
+    ET.WARNING: Alert(
+      "Steering Assist Temporarily Unavailable",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8),
+  },
+
+  EventName.preDriverDistracted: {
+    ET.PERMANENT: Alert(
+      "Pay Attention",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.promptDriverDistracted: {
+    ET.PERMANENT: Alert(
+      "Pay Attention",
+      "Driver Distracted",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
+  },
+
+  EventName.driverDistracted: {
+    ET.PERMANENT: Alert(
+      "DISENGAGE IMMEDIATELY",
+      "Driver Distracted",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
+  },
+
+  EventName.preDriverUnresponsive: {
+    ET.PERMANENT: Alert(
+      "Touch Steering Wheel: No Face Detected",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1),
+  },
+
+  EventName.promptDriverUnresponsive: {
+    ET.PERMANENT: Alert(
+      "Touch Steering Wheel",
+      "Driver Unresponsive",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
+  },
+
+  EventName.driverUnresponsive: {
+    ET.PERMANENT: Alert(
+      "DISENGAGE IMMEDIATELY",
+      "Driver Unresponsive",
+      AlertStatus.critical, AlertSize.full,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
+  },
+
+  EventName.manualRestart: {
+    ET.WARNING: Alert(
+      "TAKE CONTROL",
+      "Resume Driving Manually",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+  },
+
+  EventName.resumeRequired: {
+    ET.WARNING: Alert(
+      "Press Resume to Exit Standstill",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+  },
+
+  EventName.belowSteerSpeed: {
+    ET.WARNING: below_steer_speed_alert,
+  },
+
+  EventName.preLaneChangeLeft: {
+    ET.WARNING: Alert(
+      "Steer Left to Start Lane Change Once Safe",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.preLaneChangeRight: {
+    ET.WARNING: Alert(
+      "Steer Right to Start Lane Change Once Safe",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.laneChangeBlocked: {
+    ET.WARNING: Alert(
+      "Car Detected in Blindspot",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
+  },
+
+  EventName.laneChange: {
+    ET.WARNING: Alert(
+      "Changing Lanes",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.steerSaturated: {
+    ET.WARNING: Alert(
+      "Take Control",
+      "Turn Exceeds Steering Limit",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
+  },
+
+  # Thrown when the fan is driven at >50% but is not rotating
+  EventName.fanMalfunction: {
+    ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"),
+  },
+
+  # Camera is not outputting frames
+  EventName.cameraMalfunction: {
+    ET.PERMANENT: camera_malfunction_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"),
+    ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"),
+  },
+  # Camera framerate too low
+  EventName.cameraFrameRate: {
+    ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"),
+    ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"),
+    ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"),
+  },
+
+  # Unused
+
+  EventName.locationdTemporaryError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"),
+    ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"),
+  },
+
+  EventName.locationdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"),
+  },
+
+  # openpilot tries to learn certain parameters about your car by observing
+  # how the car behaves to steering inputs from both human and openpilot driving.
+  # This includes:
+  # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle
+  # - tire stiffness: how much grip your tires have
+  # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight
+  # This alert is thrown when any of these values exceed a sanity check. This can be caused by
+  # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub
+  EventName.paramsdTemporaryError: {
+    ET.NO_ENTRY: paramsd_invalid_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"),
+  },
+
+  EventName.paramsdPermanentError: {
+    ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"),
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"),
+    ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"),
+  },
+
+  # ********** events that affect controls state transitions **********
+
+  EventName.pcmEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
+  },
+
+  EventName.buttonEnable: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
+  },
+
+  EventName.pcmDisable: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+  },
+
+  EventName.buttonCancel: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"),
+  },
+
+  EventName.brakeHold: {
+    ET.WARNING: Alert(
+      "Press Resume to Exit Brake Hold",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+  },
+
+  EventName.parkBrake: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),
+  },
+
+  EventName.pedalPressed: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: NoEntryAlert("Pedal Pressed",
+                              visual_alert=VisualAlert.brakePressed),
+  },
+
+  EventName.steerDisengage: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: NoEntryAlert("Steering Pressed"),
+  },
+
+  EventName.preEnableStandstill: {
+    ET.PRE_ENABLE: Alert(
+      "Release Brake to Engage",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.),
+  },
+
+  EventName.gasPressedOverride: {
+    ET.OVERRIDE_LONGITUDINAL: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.steerOverride: {
+    ET.OVERRIDE_LATERAL: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1),
+  },
+
+  EventName.wrongCarMode: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: wrong_car_mode_alert,
+  },
+
+  EventName.resumeBlocked: {
+    ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"),
+  },
+
+  EventName.wrongCruiseMode: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+    ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"),
+  },
+
+  EventName.steerTempUnavailable: {
+    ET.SOFT_DISABLE: soft_disable_alert("Steering Assist Temporarily Unavailable"),
+    ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"),
+  },
+
+  EventName.steerTimeLimit: {
+    ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"),
+    ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"),
+  },
+
+  EventName.outOfSpace: {
+    ET.PERMANENT: out_of_space_alert,
+    ET.NO_ENTRY: NoEntryAlert("Out of Storage"),
+  },
+
+  EventName.belowEngageSpeed: {
+    ET.NO_ENTRY: below_engage_speed_alert,
+  },
+
+  EventName.sensorDataInvalid: {
+    ET.PERMANENT: Alert(
+      "Sensor Data Invalid",
+      "Possible Hardware Issue",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.),
+    ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"),
+    ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
+  },
+
+  EventName.noGps: {
+  },
+
+  EventName.tooDistracted: {
+    ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"),
+  },
+
+  EventName.excessiveActuation: {
+    ET.SOFT_DISABLE: soft_disable_alert("Excessive Actuation"),
+    ET.NO_ENTRY: NoEntryAlert("Excessive Actuation"),
+  },
+
+  EventName.overheat: {
+    ET.PERMANENT: overheat_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),
+    ET.NO_ENTRY: NoEntryAlert("System Overheated"),
+  },
+
+  EventName.wrongGear: {
+    ET.SOFT_DISABLE: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+  },
+
+  # This alert is thrown when the calibration angles are outside of the acceptable range.
+  # For example if the device is pointed too much to the left or the right.
+  # Usually this can only be solved by removing the mount from the windshield completely,
+  # and attaching while making sure the device is pointed straight forward and is level.
+  # See https://comma.ai/setup for more information
+  EventName.calibrationInvalid: {
+    ET.PERMANENT: calibration_invalid_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"),
+    ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"),
+  },
+
+  EventName.calibrationIncomplete: {
+    ET.PERMANENT: calibration_incomplete_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
+    ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"),
+  },
+
+  EventName.calibrationRecalibrating: {
+    ET.PERMANENT: calibration_incomplete_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"),
+    ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"),
+  },
+
+  EventName.doorOpen: {
+    ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"),
+    ET.NO_ENTRY: NoEntryAlert("Door Open"),
+  },
+
+  EventName.seatbeltNotLatched: {
+    ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"),
+    ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"),
+  },
+
+  EventName.espDisabled: {
+    ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"),
+    ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"),
+  },
+
+  EventName.lowBattery: {
+    ET.SOFT_DISABLE: soft_disable_alert("Low Battery"),
+    ET.NO_ENTRY: NoEntryAlert("Low Battery"),
+  },
+
+  # Different openpilot services communicate between each other at a certain
+  # interval. If communication does not follow the regular schedule this alert
+  # is thrown. This can mean a service crashed, did not broadcast a message for
+  # ten times the regular interval, or the average interval is more than 10% too high.
+  # Soft warnings — no disable, no entry block. UI shows a silent yellow triangle instead.
+  EventName.commIssue: {
+  },
+  EventName.commIssueAvgFreq: {
+  },
+
+  EventName.selfdrivedLagging: {
+  },
+
+  # Thrown when manager detects a service exited unexpectedly while driving
+  EventName.processNotRunning: {
+    ET.NO_ENTRY: process_not_running_alert,
+    ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"),
+  },
+
+  EventName.radarFault: {
+    ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"),
+    ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"),
+  },
+
+  EventName.radarTempUnavailable: {
+    ET.SOFT_DISABLE: soft_disable_alert("Radar Temporarily Unavailable"),
+    ET.NO_ENTRY: NoEntryAlert("Radar Temporarily Unavailable"),
+  },
+
+  # Every frame from the camera should be processed by the model. If modeld
+  # is not processing frames fast enough they have to be dropped. This alert is
+  # thrown when over 20% of frames are dropped.
+  EventName.modeldLagging: {
+    ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"),
+    ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"),
+    ET.PERMANENT: modeld_lagging_alert,
+  },
+
+  # Besides predicting the path, lane lines and lead car data the model also
+  # predicts the current velocity and rotation speed of the car. If the model is
+  # very uncertain about the current velocity while the car is moving, this
+  # usually means the model has trouble understanding the scene. This is used
+  # as a heuristic to warn the driver.
+  EventName.posenetInvalid: {
+    ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"),
+    ET.NO_ENTRY: posenet_invalid_alert,
+  },
+
+  # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we
+  # alert the driver the device might have fallen from the windshield.
+  EventName.deviceFalling: {
+    ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"),
+    ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
+  },
+
+  EventName.lowMemory: {
+    ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"),
+    ET.PERMANENT: low_memory_alert,
+    ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
+  },
+
+  EventName.accFaulted: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"),
+    ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"),
+    ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"),
+  },
+
+  EventName.cruiseFaultLateralAllowed: {
+    ET.PERMANENT: NormalPermanentAlert("Cruise Faulted", "Lane Assist will continue to work", priority=Priority.LOWEST),
+  },
+
+  EventName.espActive: {
+    ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Active"),
+    ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Active"),
+  },
+
+  EventName.controlsMismatch: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
+    ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),
+  },
+
+  # Sometimes the USB stack on the device can get into a bad state
+  # causing the connection to the panda to be lost
+  EventName.usbError: {
+    ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"),
+    ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device"),
+    ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"),
+  },
+
+  # This alert can be thrown for the following reasons:
+  # - No CAN data received at all
+  # - CAN data is received, but some message are not received at the right frequency
+  # If you're not writing a new car port, this is usually cause by faulty wiring
+  # Minor canError: low-priority silent HUD badge only — no disable, no entry block, no audible/visual alert
+  EventName.canError: {
+    ET.PERMANENT: Alert(
+      "CAN Error",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.),
+  },
+
+  EventName.canBusMissing: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"),
+    ET.PERMANENT: Alert(
+      "CAN Bus Disconnected: Likely Faulty Cable",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.),
+    ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"),
+  },
+
+  EventName.steerUnavailable: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"),
+    ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"),
+    ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"),
+  },
+
+  EventName.reverseGear: {
+    ET.PERMANENT: Alert(
+      "Reverse\nGear",
+      "",
+      AlertStatus.normal, AlertSize.full,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
+    ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"),
+    ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
+  },
+
+  # On cars that use stock ACC the car can decide to cancel ACC for various reasons.
+  # When this happens we can no long control the car so the user needs to be warned immediately.
+  EventName.cruiseDisabled: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"),
+  },
+
+  # When the relay in the harness box opens the CAN bus between the LKAS camera
+  # and the rest of the car is separated. When messages from the LKAS camera
+  # are received on the car side this usually means the relay hasn't opened correctly
+  # and this alert is thrown.
+  EventName.relayMalfunction: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"),
+    ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"),
+    ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"),
+  },
+
+  EventName.speedTooLow: {
+    ET.IMMEDIATE_DISABLE: Alert(
+      "IQ.Pilot Canceled",
+      "Speed too low",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
+  },
+
+  # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable.
+  EventName.speedTooHigh: {
+    ET.WARNING: Alert(
+      "Speed Too High",
+      "Model uncertain at this speed",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.),
+    ET.NO_ENTRY: NoEntryAlert("Slow down to engage"),
+  },
+
+  EventName.vehicleSensorsInvalid: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"),
+    ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"),
+    ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"),
+  },
+
+  EventName.personalityChanged: {
+    ET.WARNING: personality_changed_alert,
+  },
+
+  EventName.userBookmark: {
+    ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5),
+  },
+
+  EventName.audioFeedback: {
+    ET.PERMANENT: audio_feedback_alert,
+  },
+}
+
+
+if HARDWARE.get_device_type() == 'mici':
+  EVENTS.update({
+    EventName.preDriverDistracted: {
+      ET.PERMANENT: Alert(
+        "Pay Attention",
+        "",
+        AlertStatus.normal, AlertSize.small,
+        Priority.MID, VisualAlert.none, AudibleAlert.none, 2),
+    },
+    EventName.promptDriverDistracted: {
+      ET.PERMANENT: Alert(
+        "Pay Attention",
+        "Driver Distracted",
+        AlertStatus.userPrompt, AlertSize.mid,
+        Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, 1),
+    },
+    EventName.resumeRequired: {
+      ET.WARNING: Alert(
+        "Press Resume",
+        "",
+        AlertStatus.userPrompt, AlertSize.small,
+        Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
+    },
+    EventName.preLaneChangeLeft: {
+      ET.WARNING: Alert(
+        "Steer Left",
+        "Confirm Lane Change",
+        AlertStatus.normal, AlertSize.mid,
+        Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
+    },
+    EventName.preLaneChangeRight: {
+      ET.WARNING: Alert(
+        "Steer Right",
+        "Confirm Lane Change",
+        AlertStatus.normal, AlertSize.mid,
+        Priority.LOW, VisualAlert.none, AudibleAlert.none, .1),
+    },
+    EventName.laneChangeBlocked: {
+      ET.WARNING: Alert(
+        "Car in Blindspot",
+        "",
+        AlertStatus.userPrompt, AlertSize.small,
+        Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
+    },
+    EventName.steerSaturated: {
+      ET.WARNING: Alert(
+        "take control",
+        "turn exceeds limit",
+        AlertStatus.userPrompt, AlertSize.mid,
+        Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.),
+    },
+    EventName.calibrationIncomplete: {
+      ET.PERMANENT: calibration_incomplete_alert,
+      ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"),
+      ET.NO_ENTRY: NoEntryAlert("Calibrating"),
+    },
+    EventName.reverseGear: {
+      ET.PERMANENT: Alert(
+        "Reverse",
+        "",
+        AlertStatus.normal, AlertSize.full,
+        Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
+      ET.USER_DISABLE: ImmediateDisableAlert("Reverse"),
+      ET.NO_ENTRY: NoEntryAlert("Reverse"),
+    },
+  })
+
+
+if __name__ == '__main__':
+  # print all alerts by type and priority
+  from iqpilot.cereal.services import SERVICE_LIST
+  from collections import defaultdict
+
+  event_names = {v: k for k, v in EventName.schema.enumerants.items()}
+  alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list))
+
+  CP = car.CarParams.new_message()
+  CS = car.CarState.new_message()
+  sm = messaging.SubMaster(list(SERVICE_LIST.keys()))
+
+  for i, alerts in EVENTS.items():
+    for et, alert in alerts.items():
+      if callable(alert):
+        alert = alert(CP, CS, sm, False, 1, log.LongitudinalPersonality.standard)
+      alerts_by_type[et][alert.priority].append(event_names[i])
+
+  all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {}
+  for et, priority_alerts in alerts_by_type.items():
+    all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True)
+
+  for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]):
+    print(f"**** {status} ****")
+    for p, alert_list in evs:
+      print(f"  {repr(p)}:")
+      print("   ", ', '.join(alert_list), "\n")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py

@@ -0,0 +1,7 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from iqpilot.selfdrive.state_estimation.estimator import EstimatorModel, ModelDefinition, Observation, StateEstimator
+
+__all__ = ["EstimatorModel", "ModelDefinition", "Observation", "StateEstimator"]

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py

@@ -0,0 +1,311 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from collections.abc import Callable
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+
+Array = np.ndarray
+Prediction = Callable[[Array, float, dict[str, float]], Array]
+Measurement = Callable[[Array, dict[str, float]], Array]
+Injection = Callable[[Array, Array], Array]
+NativePrediction = Callable[[Array, Array, float, Array, dict[str, float]], None]
+NativeUpdate = Callable[[Array, Array, int, Array, Array], Array]
+
+
+@dataclass(frozen=True)
+class Observation:
+  kind: int
+  values: Array
+  noise: Array
+
+
+@dataclass(frozen=True)
+class ModelDefinition:
+  state_size: int
+  error_size: int
+  transition: Prediction
+  measurements: dict[int, Measurement]
+  process_noise: Array
+  observation_noise: dict[int, Array]
+  inject_error: Injection | None = None
+  error_projection: Callable[[Array], Array] | None = None
+  normalize: Callable[[Array], Array] | None = None
+  native_predict: NativePrediction | None = None
+  native_update: NativeUpdate | None = None
+
+
+@dataclass
+class _Snapshot:
+  time: float
+  state: Array
+  covariance: Array
+
+
+@dataclass
+class _Event:
+  time: float
+  observation: Observation
+  order: int
+
+
+class StateEstimator:
+  def __init__(self, model: ModelDefinition, initial_state: Array, initial_covariance: Array,
+               max_rewind_age: float = 0.0):
+    self.model = model
+    self.parameters: dict[str, float] = {}
+    self.max_rewind_age = max_rewind_age
+    self._order = 0
+    self.init_state(initial_state, initial_covariance, None)
+
+  @property
+  def x(self) -> Array:
+    return self._state.copy()
+
+  @property
+  def P(self) -> Array:
+    return self._covariance.copy()
+
+  @property
+  def t(self) -> float:
+    return self._time
+
+  def set_global(self, name: str, value: float) -> None:
+    self.parameters[name] = float(value)
+
+  def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None:
+    state = np.asarray(state, dtype=np.float64).reshape(-1)
+    covariance = np.asarray(covs, dtype=np.float64)
+    self._validate_state(state, covariance)
+    self._state = self._normalize(state.copy())
+    self._covariance = self._stabilize(covariance.copy())
+    self._time = math.nan if filter_time is None else float(filter_time)
+    self._events: list[_Event] = []
+    self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())]
+
+  def set_filter_time(self, filter_time: float | None) -> None:
+    self._time = math.nan if filter_time is None else float(filter_time)
+
+  def reset_rewind(self) -> None:
+    self._events.clear()
+    self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())]
+
+  def predict(self, time: float) -> None:
+    time = float(time)
+    if math.isnan(self._time):
+      self._time = time
+      return
+    if time < self._time:
+      raise ValueError("prediction time precedes estimator time")
+    dt = time - self._time
+    if dt == 0.0:
+      return
+    if self.model.native_predict is not None:
+      self.model.native_predict(self._state, self._covariance, dt, self.model.process_noise, self.parameters)
+      self._time = time
+      return
+    previous = self._state.copy()
+    transition_jacobian = self._jacobian(lambda value: self.model.transition(value, dt, self.parameters), previous)
+    predicted = self.model.transition(previous, dt, self.parameters)
+    projection = self._error_projection(previous)
+    if self.model.error_size == self.model.state_size:
+      error_transition = transition_jacobian
+    else:
+      error_transition = np.linalg.pinv(self._error_projection(predicted)) @ transition_jacobian @ projection
+    self._state = self._normalize(predicted)
+    self._covariance = self._stabilize(error_transition @ self._covariance @ error_transition.T + dt * self.model.process_noise)
+    self._time = time
+
+  def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None):
+    values = self._measurement_batch(kind, measurements)
+    noises = self._noise_batch(kind, len(values), noise)
+    event = _Event(float(time), Observation(kind, values, noises), self._order)
+    self._order += 1
+    if not math.isnan(self._time) and event.time < self._time:
+      if self.max_rewind_age <= 0.0 or self._time - event.time > self.max_rewind_age:
+        return None
+      return self._rewind(event)
+    result = self._apply_event(event)
+    self._events.append(event)
+    self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy()))
+    self._trim_history()
+    return result
+
+  def _apply_event(self, event: _Event):
+    self.predict(event.time)
+    prior_state = self._state.copy()
+    prior_covariance = self._covariance.copy()
+    innovations = []
+    for measurement, noise in zip(event.observation.values, event.observation.noise, strict=True):
+      innovations.append(self._update(event.observation.kind, measurement, noise))
+    return (event.time, self.x, prior_state, self.P, prior_covariance, event.observation.kind,
+            tuple(innovations), event.observation.values.copy(), event.observation.noise.copy())
+
+  def _update(self, kind: int, measurement: Array, noise: Array) -> Array:
+    measurement_function = self.model.measurements.get(kind)
+    if measurement_function is None:
+      raise KeyError(f"unknown observation kind {kind}")
+    measurement = np.asarray(measurement, dtype=np.float64).reshape(-1)
+    if noise.shape != (measurement.size, measurement.size):
+      raise ValueError("observation noise dimension mismatch")
+    if self.model.native_update is not None:
+      innovation = self.model.native_update(self._state, self._covariance, kind, measurement, noise)
+      return innovation
+    predicted = np.asarray(measurement_function(self._state, self.parameters), dtype=np.float64).reshape(-1)
+    if predicted.shape != measurement.shape:
+      raise ValueError("measurement dimension mismatch")
+    innovation = measurement - predicted
+    state_jacobian = self._jacobian(lambda value: measurement_function(value, self.parameters), self._state)
+    observation_jacobian = state_jacobian @ self._error_projection(self._state)
+    innovation_covariance = observation_jacobian @ self._covariance @ observation_jacobian.T + noise
+    gain = np.linalg.solve(innovation_covariance, observation_jacobian @ self._covariance).T
+    delta = gain @ innovation
+    self._state = self._normalize(self._inject(self._state, delta))
+    identity = np.eye(self.model.error_size)
+    residual = identity - gain @ observation_jacobian
+    self._covariance = self._stabilize(residual @ self._covariance @ residual.T + gain @ noise @ gain.T)
+    self._require_finite()
+    return innovation
+
+  def _rewind(self, new_event: _Event):
+    events = sorted(self._events + [new_event], key=lambda event: (event.time, event.order))
+    base_index = max(i for i, snapshot in enumerate(self._snapshots) if math.isnan(snapshot.time) or snapshot.time <= new_event.time)
+    base = self._snapshots[base_index]
+    retained = self._events[:base_index]
+    retained_orders = {event.order for event in retained}
+    replay = [event for event in events if event.order not in retained_orders]
+    self._state = base.state.copy()
+    self._covariance = base.covariance.copy()
+    self._time = base.time
+    self._events = retained.copy()
+    self._snapshots = self._snapshots[:base_index + 1]
+    result = None
+    for event in replay:
+      current = self._apply_event(event)
+      self._events.append(event)
+      self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy()))
+      if event is new_event:
+        result = current
+    self._trim_history()
+    return result
+
+  def _trim_history(self) -> None:
+    if self.max_rewind_age <= 0.0 or math.isnan(self._time):
+      return
+    cutoff = self._time - self.max_rewind_age
+    remove = 0
+    while remove < len(self._events) and self._events[remove].time < cutoff:
+      remove += 1
+    if remove:
+      self._events = self._events[remove:]
+      self._snapshots = self._snapshots[remove:]
+
+  def _measurement_batch(self, kind: int, measurements: Array) -> Array:
+    measurement_function = self.model.measurements.get(kind)
+    if measurement_function is None:
+      raise KeyError(f"unknown observation kind {kind}")
+    if self.model.native_update is not None and kind in self.model.observation_noise:
+      expected = self.model.observation_noise[kind].shape[0]
+    else:
+      expected = np.asarray(measurement_function(self._state, self.parameters)).size
+    values = np.asarray(measurements, dtype=np.float64)
+    if values.ndim == 1:
+      values = values.reshape(1, -1)
+    elif values.ndim != 2:
+      raise ValueError("measurements must be one or two dimensional")
+    if values.shape[1] != expected:
+      raise ValueError("measurement dimension mismatch")
+    return values
+
+  def _noise_batch(self, kind: int, count: int, noise: Array | None) -> Array:
+    if noise is None:
+      base = self.model.observation_noise.get(kind)
+      if base is None:
+        raise KeyError(f"missing observation noise for kind {kind}")
+      return np.repeat(np.asarray(base, dtype=np.float64)[None, :, :], count, axis=0)
+    noises = np.asarray(noise, dtype=np.float64)
+    if noises.ndim == 2:
+      noises = noises[None, :, :]
+    if noises.shape[0] == 1 and count > 1:
+      noises = np.repeat(noises, count, axis=0)
+    if noises.shape[0] != count:
+      raise ValueError("observation noise batch mismatch")
+    return noises
+
+  def _jacobian(self, function: Callable[[Array], Array], value: Array) -> Array:
+    output = np.asarray(function(value), dtype=np.float64).reshape(-1)
+    result = np.empty((output.size, value.size), dtype=np.float64)
+    for index in range(value.size):
+      step = np.cbrt(np.finfo(np.float64).eps) * max(1.0, abs(value[index]))
+      upper = value.copy()
+      lower = value.copy()
+      upper[index] += step
+      lower[index] -= step
+      result[:, index] = (np.asarray(function(upper)).reshape(-1) - np.asarray(function(lower)).reshape(-1)) / (2.0 * step)
+    return result
+
+  def _inject(self, state: Array, delta: Array) -> Array:
+    if self.model.inject_error is None:
+      return state + delta
+    return self.model.inject_error(state, delta)
+
+  def _error_projection(self, state: Array) -> Array:
+    if self.model.error_projection is None:
+      return np.eye(self.model.state_size, self.model.error_size)
+    return np.asarray(self.model.error_projection(state), dtype=np.float64)
+
+  def _normalize(self, state: Array) -> Array:
+    if self.model.normalize is None:
+      return np.asarray(state, dtype=np.float64).reshape(-1)
+    return np.asarray(self.model.normalize(state), dtype=np.float64).reshape(-1)
+
+  def _stabilize(self, covariance: Array) -> Array:
+    covariance = (covariance + covariance.T) * 0.5
+    eigenvalues, eigenvectors = np.linalg.eigh(covariance)
+    if eigenvalues[0] < -1e-10:
+      raise FloatingPointError("covariance is not positive semidefinite")
+    return (eigenvectors * np.maximum(eigenvalues, 0.0)) @ eigenvectors.T
+
+  def _validate_state(self, state: Array, covariance: Array) -> None:
+    if state.shape != (self.model.state_size,):
+      raise ValueError("state dimension mismatch")
+    if covariance.shape != (self.model.error_size, self.model.error_size):
+      raise ValueError("covariance dimension mismatch")
+    if self.model.process_noise.shape != covariance.shape:
+      raise ValueError("process noise dimension mismatch")
+    if not np.isfinite(state).all() or not np.isfinite(covariance).all():
+      raise ValueError("state and covariance must be finite")
+
+  def _require_finite(self) -> None:
+    if not np.isfinite(self._state).all() or not np.isfinite(self._covariance).all():
+      raise FloatingPointError("estimator produced non-finite values")
+
+
+class EstimatorModel:
+  def __init__(self, estimator: StateEstimator):
+    self.filter = estimator
+
+  @property
+  def x(self) -> Array:
+    return self.filter.x
+
+  @property
+  def P(self) -> Array:
+    return self.filter.P
+
+  @property
+  def t(self) -> float:
+    return self.filter.t
+
+  def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None:
+    self.filter.init_state(state, covs, filter_time)
+
+  def predict(self, time: float) -> None:
+    self.filter.predict(time)
+
+  def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None):
+    return self.filter.predict_and_observe(time, kind, measurements, noise)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.cereal import car
+from iqpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER
+
+FEEDBACK_MAX_DURATION = 10.0
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+def main():
+  params = Params()
+  pm = messaging.PubMaster(['userBookmark', 'audioFeedback'])
+  sm = messaging.SubMaster(['rawAudioData', 'bookmarkButton'])
+  should_record_audio = False
+  block_num = 0
+  waiting_for_release = False
+  early_stop_triggered = False
+
+  while True:
+    sm.update()
+    should_send_bookmark = False
+
+    if False and sm.updated['carState'] and sm['carState'].canValid and not sm['iqState'].aol.available:
+      for be in sm['carState'].buttonEvents:
+        if be.type == ButtonType.lkas:
+          if be.pressed:
+            if not should_record_audio:
+              if params.get_bool("RecordAudioFeedback"):
+                should_record_audio = True
+                block_num = 0
+                waiting_for_release = False
+                early_stop_triggered = False
+                cloudlog.info("LKAS button pressed - starting 10-second audio feedback")
+              else:
+                should_send_bookmark = True
+                cloudlog.info("LKAS button pressed - bookmarking")
+            elif should_record_audio and not waiting_for_release:
+              waiting_for_release = True
+          elif waiting_for_release:
+            waiting_for_release = False
+            early_stop_triggered = True
+            cloudlog.info("LKAS button released - ending recording early")
+
+    if should_record_audio and sm.updated['rawAudioData']:
+      raw_audio = sm['rawAudioData']
+      msg = messaging.new_message('audioFeedback', valid=True)
+      msg.audioFeedback.audio.data = raw_audio.data
+      msg.audioFeedback.audio.sampleRate = raw_audio.sampleRate
+      msg.audioFeedback.blockNum = block_num
+      block_num += 1
+      if (block_num * SAMPLE_BUFFER / SAMPLE_RATE) >= FEEDBACK_MAX_DURATION or early_stop_triggered:
+        should_send_bookmark = True
+        should_record_audio = False
+        early_stop_triggered = False
+        cloudlog.info("10-second recording completed or second button press - stopping audio feedback")
+      pm.send('audioFeedback', msg)
+
+    if sm.updated['bookmarkButton']:
+      cloudlog.info("Bookmark button pressed!")
+      should_send_bookmark = True
+
+    if should_send_bookmark:
+      msg = messaging.new_message('userBookmark', valid=True)
+      pm.send('userBookmark', msg)
+
+
+if __name__ == '__main__':
+  main()

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py

@@ -0,0 +1,21 @@
+# Python version of system/camerad/cameras/nv12_info.h
+# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE)
+
+def align(val: int, alignment: int) -> int:
+  return ((val + alignment - 1) // alignment) * alignment
+
+def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]:
+  """Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions."""
+  stride = align(width, 128)
+  y_height = align(height, 32)
+  uv_height = align(height // 2, 16)
+
+  # VENUS_BUFFER_SIZE for NV12
+  y_plane = stride * y_height
+  uv_plane = stride * uv_height + 4096
+  size = y_plane + uv_plane + max(16 * 1024, 8 * stride)
+  size = align(size, 4096)
+  size += align(width, 512) * 512  # kernel padding for non-aligned frames
+  size = align(size, 4096)
+
+  return stride, y_height, uv_height, size

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/__init__.py

@@ -0,0 +1,22 @@
+import os
+from typing import cast
+
+from iqpilot.system.hardware.base import HardwareBase
+from iqpilot.system.hardware.tici.hardware import Tici
+from iqpilot.system.hardware.pc.hardware import Pc
+
+TICI = os.path.isfile('/TICI')
+AGNOS = os.path.isfile('/AGNOS')
+PC = not TICI
+
+
+if TICI:
+  HARDWARE = cast(HardwareBase, Tici())
+else:
+  HARDWARE = cast(HardwareBase, Pc())
+
+# Only comma 3/3X expose the DMA-BUF EGL extensions used by the zero-copy
+# camera renderer and the direct EGL frame-pacing calls. /TICI is also present
+# on comma 4, so it identifies the AGNOS hardware family rather than this GPU
+# capability.
+EGL_DMA_BUF_SUPPORTED = TICI and HARDWARE.get_device_type() in ("tici", "tizi")

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/base.py

@@ -0,0 +1,228 @@
+import os
+from abc import abstractmethod, ABC
+from dataclasses import dataclass, fields
+
+from iqpilot.cereal import log
+
+NetworkType = log.DeviceState.NetworkType
+NetworkStrength = log.DeviceState.NetworkStrength
+
+class LPAError(RuntimeError):
+  pass
+
+class LPAProfileNotFoundError(LPAError):
+  pass
+
+@dataclass
+class Profile:
+  iccid: str
+  nickname: str
+  enabled: bool
+  provider: str
+
+@dataclass
+class ThermalZone:
+  # a zone from /sys/class/thermal/thermal_zone*
+  name: str             # a.k.a type
+  scale: float = 1000.  # scale to get degrees in C
+  zone_number = -1
+
+  def read(self) -> float:
+    if self.zone_number < 0:
+      for n in os.listdir("/sys/devices/virtual/thermal"):
+        if not n.startswith("thermal_zone"):
+          continue
+        with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f:
+          if f.read().strip() == self.name:
+            self.zone_number = int(n.removeprefix("thermal_zone"))
+            break
+
+    try:
+      with open(f"/sys/devices/virtual/thermal/thermal_zone{self.zone_number}/temp") as f:
+        return int(f.read()) / self.scale
+    except FileNotFoundError:
+      return 0
+
+@dataclass
+class ThermalConfig:
+  cpu: list[ThermalZone] | None = None
+  gpu: list[ThermalZone] | None = None
+  dsp: ThermalZone | None = None
+  pmic: list[ThermalZone] | None = None
+  memory: ThermalZone | None = None
+  intake: ThermalZone | None = None
+  exhaust: ThermalZone | None = None
+  case: ThermalZone | None = None
+
+  def get_msg(self):
+    ret = {}
+    for f in fields(ThermalConfig):
+      v = getattr(self, f.name)
+      if v is not None:
+        if isinstance(v, list):
+          ret[f.name + "TempC"] = [x.read() for x in v]
+        else:
+          ret[f.name + "TempC"] = v.read()
+    return ret
+
+class LPABase(ABC):
+  @abstractmethod
+  def list_profiles(self) -> list[Profile]:
+    pass
+
+  @abstractmethod
+  def get_active_profile(self) -> Profile | None:
+    pass
+
+  @abstractmethod
+  def delete_profile(self, iccid: str) -> None:
+    pass
+
+  @abstractmethod
+  def bootstrap(self) -> None:
+    pass
+
+  @abstractmethod
+  def download_profile(self, qr: str, nickname: str | None = None) -> None:
+    pass
+
+  @abstractmethod
+  def nickname_profile(self, iccid: str, nickname: str) -> None:
+    pass
+
+  @abstractmethod
+  def switch_profile(self, iccid: str) -> None:
+    pass
+
+  def is_comma_profile(self, iccid: str) -> bool:
+    return any(iccid.startswith(prefix) for prefix in ('8985235',))
+
+class HardwareBase(ABC):
+  @staticmethod
+  def get_cmdline() -> dict[str, str]:
+    with open('/proc/cmdline') as f:
+      cmdline = f.read()
+    return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2}
+
+  @staticmethod
+  def read_param_file(path, parser, default=0):
+    try:
+      with open(path) as f:
+        return parser(f.read())
+    except Exception:
+      return default
+
+  def booted(self) -> bool:
+    return True
+
+  def reboot(self, reason=None):
+    print("REBOOT!")
+
+  def uninstall(self):
+    print("uninstall")
+
+  def get_os_version(self):
+    return None
+
+  @abstractmethod
+  def get_device_type(self):
+    pass
+
+  def get_imei(self, slot) -> str:
+    return ""
+
+  def get_serial(self):
+    return ""
+
+  def get_network_info(self):
+    return None
+
+  def get_network_type(self):
+    return NetworkType.none
+
+  def get_sim_info(self):
+    return {
+      'sim_id': '',
+      'mcc_mnc': None,
+      'network_type': ["Unknown"],
+      'sim_state': ["ABSENT"],
+      'data_connected': False
+    }
+
+  def get_sim_lpa(self) -> LPABase:
+    raise NotImplementedError("SIM LPA not available")
+
+  def get_network_strength(self, network_type):
+    return NetworkStrength.unknown
+
+  def get_network_metered(self, network_type) -> bool:
+    return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet)
+
+  def get_current_power_draw(self):
+    return 0
+
+  def get_som_power_draw(self):
+    return 0
+
+  def shutdown(self):
+    print("SHUTDOWN!")
+
+  def get_thermal_config(self):
+    return ThermalConfig()
+
+  def set_display_power(self, on: bool):
+    pass
+
+  def set_screen_brightness(self, percentage):
+    pass
+
+  def get_screen_brightness(self):
+    return 0
+
+  def set_power_save(self, powersave_enabled):
+    pass
+
+  def get_gpu_usage_percent(self):
+    return 0
+
+  def get_modem_version(self):
+    return None
+
+  def get_modem_temperatures(self):
+    return []
+
+  def initialize_hardware(self):
+    pass
+
+  def configure_modem(self):
+    pass
+
+  def reboot_modem(self):
+    pass
+
+  def recover_sim_detection(self) -> bool:
+    return False
+
+  def get_networks(self):
+    return None
+
+  def has_internal_panda(self) -> bool:
+    return False
+
+  def reset_internal_panda(self):
+    pass
+
+  def recover_internal_panda(self):
+    pass
+
+  def get_modem_data_usage(self):
+    return -1, -1
+
+  def get_voltage(self) -> float:
+    return 0.
+
+  def get_current(self) -> float:
+    return 0.
+
+  def set_ir_power(self, percent: int):
+    pass

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/hw.py

@@ -0,0 +1,137 @@
+import os
+import platform
+from pathlib import Path
+
+from iqpilot.system.hardware import PC
+
+DEFAULT_DOWNLOAD_CACHE_ROOT = "/tmp/comma_download_cache"
+
+class Paths:
+  _persist_root_cache: str | None = None
+
+  @staticmethod
+  def _is_writable_persist_root(path: str) -> bool:
+    try:
+      os.makedirs(path, exist_ok=True)
+      comma_dir = os.path.join(path, "comma")
+      os.makedirs(comma_dir, exist_ok=True)
+
+      probe_path = os.path.join(comma_dir, ".rw_probe")
+      with open(probe_path, "w") as f:
+        f.write("1")
+      os.remove(probe_path)
+      return True
+    except OSError:
+      return False
+
+  @staticmethod
+  def comma_home() -> str:
+    return os.path.join(str(Path.home()), ".comma" + os.environ.get("OPENPILOT_PREFIX", ""))
+
+  @staticmethod
+  def params() -> str:
+    if os.environ.get("PARAMS_ROOT"):
+      return os.environ["PARAMS_ROOT"]
+    return os.path.join(Paths.comma_home(), "params") if PC else "/data/params"
+
+  @staticmethod
+  def log_root() -> str:
+    if os.environ.get('LOG_ROOT', False):
+      return os.environ['LOG_ROOT']
+    elif PC:
+      return str(Path(Paths.comma_home()) / "media" / "0" / "realdata")
+    else:
+      return '/data/media/0/realdata/'
+
+  @staticmethod
+  def log_root_external() -> str:
+    return '/mnt/external_realdata/'
+
+  @staticmethod
+  def swaglog_root() -> str:
+    if PC:
+      return os.path.join(Paths.comma_home(), "log")
+    else:
+      return "/data/log/"
+
+  @staticmethod
+  def swaglog_ipc() -> str:
+    return "ipc:///tmp/logmessage" + os.environ.get("OPENPILOT_PREFIX", "")
+
+  @staticmethod
+  def download_cache_root() -> str:
+    if os.environ.get('COMMA_CACHE', False):
+      return os.environ['COMMA_CACHE'] + "/"
+    return DEFAULT_DOWNLOAD_CACHE_ROOT + os.environ.get("OPENPILOT_PREFIX", "") + "/"
+
+  @staticmethod
+  def persist_root() -> str:
+    if PC:
+      return os.path.join(Paths.comma_home(), "persist")
+
+    if Paths._persist_root_cache is not None:
+      return Paths._persist_root_cache
+
+    for candidate in ("/persist", "/data/persist"):
+      if Paths._is_writable_persist_root(candidate):
+        Paths._persist_root_cache = candidate
+        return candidate
+
+    # Keep previous behavior as a last resort.
+    Paths._persist_root_cache = "/persist"
+    return Paths._persist_root_cache
+
+  @staticmethod
+  def stats_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "stats")
+    else:
+      return "/data/stats/"
+
+  @staticmethod
+  def stats_iq_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "stats")
+    else:
+      return "/data/stats_iq/"
+
+  @staticmethod
+  def config_root() -> str:
+    if PC:
+      return Paths.comma_home()
+    else:
+      return "/tmp/.comma"
+
+  @staticmethod
+  def shm_path() -> str:
+    if PC and platform.system() == "Darwin":
+      return "/tmp"  # This is not really shared memory on macOS, but it's the closest we can get
+    return "/dev/shm"
+
+  @staticmethod
+  def model_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "media" / "0" / "models")
+    else:
+      return "/data/media/0/models"
+
+  @staticmethod
+  def crash_log_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "community" / "crashes")
+    else:
+      return "/data/community/crashes"
+
+  @staticmethod
+  def mapd_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "media" / "0" / "osm")
+    else:
+      return "/data/media/0/osm"
+
+  @staticmethod
+  def screen_recordings_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "media" / "0" / "screen_recordings")
+    else:
+      return "/data/media/0/screen_recordings"

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/hardware.py

@@ -0,0 +1,12 @@
+from iqpilot.cereal import log
+from iqpilot.system.hardware.base import HardwareBase
+
+NetworkType = log.DeviceState.NetworkType
+
+
+class Pc(HardwareBase):
+  def get_device_type(self):
+    return "pc"
+
+  def get_network_type(self):
+    return NetworkType.wifi

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+import time
+from collections import namedtuple
+
+from iqpilot.common.i2c import SMBus
+
+# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf
+
+AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask'])
+EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2'])
+
+
+def configs_from_eq_params(base, eq_params):
+  return [
+    AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF),
+    AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF),
+    AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF),
+    AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF),
+    AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF),
+    AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF),
+    AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF),
+    AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF),
+    AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF),
+    AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF),
+  ]
+
+
+BASE_CONFIG = [
+  AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000),
+  AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000),
+  AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011),
+  AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000),
+  AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
+  AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
+  AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
+  AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
+  AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
+  AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
+  AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
+  AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
+  AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
+  AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
+  AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
+  AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
+  AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
+  AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
+  AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
+  AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
+  AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011),
+  AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111),
+  AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000),
+  AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001),
+  AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000),
+  AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100),
+  AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010),
+  AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001),
+  AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000),
+  AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000),
+  AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000),
+  AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000),
+  AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000),
+  AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000),
+  AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
+]
+
+CONFIGS = {
+  "tici": [
+    AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
+    AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
+    AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
+    AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
+    *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
+    *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
+    *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
+    *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
+    *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
+  ],
+  "tizi": [
+    AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
+    AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
+    AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011),
+    AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
+    AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111),
+    AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111),
+    AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
+    AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001),
+    AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
+    AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111),
+    AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000),
+    AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111),
+    AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000),
+    AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001),
+    AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000),
+    AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000),
+    AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000),
+    AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000),
+    AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010),
+    AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001),
+    AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000),
+  ],
+}
+
+
+class Amplifier:
+  AMP_I2C_BUS = 0
+  AMP_ADDRESS = 0x10
+
+  def __init__(self, debug=False):
+    self.debug = debug
+
+  def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig:
+    return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)
+
+  def _set_configs(self, configs: list[AmpConfig]) -> None:
+    with SMBus(self.AMP_I2C_BUS) as bus:
+      for config in configs:
+        if self.debug:
+          print(f"Setting \"{config.name}\" to {config.value}:")
+
+        old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True)
+        new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask)
+        bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True)
+
+        if self.debug:
+          print(f"  Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}")
+
+  def set_configs(self, configs: list[AmpConfig]) -> bool:
+    tries = 15
+    backoff = 0.
+    for i in range(tries):
+      try:
+        self._set_configs(configs)
+        return True
+      except OSError:
+        backoff += 0.1
+        time.sleep(backoff)
+        print(f"Failed to set amp config, {tries - i - 1} retries left")
+    return False
+
+  def set_global_shutdown(self, amp_disabled: bool) -> bool:
+    return self.set_configs([self._get_shutdown_config(amp_disabled), ])
+
+  def initialize_configuration(self, model: str) -> bool:
+    cfgs = [
+      self._get_shutdown_config(True),
+      *BASE_CONFIG,
+      *CONFIGS[model],
+      self._get_shutdown_config(False),
+    ]
+    return self.set_configs(cfgs)
+
+
+if __name__ == "__main__":
+  with open("/sys/firmware/devicetree/base/model") as f:
+    model = f.read().strip('\x00')
+  model = model.split('comma ')[-1]
+
+  amp = Amplifier()
+  amp.initialize_configuration(model)

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/hardware.py

@@ -0,0 +1,740 @@
+import math
+import os
+import sys
+import subprocess
+import time
+import tempfile
+from enum import IntEnum
+from functools import cached_property, lru_cache
+from pathlib import Path
+
+from iqpilot.cereal import log
+from iqpilot.common.utils import sudo_read, sudo_write
+from iqpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action
+from iqpilot.system.hardware.base import HardwareBase, LPABase, ThermalConfig, ThermalZone
+from iqpilot.system.hardware.tici import iwlist
+from iqpilot.system.hardware.tici.lpa import TiciLPA
+from iqpilot.system.hardware.tici.pins import GPIO
+from iqpilot.system.hardware.tici.amplifier import Amplifier
+
+NM = 'org.freedesktop.NetworkManager'
+NM_CON_ACT = NM + '.Connection.Active'
+NM_DEV = NM + '.Device'
+NM_DEV_WL = NM + '.Device.Wireless'
+NM_DEV_STATS = NM + '.Device.Statistics'
+NM_AP = NM + '.AccessPoint'
+DBUS_PROPS = 'org.freedesktop.DBus.Properties'
+
+MM = 'org.freedesktop.ModemManager1'
+MM_MODEM = MM + ".Modem"
+MM_MODEM_SIMPLE = MM + ".Modem.Simple"
+MM_SIM = MM + ".Sim"
+
+class MM_MODEM_STATE(IntEnum):
+  FAILED        = -1
+  UNKNOWN       = 0
+  INITIALIZING  = 1
+  LOCKED        = 2
+  DISABLED      = 3
+  DISABLING     = 4
+  ENABLING      = 5
+  ENABLED       = 6
+  SEARCHING     = 7
+  REGISTERED    = 8
+  DISCONNECTING = 9
+  CONNECTING    = 10
+  CONNECTED     = 11
+
+class NMActiveConnectionState(IntEnum):
+  UNKNOWN = 0
+  ACTIVATING = 1
+  ACTIVATED = 2
+  DEACTIVATING = 3
+  DEACTIVATED = 4
+
+class NMMetered(IntEnum):
+  NM_METERED_UNKNOWN = 0
+  NM_METERED_YES = 1
+  NM_METERED_NO = 2
+  NM_METERED_GUESS_YES = 3
+  NM_METERED_GUESS_NO = 4
+
+TIMEOUT = 0.1
+REFRESH_RATE_MS = 1000
+
+NetworkType = log.DeviceState.NetworkType
+NetworkStrength = log.DeviceState.NetworkStrength
+
+# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology
+MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5
+MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14
+
+# MMModemStateFailedReason
+MM_MODEM_STATE_FAILED_REASON_SIM_MISSING = 2
+
+
+def affine_irq(val, action):
+  irqs = get_irqs_for_action(action)
+  if len(irqs) == 0:
+    return
+
+  for i in irqs:
+    sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list")
+
+@lru_cache
+def get_device_type():
+  # lru_cache and cache can cause memory leaks when used in classes
+  try:
+    with open("/sys/firmware/devicetree/base/model") as f:
+      model = f.read().strip('\x00')
+  except FileNotFoundError:
+    # off-device (e.g. the prebuilt build container fakes /TICI but has no
+    # devicetree); import must not crash. Not a real device type.
+    return "unknown"
+  return model.split('comma ')[-1]
+
+class Tici(HardwareBase):
+  @staticmethod
+  def _ensure_system_python_path() -> None:
+    system_site = "/usr/lib/python3/dist-packages"
+    if system_site not in sys.path and os.path.isdir(system_site):
+      sys.path.append(system_site)
+
+  @staticmethod
+  def _run_direct_modem_command(command: str) -> None:
+    import serial
+
+    last_error: Exception | None = None
+    for device in ("/dev/ttyUSB2", "/dev/ttyUSB3"):
+      if not os.path.exists(device):
+        continue
+
+      try:
+        with serial.Serial(device, baudrate=9600, timeout=2) as modem:
+          modem.reset_input_buffer()
+          modem.write((command + "\r").encode("ascii"))
+
+          deadline = time.monotonic() + 3.0
+          while time.monotonic() < deadline:
+            line = modem.readline().decode(errors="ignore").strip()
+            if not line:
+              continue
+            if line == "OK":
+              return
+            if line == "ERROR" or "ERROR" in line:
+              raise RuntimeError(f"{device}: {line}")
+          raise TimeoutError(f"{device}: timed out waiting for modem response")
+      except Exception as e:
+        last_error = e
+
+    if last_error is not None:
+      raise last_error
+    raise RuntimeError("No modem AT port available")
+
+  @cached_property
+  def bus(self):
+    try:
+      import dbus
+    except ModuleNotFoundError:
+      self._ensure_system_python_path()
+      import dbus
+    return dbus.SystemBus()
+
+  @cached_property
+  def nm(self):
+    return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
+
+  @property # this should not be cached, in case the modemmanager restarts
+  def mm(self):
+    return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
+
+  @cached_property
+  def amplifier(self):
+    if self.get_device_type() == "mici":
+      return None
+    if os.path.exists('/tmp/lite_hw') or os.environ.get('LITE') == '1':
+      return None
+    return Amplifier()
+
+  def get_os_version(self):
+    with open("/VERSION") as f:
+      return f.read().strip()
+
+  def get_device_type(self):
+    return get_device_type()
+
+  def reboot(self, reason=None):
+    subprocess.check_output(["sudo", "reboot"])
+
+  def uninstall(self):
+    Path("/data/__system_reset__").touch()
+    os.sync()
+    self.reboot()
+
+  def get_serial(self):
+    return self.get_cmdline()['androidboot.serialno']
+
+  def get_voltage(self):
+    with open("/sys/class/hwmon/hwmon1/in1_input") as f:
+      return int(f.read())
+
+  def get_current(self):
+    with open("/sys/class/hwmon/hwmon1/curr1_input") as f:
+      return int(f.read())
+
+  def set_ir_power(self, percent: int):
+    if self.get_device_type() in ("tici", "tizi"):
+      return
+
+    value = int((percent / 100) * 300)
+    with open("/sys/class/leds/led:switch_2/brightness", "w") as f:
+      f.write("0\n")
+    with open("/sys/class/leds/led:torch_2/brightness", "w") as f:
+      f.write(f"{value}\n")
+    with open("/sys/class/leds/led:switch_2/brightness", "w") as f:
+      f.write(f"{value}\n")
+
+  def get_network_type(self):
+    try:
+      primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      primary_connection = self.bus.get_object(NM, primary_connection)
+      primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+      if primary_type == '802-3-ethernet':
+        return NetworkType.ethernet
+      elif primary_type == '802-11-wireless':
+        return NetworkType.wifi
+      else:
+        active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+        for conn in active_connections:
+          c = self.bus.get_object(NM, conn)
+          tp = c.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+          if tp == 'gsm':
+            modem = self.get_modem()
+            modem_state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+            if modem_state < MM_MODEM_STATE.REGISTERED:
+              return NetworkType.none
+            access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+            if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE:
+              return NetworkType.cell4G
+            elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS:
+              return NetworkType.cell3G
+            else:
+              return NetworkType.cell2G
+    except Exception:
+      pass
+
+    return NetworkType.none
+
+  def get_modem(self):
+    objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=TIMEOUT)
+    if not objects:
+      raise RuntimeError("ModemManager returned no modems")
+    modem_path = next(iter(objects))
+    return self.bus.get_object(MM, modem_path)
+
+  def get_wlan(self):
+    wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM, timeout=TIMEOUT)
+    return self.bus.get_object(NM, wlan_path)
+
+  def get_wwan(self):
+    wwan_path = self.nm.GetDeviceByIpIface('wwan0', dbus_interface=NM, timeout=TIMEOUT)
+    return self.bus.get_object(NM, wwan_path)
+
+  def get_sim_info(self):
+    modem = self.get_modem()
+    sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+    if sim_path == "/":
+      return {
+        'sim_id': '',
+        'mcc_mnc': None,
+        'network_type': ["Unknown"],
+        'sim_state': ["ABSENT"],
+        'data_connected': False
+      }
+    else:
+      sim = self.bus.get_object(MM, sim_path)
+      return {
+        'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
+        'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
+        'network_type': ["Unknown"],
+        'sim_state': ["READY"],
+        'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED,
+      }
+
+  def get_sim_lpa(self) -> LPABase:
+    return TiciLPA()
+
+  def get_imei(self, slot):
+    if slot != 0:
+      return ""
+
+    return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
+
+  def get_network_info(self):
+    if self.get_device_type() == "mici":
+      return None
+    try:
+      modem = self.get_modem()
+      info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+    except Exception:
+      return None
+
+    if info and info.startswith('+QNWINFO: '):
+      info = info.replace('+QNWINFO: ', '').replace('"', '').split(',')
+      extra = "" if extra is None else extra.replace('+QENG: "servingcell",', '').replace('"', '')
+      state = "" if state is None else MM_MODEM_STATE(state).name
+
+      if len(info) != 4:
+        return None
+
+      technology, operator, band, channel = info
+
+      return({
+        'technology': technology,
+        'operator': operator,
+        'band': band,
+        'channel': int(channel),
+        'extra': extra,
+        'state': state,
+      })
+    else:
+      return None
+
+  def parse_strength(self, percentage):
+    if percentage < 25:
+      return NetworkStrength.poor
+    elif percentage < 50:
+      return NetworkStrength.moderate
+    elif percentage < 75:
+      return NetworkStrength.good
+    else:
+      return NetworkStrength.great
+
+  def get_network_strength(self, network_type):
+    network_strength = NetworkStrength.unknown
+
+    try:
+      if network_type == NetworkType.none:
+        pass
+      elif network_type == NetworkType.wifi:
+        wlan = self.get_wlan()
+        active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+        if active_ap_path != "/":
+          active_ap = self.bus.get_object(NM, active_ap_path)
+          strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
+          network_strength = self.parse_strength(strength)
+      else:  # Cellular
+        modem = self.get_modem()
+        strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)[0])
+        network_strength = self.parse_strength(strength)
+    except Exception:
+      pass
+
+    return network_strength
+
+  def get_network_metered(self, network_type) -> bool:
+    try:
+      primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      primary_connection = self.bus.get_object(NM, primary_connection)
+      primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+      for dev in primary_devices:
+        dev_obj = self.bus.get_object(NM, str(dev))
+        metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+        if network_type == NetworkType.wifi:
+          if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]:
+            return True
+        elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]:
+          if metered_prop == NMMetered.NM_METERED_NO:
+            return False
+    except Exception:
+      pass
+
+    return super().get_network_metered(network_type)
+
+  def get_modem_version(self):
+    try:
+      modem = self.get_modem()
+      return modem.Get(MM_MODEM, 'Revision', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+    except Exception:
+      return None
+
+  def get_modem_temperatures(self):
+    timeout = 0.2  # Default timeout is too short
+    try:
+      modem = self.get_modem()
+      temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)
+      return list(filter(lambda t: t != 255, map(int, temps.split(' ')[1].split(','))))
+    except Exception:
+      return []
+
+
+  def get_current_power_draw(self):
+    return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6)
+
+  def get_som_power_draw(self):
+    return (self.read_param_file("/sys/class/power_supply/bms/voltage_now", int) * self.read_param_file("/sys/class/power_supply/bms/current_now", int) / 1e12)
+
+  def shutdown(self):
+    os.system("sudo poweroff")
+
+  def get_thermal_config(self):
+    intake, exhaust, case = None, None, None
+    if self.get_device_type() == "mici":
+      case = ThermalZone("case")
+      intake = ThermalZone("intake")
+      exhaust = ThermalZone("exhaust")
+    return ThermalConfig(cpu=[ThermalZone(f"cpu{i}-silver-usr") for i in range(4)] +
+                             [ThermalZone(f"cpu{i}-gold-usr") for i in range(4)],
+                         gpu=[ThermalZone("gpu0-usr"), ThermalZone("gpu1-usr")],
+                         dsp=ThermalZone("compute-hvx-usr"),
+                         memory=ThermalZone("ddr-usr"),
+                         pmic=[ThermalZone("pm8998_tz"), ThermalZone("pm8005_tz")],
+                         intake=intake,
+                         exhaust=exhaust,
+                         case=case)
+
+  def set_display_power(self, on):
+    try:
+      with open("/sys/class/backlight/panel0-backlight/bl_power", "w") as f:
+        f.write("0" if on else "4")
+    except Exception:
+      pass
+
+  def set_screen_brightness(self, percentage):
+    try:
+      with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
+        max_brightness = float(f.read().strip())
+
+      val = int(percentage * (max_brightness / 100.))
+      with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f:
+        f.write(str(val))
+    except Exception:
+      pass
+
+  def get_screen_brightness(self):
+    try:
+      with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
+        max_brightness = float(f.read().strip())
+
+      with open("/sys/class/backlight/panel0-backlight/brightness") as f:
+        return int(float(f.read()) / (max_brightness / 100.))
+    except Exception:
+      return 0
+
+  def set_power_save(self, powersave_enabled):
+    # amplifier, 100mW at idle
+    if self.amplifier is not None:
+      self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled)
+      if not powersave_enabled:
+        self.amplifier.initialize_configuration(self.get_device_type())
+
+    # *** CPU config ***
+
+    # offline big cluster
+    for i in range(4, 8):
+      val = '0' if powersave_enabled else '1'
+      sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online')
+
+    for n in ('0', '4'):
+      if powersave_enabled and n == '4':
+        continue
+      gov = 'ondemand' if powersave_enabled else 'performance'
+      sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor')
+
+    # *** IRQ config ***
+
+    # GPU, modeld core
+    affine_irq(7, "kgsl-3d0")
+
+    # camerad core
+    camera_irqs = ("a5", "cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid-lite", "ife-lite")
+    for n in camera_irqs:
+      affine_irq(6, n)
+
+  def get_gpu_usage_percent(self):
+    try:
+      with open('/sys/class/kgsl/kgsl-3d0/gpubusy') as f:
+        used, total = f.read().strip().split()
+      return 100.0 * int(used) / int(total)
+    except Exception:
+      return 0
+
+  def initialize_hardware(self):
+    if self.amplifier is not None:
+      self.amplifier.initialize_configuration(self.get_device_type())
+
+    # Allow hardwared to write engagement status to kmsg
+    os.system("sudo chmod a+w /dev/kmsg")
+
+    # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL
+    gpio_init(GPIO.SOM_ST_IO, True)
+    gpio_set(GPIO.SOM_ST_IO, 1)
+
+    # *** IRQ config ***
+
+    # mask off big cluster from default affinity
+    sudo_write("f", "/proc/irq/default_smp_affinity")
+
+    # move these off the default core
+    affine_irq(1, "msm_vidc")  # encoders
+    affine_irq(1, "i2c_geni")  # sensors
+
+    # *** GPU config ***
+    # https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216
+    affine_irq(5, "fts_ts")    # touch
+    affine_irq(5, "msm_drm")   # display
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on")
+    sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
+    sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
+    sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
+    sudo_write("710", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
+
+    # setup governors
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor")
+    sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor")
+
+    # *** VIDC (encoder) config ***
+    sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling")
+    sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation")
+
+    # pandad core
+    affine_irq(3, "spi_geni")         # SPI
+    if "tici" in self.get_device_type():
+      affine_irq(3, "xhci-hcd:usb3")
+      affine_irq(3, "xhci-hcd:usb1")
+    try:
+      pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip()
+      subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid])
+      subprocess.call(["sudo", "taskset", "-pc", "3", pid], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+    except subprocess.CalledProcessException as e:
+      print(str(e))
+
+  def configure_modem(self):
+    from iqpilot.common.params import Params
+
+    sim_info = self.get_sim_info()
+    sim_id = sim_info.get('sim_id', '')
+    params = Params()
+    manual_apn = params.get("GsmApn", encoding="utf-8") or ""
+    metered_enabled = params.get_bool("GsmMetered")
+
+    modem = self.get_modem()
+    try:
+      manufacturer = str(modem.Get(MM_MODEM, 'Manufacturer', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
+    except Exception:
+      manufacturer = None
+
+    cmds = []
+    is_comma_profile = self.get_sim_lpa().is_comma_profile(sim_id)
+    roaming_enabled = params.get_bool("GsmRoaming")
+    initial_eps_apn = "" if is_comma_profile else manual_apn
+
+    if not is_comma_profile and params.get("GsmRoaming") is None:
+      params.put_bool("GsmRoaming", True)
+      roaming_enabled = True
+
+    subprocess.call([
+      "nmcli", "connection", "modify", "lte",
+      "gsm.auto-config", "no" if manual_apn else "yes",
+      "gsm.apn", manual_apn,
+      "gsm.home-only", "no" if roaming_enabled else "yes",
+      "gsm.network-id", "",
+      "gsm.initial-eps-bearer-configure", "yes" if initial_eps_apn else "no",
+      "gsm.initial-eps-bearer-apn", initial_eps_apn,
+      "connection.metered", "unknown" if metered_enabled else "no",
+    ], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
+
+    if self.get_device_type() in ("tici", "tizi"):
+      if initial_eps_apn:
+        subprocess.call(["mmcli", "-m", "any", f'--3gpp-set-initial-eps-bearer-settings=apn={initial_eps_apn}'])
+      else:
+        subprocess.call(["mmcli", "-m", "any", '--3gpp-set-initial-eps-bearer-settings=apn='])
+
+      cmds += [
+        # configure modem as data-centric
+        'AT+QNVW=5280,0,"0102000000000000"',
+        'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
+        'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
+      ]
+      if self.get_device_type() == "tizi":
+        cmds += [
+          'AT+QSIMDET=1,0',
+          'AT+QSIMSTAT=1',
+        ]
+    elif manufacturer == 'Cavli Inc.':
+      cmds += [
+        'AT^SIMSWAP=1',     # use SIM slot, instead of internal eSIM
+        'AT$QCSIMSLEEP=0',  # disable SIM sleep
+        'AT$QCSIMCFG=SimPowerSave,0',  # more sleep disable
+
+        # ethernet config
+        'AT$QCPCFG=usbNet,0',
+        'AT$QCNETDEVCTL=3,1',
+      ]
+    else:
+      # this modem gets upset with too many AT commands
+      if sim_id is None or len(sim_id) == 0:
+        cmds += [
+          # SIM sleep disable
+          'AT$QCSIMSLEEP=0',
+          'AT$QCSIMCFG=SimPowerSave,0',
+
+          # ethernet config
+          'AT$QCPCFG=usbNet,1',
+        ]
+
+    for cmd in cmds:
+      try:
+        modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      except Exception:
+        pass
+
+    # eSIM prime
+    dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
+    if self.get_sim_lpa().is_comma_profile(sim_id) and not os.path.exists(dest):
+      with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
+        dat = f.read()
+        dat = dat.replace("sim-id=", f"sim-id={sim_id}")
+        tf.write(dat)
+        tf.flush()
+
+        # needs to be root
+        os.system(f"sudo cp {tf.name} {dest}")
+      os.system(f"sudo nmcli con load {dest}")
+
+  def recover_sim_detection(self) -> bool:
+    # A worn SIM-tray presence switch can read "removed" while the SIM pads still make
+    # contact; with hot-swap detect armed (AT+QSIMDET=1) the modem never powers the SIM
+    # and lands in failed/sim-missing. Disabling detect and rebooting the modem makes it
+    # probe the SIM electrically. Safe to retry on failure: firing disarms the QSIMDET
+    # gate, so a genuinely SIM-less device gets at most one extra modem reboot per boot.
+    if self.get_device_type() not in ("tici", "tizi"):
+      return False
+
+    try:
+      modem = self.get_modem()
+      state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      if state != MM_MODEM_STATE.FAILED:
+        return False
+      reason = modem.Get(MM_MODEM, 'StateFailedReason', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      if reason != MM_MODEM_STATE_FAILED_REASON_SIM_MISSING:
+        return False
+      detect = str(modem.Command('AT+QSIMDET?', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)).strip()
+      if not detect.startswith('+QSIMDET: 1'):
+        return False
+      modem.Command('AT+QSIMDET=0,0', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      modem.Command('AT+CFUN=1,1', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+      return True
+    except Exception:
+      return False
+
+  def reboot_modem(self):
+    modem = None
+    try:
+      modem = self.get_modem()
+    except Exception:
+      pass
+
+    if modem is not None:
+      for state in (0, 1):
+        try:
+          modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
+        except Exception:
+          pass
+      return
+
+    for state in (0, 1):
+      try:
+        self._run_direct_modem_command(f"AT+CFUN={state}")
+      except Exception:
+        pass
+
+  def get_networks(self):
+    r = {}
+
+    wlan = iwlist.scan()
+    if wlan is not None:
+      r['wlan'] = wlan
+
+    lte_info = self.get_network_info()
+    if lte_info is not None:
+      extra = lte_info['extra']
+
+      # <state>,"LTE",<is_tdd>,<mcc>,<mnc>,<cellid>,<pcid>,<earfcn>,<freq_band_ind>,
+      # <ul_bandwidth>,<dl_bandwidth>,<tac>,<rsrp>,<rsrq>,<rssi>,<sinr>,<srxlev>
+      if 'LTE' in extra:
+        extra = extra.split(',')
+        try:
+          r['lte'] = [{
+            "mcc": int(extra[3]),
+            "mnc": int(extra[4]),
+            "cid": int(extra[5], 16),
+            "nmr": [{"pci": int(extra[6]), "earfcn": int(extra[7])}],
+          }]
+        except (ValueError, IndexError):
+          pass
+
+    return r
+
+  def get_modem_data_usage(self):
+    try:
+      wwan = self.get_wwan()
+
+      # Ensure refresh rate is set so values don't go stale
+      refresh_rate = wwan.Get(NM_DEV_STATS, 'RefreshRateMs', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      if refresh_rate != REFRESH_RATE_MS:
+        u = type(refresh_rate)
+        wwan.Set(NM_DEV_STATS, 'RefreshRateMs', u(REFRESH_RATE_MS), dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+
+      tx = wwan.Get(NM_DEV_STATS, 'TxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      rx = wwan.Get(NM_DEV_STATS, 'RxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
+      return int(tx), int(rx)
+    except Exception:
+      return -1, -1
+
+  def has_internal_panda(self):
+    return True
+
+  def reset_internal_panda(self):
+    gpio_init(GPIO.STM_RST_N, True)
+    gpio_init(GPIO.STM_BOOT0, True)
+
+    gpio_set(GPIO.STM_RST_N, 1)
+    gpio_set(GPIO.STM_BOOT0, 0)
+    time.sleep(1)
+    gpio_set(GPIO.STM_RST_N, 0)
+
+  def recover_internal_panda(self):
+    gpio_init(GPIO.STM_RST_N, True)
+    gpio_init(GPIO.STM_BOOT0, True)
+
+    gpio_set(GPIO.STM_RST_N, 1)
+    gpio_set(GPIO.STM_BOOT0, 1)
+    time.sleep(0.5)
+    gpio_set(GPIO.STM_RST_N, 0)
+    time.sleep(0.5)
+    gpio_set(GPIO.STM_BOOT0, 0)
+
+  def booted(self):
+    # this normally boots within 8s, but on rare occasions takes 30+s
+    encoder_state = sudo_read("/sys/kernel/debug/msm_vidc/core0/info")
+    if "Core state: 0" in encoder_state and (time.monotonic() < 60*2):
+      return False
+    return True
+
+if __name__ == "__main__":
+  t = Tici()
+  t.configure_modem()
+  t.initialize_hardware()
+  t.set_power_save(False)
+  print(t.get_sim_info())

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py

@@ -0,0 +1,35 @@
+import subprocess
+
+
+def scan(interface="wlan0"):
+  result = []
+  try:
+    r = subprocess.check_output(["iwlist", interface, "scan"], encoding='utf8')
+
+    mac = None
+    for line in r.split('\n'):
+      if "Address" in line:
+        # Based on the adapter eithere a percentage or dBm is returned
+        # Add previous network in case no dBm signal level was seen
+        if mac is not None:
+          result.append({"mac": mac})
+          mac = None
+
+        mac = line.split(' ')[-1]
+      elif "dBm" in line:
+        try:
+          level = line.split('Signal level=')[1]
+          rss = int(level.split(' ')[0])
+          result.append({"mac": mac, "rss": rss})
+          mac = None
+        except ValueError:
+          continue
+
+    # Add last network if no dBm was found
+    if mac is not None:
+      result.append({"mac": mac})
+
+    return result
+
+  except Exception:
+    return None

artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/pins.py

@@ -0,0 +1,30 @@
+# GPIO pin definitions
+class GPIO:
+  # both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
+  HUB_RST_N = 30
+  UBLOX_RST_N = 32
+  UBLOX_SAFEBOOT_N = 33
+  GNSS_PWR_EN = 34 # SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN
+
+  STM_RST_N = 124
+  STM_BOOT0 = 134
+  STM_PWR_EN_N = 41 # because STM32H7 RST doesn't generate a full power-on-reset
+
+  SIREN = 42
+  SOM_ST_IO = 49
+
+  LTE_RST_N = 50
+  LTE_PWRKEY = 116
+  LTE_BOOT = 52
+
+  # GPIO_CAM0_DVDD_EN = /sys/kernel/debug/regulator/camera_rear_ldo
+  CAM0_AVDD_EN = 8
+  CAM0_RSTN = 9
+  CAM1_RSTN = 7
+  CAM2_RSTN = 12
+
+  # Sensor interrupts
+  BMX055_ACCEL_INT = 21
+  BMX055_GYRO_INT = 23
+  BMX055_MAGN_INT = 87
+  LSM_INT = 84