IQ.Pilot Release Commit @ 0798119

iqpilot/selfdrive/assets/offroad/icon_home.svg

@@ -0,0 +1,12 @@
+<svg
+  xmlns="http://www.w3.org/2000/svg"
+  width="48"
+  height="48"
+  viewBox="0 0 24 24"
+  fill="#ffffff"
+>
+  <path
+    fill-rule="evenodd"
+    d="M 4 2 h 16 a 2 2 0 0 1 2 2 v 6 a 2 2 0 0 1 -2 2 h -4.5 c -1.3 0 -1.9 3.2 -3.5 3.2 s -2.2 -3.2 -3.5 -3.2 H 4 a 2 2 0 0 1 -2 -2 V 4 a 2 2 0 0 1 2 -2 z M 4 3.2 h 16 a 0.8 0.8 0 0 1 0.8 0.8 v 6.2 a 0.8 0.8 0 0 1 -0.8 0.8 H 4 a 0.8 0.8 0 0 1 -0.8 -0.8 V 4 a 0.8 0.8 0 0 1 0.8 -0.8 z M 12 12.95 a 0.65 0.65 0 1 0 0 1.3 a 0.65 0.65 0 0 0 0 -1.3 z M 10.5 12.6 a 0.3 0.3 0 1 0 0 0.6 a 0.3 0.3 0 0 0 0 -0.6 z M 13.5 12.6 a 0.3 0.3 0 1 0 0 0.6 a 0.3 0.3 0 0 0 0 -0.6 z"
+  />
+</svg>

iqpilot/selfdrive/assets/offroad/icon_longitudinal.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><path fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2" d="m12 14l4-4M3.34 19a10 10 0 1 1 17.32 0"/></svg>

iqpilot/selfdrive/assets/offroad/icon_models.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><g fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2"><path d="M12 5a3 3 0 1 0-5.997.125a4 4 0 0 0-2.526 5.77a4 4 0 0 0 .556 6.588A4 4 0 1 0 12 18Z"/><path d="M9 13a4.5 4.5 0 0 0 3-4M6.003 5.125A3 3 0 0 0 6.401 6.5m-2.924 4.396a4 4 0 0 1 .585-.396M6 18a4 4 0 0 1-1.967-.516M12 13h4m-4 5h6a2 2 0 0 1 2 2v1M12 8h8m-4 0V5a2 2 0 0 1 2-2"/><circle cx="16" cy="13" r=".5"/><circle cx="18" cy="3" r=".5"/><circle cx="20" cy="21" r=".5"/><circle cx="20" cy="8" r=".5"/></g></svg>

iqpilot/selfdrive/assets/offroad/icon_software.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><g fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2"><path d="M12 15V3m9 12v4a2 2 0 0 1-2 2H5a2 2 0 0 1-2-2v-4"/><path d="m7 10l5 5l5-5"/></g></svg>

iqpilot/selfdrive/assets/offroad/icon_toggle.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><g fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2"><circle cx="9" cy="12" r="3"/><rect width="20" height="14" x="2" y="5" rx="7"/></g></svg>

iqpilot/selfdrive/assets/offroad/icon_vehicle.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><g fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2"><path d="m21 8l-2 2l-1.5-3.7A2 2 0 0 0 15.646 5H8.4a2 2 0 0 0-1.903 1.257L5 10L3 8m4 6h.01M17 14h.01"/><rect width="18" height="8" x="3" y="10" rx="2"/><path d="M5 18v2m14-2v2"/></g></svg>

iqpilot/selfdrive/assets/offroad/icon_visuals.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="1em" height="1em" viewBox="0 0 24 24"><g fill="none" stroke="white" stroke-linecap="round" stroke-linejoin="round" stroke-width="2"><path d="M12 22a1 1 0 0 1 0-20a10 9 0 0 1 10 9a5 5 0 0 1-5 5h-2.25a1.75 1.75 0 0 0-1.4 2.8l.3.4a1.75 1.75 0 0 1-1.4 2.8z"/><circle cx="13.5" cy="6.5" r=".5" fill="white"/><circle cx="17.5" cy="10.5" r=".5" fill="white"/><circle cx="6.5" cy="12.5" r=".5" fill="white"/><circle cx="8.5" cy="7.5" r=".5" fill="white"/></g></svg>

iqpilot/selfdrive/car/enhanced_stock_longitudinal_control.py

@@ -0,0 +1,67 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from __future__ import annotations
+
+from openpilot.common.constants import CV
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+
+from iqdbc.car import structs
+
+ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY = "enhancedStockLongitudinalControl.setSpeedKph"
+
+
+def _float_param(key: str, value: float) -> dict[str, object]:
+  return {"key": key, "type": "float", "value": f"{float(value):.3f}".encode("utf-8")}
+
+
+def _clamp_set_speed_kph(value: float) -> float:
+  return max(0.0, min(V_CRUISE_MAX, float(value)))
+
+
+def build_iq_control_params_from_plan(CP: structs.CarParams, iq_plan, selfdrive_enabled: bool,
+                                      current_set_speed_kph: float, previous_sync_limit_kph: float | None,
+                                      pending_sync_limit_kph: float | None) -> tuple[list[dict[str, object]], float | None, float | None]:
+  if not CP.openpilotLongitudinalControl or not selfdrive_enabled:
+    return [], None, None
+
+  resolver = getattr(getattr(iq_plan, "speedLimit", None), "resolver", None)
+  assist = getattr(getattr(iq_plan, "speedLimit", None), "assist", None)
+  if resolver is None or assist is None:
+    return [], None, None
+
+  speed_limit_final_last = float(getattr(resolver, "speedLimitFinalLast", 0.0) or 0.0)
+  assist_enabled = bool(getattr(assist, "enabled", False))
+  if not assist_enabled or speed_limit_final_last <= 0.0:
+    return [], None, None
+
+  resolved_limit_kph = _clamp_set_speed_kph(speed_limit_final_last * CV.MS_TO_KPH)
+  limit_changed = previous_sync_limit_kph is None or abs(resolved_limit_kph - previous_sync_limit_kph) > 0.05
+  if limit_changed:
+    pending_sync_limit_kph = resolved_limit_kph
+
+  if pending_sync_limit_kph is not None:
+    if abs(current_set_speed_kph - pending_sync_limit_kph) <= 0.25:
+      pending_sync_limit_kph = None
+      set_speed_kph = _clamp_set_speed_kph(current_set_speed_kph or resolved_limit_kph)
+    else:
+      set_speed_kph = pending_sync_limit_kph
+  else:
+    set_speed_kph = _clamp_set_speed_kph(current_set_speed_kph or resolved_limit_kph)
+
+  return [_float_param(ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY, set_speed_kph)], resolved_limit_kph, pending_sync_limit_kph
+
+
+def get_set_speed_kph_from_params(params) -> float | None:
+  for param in params:
+    key = param.key if hasattr(param, "key") else param.get("key")
+    if key != ENHANCED_STOCK_LONGITUDINAL_CONTROL_SET_SPEED_KPH_KEY:
+      continue
+    raw_value = param.value if hasattr(param, "value") else param.get("value")
+    try:
+      raw = raw_value.decode("utf-8") if isinstance(raw_value, (bytes, bytearray)) else str(raw_value)
+      value = float(raw)
+    except (AttributeError, TypeError, ValueError):
+      return None
+    return _clamp_set_speed_kph(value)
+  return None

iqpilot/selfdrive/car/gap_button_actions.py

@@ -0,0 +1,52 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Maps the distance/gap steering-wheel button to an IQ.Pilot action: holding it for
+long enough toggles Experimental mode exactly once per hold. Only active when
+IQ.Pilot owns longitudinal control and cruise is available.
+"""
+from cereal import car, custom
+from iqdbc.car import structs
+from openpilot.common.params import Params
+
+_Button = car.CarState.ButtonEvent.Type
+_IQEvent = custom.IQOnroadEvent.EventName
+_GAP_BUTTON = _Button.gapAdjustCruise
+
+HOLD_FRAMES_TO_TOGGLE = 50
+
+
+class GapButtonActions:
+  def __init__(self, CP: structs.CarParams):
+    self._CP = CP
+    self._params = Params()
+    self._gap_hold_frames = 0
+    self._already_toggled = False
+    # read (and cleared) by the personality-decrement handler in selfdrived so a
+    # release that ends a toggle-hold does not also decrement personality
+    self.experimental_mode_switched = False
+
+  def update(self, CS, events, experimental_mode) -> None:
+    if not (self._CP.openpilotLongitudinalControl and CS.cruiseState.available):
+      return
+    self._advance_hold(CS)
+    self._toggle_experimental_on_long_hold(events, experimental_mode)
+
+  def _advance_hold(self, CS) -> None:
+    # once counting, keep incrementing each frame the hold persists
+    if self._gap_hold_frames > 0:
+      self._gap_hold_frames += 1
+    # a fresh press seeds the counter; a release zeroes it
+    for be in CS.buttonEvents:
+      if be.type.raw == _GAP_BUTTON:
+        self._gap_hold_frames = int(be.pressed)
+        if not be.pressed:
+          self._already_toggled = False
+
+  def _toggle_experimental_on_long_hold(self, events, experimental_mode) -> None:
+    if self._already_toggled or self._gap_hold_frames < HOLD_FRAMES_TO_TOGGLE:
+      return
+    self._params.put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+    events.add(_IQEvent.experimentalToggled)
+    self._already_toggled = True
+    self.experimental_mode_switched = True

iqpilot/selfdrive/car/interfaces.py

@@ -0,0 +1,76 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqdbc.car import structs as _dbc
+from openpilot.common.params import Params as _Store
+from openpilot.common.swaglog import cloudlog as _log
+from openpilot.selfdrive.controls.lib.latcontrol_torque import get_nn_model_path as _resolve_nn
+
+import openpilot.system.sentry as _telemetry
+
+_ANGLE = _dbc.CarParams.SteerControlType.angle
+
+# Port tunables surfaced to the fingerprint step, flat so the read is one pass.
+_TUNABLES = (
+  "IQHyundaiLongTune",
+  "IQSubaruCreepAssist",
+  "IQSubaruCreepAssistManualBrake",
+  "IQTeslaTorqueBlend",
+  "IQToyotaFactoryLong",
+  "ToyotaSnGHack",
+)
+
+
+def initialize_params(store):
+  return [{name: store.get(name, return_default=True)} for name in _TUNABLES]
+
+
+def log_fingerprint(cp) -> None:
+  ident = cp.carFingerprint
+  if ident == "MOCK":
+    _telemetry.capture_fingerprint_mock()
+  else:
+    _telemetry.capture_fingerprint(ident, cp.brand)
+
+
+def set_speed_limit_controller_availability(cp, cp_iq, store=None) -> bool:
+  """Gate the speed-limit controller off on platforms that can't run it, dropping a
+  stuck 'control' mode down to 'warning'."""
+  store = store or _Store()
+  brand = cp.brand
+  off = (brand == "rivian"
+         or (brand == "tesla" and store.get_bool("IsReleaseIqBranch"))
+         or (not cp.openpilotLongitudinalControl and cp_iq.pcmCruiseSpeed))
+  if off and store.get("IQSpeedAssistMode", return_default=True) == 3:  # control -> warning
+    store.put("IQSpeedAssistMode", 2)
+  return not off
+
+
+def _stamp_lateral_model(cp, cp_iq, store) -> bool:
+  where, label, precise = _resolve_nn(cp)
+  nn = cp_iq.iqLateralNet
+  nn.model.path, nn.model.name, nn.fuzzyFingerprint = where, label, not precise
+  if label == "MOCK":
+    _log.error({"nnff event": "car doesn't match any Neural Network model"})
+    return False
+  return cp.steerControlType != _ANGLE and store.get_bool("NeuralNetworkFeedForward")
+
+
+def _cleanup_unsupported_params(cp, cp_iq, store=None) -> None:
+  store = store or _Store()
+  doomed = {
+    "NeuralNetworkFeedForward": cp.steerControlType == _ANGLE,
+    "LongIncrementsEnabled": not cp.openpilotLongitudinalControl and cp_iq.pcmCruiseSpeed,
+  }
+  for name, gone in doomed.items():
+    if gone:
+      _log.warning(f"unsupported on this port, clearing {name}")
+      store.remove(name)
+  set_speed_limit_controller_availability(cp, cp_iq, store)
+
+
+def apply_iq_car_config(ci, store=None) -> None:
+  store = store or _Store()
+  if _stamp_lateral_model(ci.CP, ci.CP_IQ, store):
+    ci.configure_torque_tune(ci.CP.carFingerprint, ci.CP.lateralTuning)
+  _cleanup_unsupported_params(ci.CP, ci.CP_IQ, store)

iqpilot/selfdrive/car/long_increments.py

@@ -0,0 +1,58 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from dataclasses import dataclass
+
+from openpilot.common.params import Params
+
+# Cruise set-speed step (in the caller's working unit, kph or the imperial increment)
+# a user is allowed to dial in for the accel/decel cruise buttons.
+MIN_BUTTON_STEP = 1
+MAX_BUTTON_STEP = 10
+
+# Once the resolved step reaches this size, we snap the set speed to the nearest
+# multiple of the step (e.g. a step of 5 lands on 45/50/55...) instead of just
+# adding it on top of whatever odd number the set speed currently sits at.
+SNAP_TO_GRID_THRESHOLD = 5
+
+# Stock behavior (feature disabled): tap moves by one unit, a held button moves
+# five times faster. This mirrors what every other unmodified button-input car
+# already does, so it's kept as the fallback rather than living in this module.
+STOCK_HOLD_MULTIPLIER = 5
+
+
+@dataclass(frozen=True)
+class LongIncrementConfig:
+  enabled: bool
+  tap_step: int
+  hold_step: int
+
+
+def _clamp_step(value) -> int:
+  try:
+    step = int(value)
+  except (TypeError, ValueError):
+    return MIN_BUTTON_STEP
+  return min(max(step, MIN_BUTTON_STEP), MAX_BUTTON_STEP)
+
+
+def read_long_increment_config(params: Params) -> LongIncrementConfig:
+  return LongIncrementConfig(
+    enabled=params.get_bool("LongIncrementsEnabled"),
+    tap_step=_clamp_step(params.get("LongIncrementTapStep", return_default=True)),
+    hold_step=_clamp_step(params.get("LongIncrementHoldStep", return_default=True)),
+  )
+
+
+def resolve_button_step(config: LongIncrementConfig, held: bool, unit_step: float) -> tuple[bool, float]:
+  """
+  Turn a single tap/hold cruise button event into a (snap_to_grid, delta) pair,
+  where delta is expressed in the same unit as unit_step (kph, or the mph-derived
+  increment used for imperial cars).
+  """
+  if not config.enabled:
+    return held, unit_step * (STOCK_HOLD_MULTIPLIER if held else 1)
+
+  multiplier = config.hold_step if held else config.tap_step
+  snap_to_grid = multiplier >= SNAP_TO_GRID_THRESHOLD
+  return snap_to_grid, unit_step * multiplier

iqpilot/selfdrive/car/refresh_car_list.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.iqpilot.selfdrive.car.vehicle_catalog import load_catalog
+
+
+def refresh_car_list_param() -> None:
+  platforms = load_catalog()
+  if not platforms:
+    cloudlog.warning("vehicle catalog not found; leaving CarList param unchanged")
+    return
+
+  params = Params()
+  if params.get("CarList") == platforms:
+    cloudlog.warning("CarList param already current, nothing to write")
+    return
+
+  params.put("CarList", platforms)
+  cloudlog.warning("CarList param refreshed from vehicle catalog")
+
+
+if __name__ == "__main__":
+  refresh_car_list_param()

iqpilot/selfdrive/car/tests/test_longitudinal_mode_persistence.py

@@ -0,0 +1,38 @@
+from cereal import custom
+from iqdbc.car import structs
+
+from openpilot.iqpilot.selfdrive.car.interfaces import _cleanup_unsupported_params
+
+
+class DummyParams:
+  def __init__(self):
+    self.removed: list[str] = []
+    self.values: dict[str, object] = {}
+
+  def remove(self, key: str) -> None:
+    self.removed.append(key)
+
+  def get_bool(self, key: str) -> bool:
+    return bool(self.values.get(key, False))
+
+  def get(self, key: str, return_default: bool = False):
+    return self.values.get(key)
+
+  def put(self, key: str, value) -> None:
+    self.values[key] = value
+
+
+class TestLongitudinalModePersistence:
+  def test_iq_dynamic_mode_is_not_removed_when_openpilot_long_is_unavailable(self):
+    params = DummyParams()
+    cp = structs.CarParams()
+    cp.openpilotLongitudinalControl = False
+    cp.steerControlType = structs.CarParams.SteerControlType.torque
+
+    cp_iq = custom.IQCarParams()
+    cp_iq.pcmCruiseSpeed = True
+
+    _cleanup_unsupported_params(cp, cp_iq, params)
+
+    assert "IQDynamicMode" not in params.removed
+    assert "LongIncrementsEnabled" in params.removed

iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py

@@ -0,0 +1,121 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import car, custom
+from openpilot.common.constants import CV
+from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from openpilot.selfdrive.car.cruise import VCruiseHelper
+
+
+class TestSpeedLimitSetSpeedMirror:
+  def setup_method(self):
+    self.CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+    self.CP_IQ = custom.IQCarParams(pcmCruiseSpeed=True)
+    self.v_cruise_helper = VCruiseHelper(self.CP, self.CP_IQ)
+    self.v_cruise_helper.set_speed_to_limit = True
+
+  @staticmethod
+  def _iq_plan(limit_mps: float, state) -> SimpleNamespace:
+    resolver = SimpleNamespace(
+      speedLimitValid=limit_mps > 0,
+      speedLimitLastValid=limit_mps > 0,
+      speedLimitFinalLast=limit_mps,
+    )
+    assist = SimpleNamespace(state=state)
+    return SimpleNamespace(speedLimit=SimpleNamespace(resolver=resolver, assist=assist))
+
+  def test_op_long_mirrors_active_speed_limit_target_into_cluster_speed(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.active))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_syncs_to_new_limit_even_when_assist_not_active(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_allows_manual_set_speed_changes_between_limit_changes(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    # First cycle after a valid limit appears will sync to the resolved target.
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    # On later cycles with the same limit, manual set speed changes should be preserved.
+    CS = car.CarState(cruiseState={"available": True, "speed": 15.64, "speedCluster": 15.64})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(15.64 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(15.64 * CV.MS_TO_KPH, abs=0.1)
+
+  def test_op_long_resyncs_when_limit_changes(self):
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(17.88, custom.IQPlan.SpeedLimit.AssistState.inactive))
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    CS = car.CarState(cruiseState={"available": True, "speed": 15.64, "speedCluster": 15.64})
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    self.v_cruise_helper.update_speed_limit_assist(False, self._iq_plan(13.41, custom.IQPlan.SpeedLimit.AssistState.inactive))
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1)
+    assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1)
+
+
+def test_set_speed_does_not_follow_limit_when_feature_off():
+  # Default off: set speed must stay the driver's value (limiter-only via planner min-blend).
+  CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+  CP_IQ = custom.IQCarParams(pcmCruiseSpeed=True)
+  helper = VCruiseHelper(CP, CP_IQ)
+  helper.set_speed_to_limit = False
+  helper.update_speed_limit_assist(False, TestSpeedLimitSetSpeedMirror._iq_plan(
+    17.88, custom.IQPlan.SpeedLimit.AssistState.active))
+
+  CS = car.CarState(cruiseState={"available": True, "speed": 22.35, "speedCluster": 22.35})
+  helper.update_v_cruise(CS, enabled=True, is_metric=False)
+
+  # Set speed tracks the car's cruise speed, NOT the 17.88 m/s limit.
+  assert helper.v_cruise_kph == pytest.approx(22.35 * CV.MS_TO_KPH, abs=0.1)
+
+
+def test_enhanced_stock_longitudinal_control_syncs_once_then_follows_cluster_speed():
+  CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True)
+  resolver = SimpleNamespace(speedLimitFinalLast=17.88)
+  assist = SimpleNamespace(enabled=True)
+  iq_plan = SimpleNamespace(speedLimit=SimpleNamespace(resolver=resolver, assist=assist))
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=100.0, previous_sync_limit_kph=None, pending_sync_limit_kph=None
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=22.0, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=17.88 * CV.MS_TO_KPH, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert sync_limit == pytest.approx(17.88 * CV.MS_TO_KPH, abs=0.1)
+  assert pending_limit is None
+
+  params, sync_limit, pending_limit = build_iq_control_params_from_plan(
+    CP, iq_plan, True, current_set_speed_kph=22.0, previous_sync_limit_kph=sync_limit, pending_sync_limit_kph=pending_limit
+  )
+  assert float(params[0]["value"].decode("utf-8")) == pytest.approx(22.0, abs=0.1)

iqpilot/selfdrive/car/vehicle_catalog.py

@@ -0,0 +1,85 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import json
+import os
+
+from openpilot.common.basedir import BASEDIR
+
+SCHEMA = "iqlvbs/supported-vehicles"
+REV = 1
+
+CATALOG_FILENAME = "vehicle_catalog.json"
+_CANDIDATE_PARTS = (
+  ("iqpilot", "selfdrive", "car", CATALOG_FILENAME),
+)
+
+# in-memory (car-interface) field  ->  on-disk compact key
+_ATTR_TO_KEY = (
+  ("platform", "id"),
+  ("make", "mk"),
+  ("brand", "grp"),
+  ("model", "mdl"),
+  ("year", "yrs"),
+  ("package", "req"),
+)
+
+
+def _reference(platform: str, years: list[str], claimed: set[str]) -> str:
+  span = f"{years[0]}-{years[-1]}" if len(years) > 1 else (years[0] if years else "na")
+  stem = f"{platform}|{span}"
+  ref, bump = stem, 2
+  while ref in claimed:
+    ref = f"{stem}#{bump}"
+    bump += 1
+  claimed.add(ref)
+  return ref
+
+
+def encode(vehicles: dict[str, dict]) -> dict:
+  records: dict[str, dict] = {}
+  claimed: set[str] = set()
+  for label, attrs in vehicles.items():
+    years = list(attrs.get("year") or [])
+    ref = _reference(attrs.get("platform", ""), years, claimed)
+    record = {"label": label}
+    for attr, key in _ATTR_TO_KEY:
+      record[key] = attrs.get(attr)
+    records[ref] = record
+  return {"catalog": SCHEMA, "rev": REV, "vehicles": records}
+
+
+def decode(envelope: dict) -> dict[str, dict]:
+  vehicles: dict[str, dict] = {}
+  for record in (envelope.get("vehicles") or {}).values():
+    attrs = {attr: record.get(key) for attr, key in _ATTR_TO_KEY}
+    vehicles[record.get("label", "")] = attrs
+  return vehicles
+
+
+def catalog_path(basedir: str = BASEDIR) -> str | None:
+  for parts in _CANDIDATE_PARTS:
+    candidate = os.path.join(basedir, *parts)
+    if os.path.isfile(candidate):
+      return candidate
+  return None
+
+
+def load_catalog(basedir: str = BASEDIR) -> dict[str, dict]:
+  path = catalog_path(basedir)
+  if path is None:
+    return {}
+  with open(path) as handle:
+    return decode(json.load(handle))
+
+
+def _write(vehicles: dict[str, dict], basedir: str = BASEDIR) -> str:
+  out = os.path.join(basedir, "iqpilot", "selfdrive", "car", CATALOG_FILENAME)
+  with open(out, "w") as handle:
+    json.dump(encode(vehicles), handle, indent=2, ensure_ascii=False)
+  return out
+
+
+if __name__ == "__main__":
+  from iqdbc.lvbs.car.car_catalog import build_car_catalog
+  print("wrote", _write(build_car_catalog()))

iqpilot/selfdrive/constructiond.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+constructiond: work-zone detector for Speed Limit Assist.
+
+Samples the road camera at ~2 Hz and looks for work-zone orange (barrels, drums,
+diamond signs) in the NV12 chroma plane. Sunlit yellow lane paint renders with
+nearly identical hue to barrel orange on this camera, but its red chroma (V)
+saturates below ~168 while retroreflective barrel orange reaches 170-190, so the
+V floor is the load-bearing threshold — do not lower it without re-running the
+paint/barrel separation sweep on real footage.
+"""
+from collections import deque
+
+import time
+
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal import custom
+from msgq.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.common.swaglog import cloudlog
+
+State = custom.IQConstructionZone.State
+
+ANALYSIS_PERIOD = 0.5
+
+# fractions of the chroma plane; excludes sky and hood
+ROI_TOP, ROI_BOTTOM = 0.40, 0.94
+ROI_LEFT, ROI_RIGHT = 0.04, 0.96
+
+V_MIN = 170     # paint tops out ~168 in every DAYLIGHT condition sampled
+CB_MIN = 16
+HUE_LO, HUE_HI = 0.65, 2.0  # (V-128)/(128-U): yellow paint ~0.4, barrel orange ~0.7-1.5, red ~3.0
+
+# Daylight-only gate: retroreflective amber markers (guardrail chevrons, object
+# markers) blaze past V_MIN under headlights at night — 137px on one chevron on
+# real footage — and there is no night work-zone ground truth to tune against.
+# Night ROI mean luma measured ~36-38, validated daytime footage 84-98.
+LUMA_MIN = 65.0
+
+# hits only accumulate at highway-approach speeds: brightly lit lots (truck
+# stops) can pass the luma gate at night with orange signage, and the clamp is
+# meaningless below it anyway. An already-active zone still holds while slowed.
+MIN_ENTER_SPEED = 13.4  # m/s (~30 mph)
+
+HIT_FRAC = 3.0e-4
+WASH_FRAC = 0.10  # more orange than this is scene lighting (sunset), not objects
+ENTER_HITS = 3
+ENTER_WINDOW = 10  # analyses (~5 s)
+HOLD_SEC = 120.0   # barrel-free stretches inside a zone last minutes; hold through them
+
+
+def orange_fraction(buf) -> tuple[float, float]:
+  """Returns (hot-orange fraction, mean luma) of the road ROI."""
+  h, w, stride, uv_off = buf.height, buf.width, buf.stride, buf.uv_offset
+  ch, cw = h // 2, w // 2
+  uv = buf.data[uv_off:uv_off + ch * stride].reshape(ch, stride)
+
+  r0, r1 = int(ROI_TOP * ch), int(ROI_BOTTOM * ch)
+  c0, c1 = int(ROI_LEFT * cw), int(ROI_RIGHT * cw)
+  step = 2 if cw > 700 else 1
+
+  u = uv[r0:r1:step, 2 * c0:2 * c1:2 * step].astype(np.int16)
+  v = uv[r0:r1:step, 2 * c0 + 1:2 * c1:2 * step].astype(np.int16)
+
+  cb = 128 - u
+  cr = v - 128
+  mask = (v >= V_MIN) & (cb >= CB_MIN) & (cr >= HUE_LO * cb) & (cr <= HUE_HI * cb)
+
+  y_plane = buf.data[:h * stride].reshape(h, stride)
+  luma = float(y_plane[2 * r0:2 * r1:4, 2 * c0:2 * c1:8].mean())
+
+  return float(mask.mean()), luma
+
+
+class ConstructionZoneDetector:
+  def __init__(self):
+    self.recent = deque(maxlen=ENTER_WINDOW)
+    self.last_hit_t: float | None = None
+    self.active = False
+    self.frac = 0.0
+
+  @property
+  def state(self):
+    if self.active:
+      return State.active
+    if any(self.recent):
+      return State.pending
+    return State.inactive
+
+  def seconds_since_hit(self, now: float) -> float:
+    if self.last_hit_t is None:
+      return -1.0
+    return now - self.last_hit_t
+
+  def update(self, frac: float, now: float, luma: float = 255.0, v_ego: float = 255.0) -> bool:
+    self.frac = frac
+    hit = (HIT_FRAC <= frac <= WASH_FRAC) and luma >= LUMA_MIN and v_ego >= MIN_ENTER_SPEED
+    self.recent.append(hit)
+    if hit:
+      self.last_hit_t = now
+
+    if self.active:
+      if self.last_hit_t is None or (now - self.last_hit_t) > HOLD_SEC:
+        self.active = False
+        self.recent.clear()
+    elif sum(self.recent) >= ENTER_HITS:
+      self.active = True
+    return self.active
+
+
+def main():
+  pm = messaging.PubMaster(["iqConstructionZone"])
+  car_state_sock = messaging.sub_sock("carState", conflate=True)
+  detector = ConstructionZoneDetector()
+  v_ego = 0.0
+
+  vipc = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True)
+  while not vipc.connect(False):
+    time.sleep(0.5)
+  cloudlog.info("constructiond: connected to road camera stream")
+
+  while True:
+    t0 = time.monotonic()
+    buf = vipc.recv(200)
+    if buf is None:
+      # no publish on camera stall: SLC sees us stale and releases the clamp
+      continue
+
+    cs = messaging.recv_one_or_none(car_state_sock)
+    if cs is not None:
+      v_ego = cs.carState.vEgo
+
+    frac, luma = orange_fraction(buf)
+    detector.update(frac, t0, luma, v_ego)
+
+    msg = messaging.new_message("iqConstructionZone")
+    msg.valid = True
+    cz = msg.iqConstructionZone
+    cz.state = detector.state
+    cz.active = detector.active
+    cz.orangeFraction = frac
+    cz.secondsSinceHit = detector.seconds_since_hit(t0)
+    pm.send("iqConstructionZone", msg)
+
+    time.sleep(max(0.0, ANALYSIS_PERIOD - (time.monotonic() - t0)))
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/iq_controls_layer.py

@@ -0,0 +1,120 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+IQ.Pilot's controls-side extension layer. Controls mixes this in to gain the extra
+sub/pub services, the IQ car-control message (radar blend, SLC set-speed sync, AOL
+guidance continuity) and the lateral-engage gate, without touching stock controlsd.
+"""
+import time
+
+import cereal.messaging as messaging
+from cereal import log, custom
+
+from iqdbc.car import structs
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.iqpilot.common.steer_delay import resolve_steer_delay
+from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from openpilot.iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
+from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.radar_manager import RadarManager
+
+_PARAM_REFRESH_S = 3.0
+_LEAD_FIELDS = ("dRel", "yRel", "vRel", "aRel", "vLead", "dPath", "vLat", "vLeadK",
+                "aLeadK", "fcw", "status", "aLeadTau", "modelProb", "radar", "radarTrackId")
+
+
+class IQControlsLayer(InferenceStateBase):
+  def __init__(self, CP: structs.CarParams, params: Params):
+    InferenceStateBase.__init__(self)
+    self.CP = CP
+    self.params = params
+    self.blinker_pause_lateral = IQSignalPauseController()
+
+    cloudlog.info("IQ controls layer waiting for IQCarParams")
+    self.CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
+    cloudlog.info("IQ controls layer got IQCarParams")
+
+    self.iq_sub_services = ['radarState', 'iqState', 'iqPlan', 'iqNavState']
+    self.iq_pub_services = ['iqCarControl']
+    self.radar_manager = RadarManager(CP, params)
+
+    self._next_param_refresh = 0.0
+    self._needs_iq_lead_data = CP.brand == "hyundai"
+    self._maneuver_mode = params.get_bool("LateralManeuverMode")
+    self._sync_set_speed = self._want_set_speed_to_limit()
+    self._slc_limit_kph = None
+    self._slc_limit_pending_kph = None
+
+  def _want_set_speed_to_limit(self) -> bool:
+    try:
+      return self.params.get_bool("SLCSetSpeedToLimit")
+    except Exception:
+      return False
+
+  # --- periodic param refresh (throttled to a few Hz) --------------------------
+  def refresh_iq_params(self, sm: messaging.SubMaster) -> None:
+    now = time.monotonic()
+    if now - self._next_param_refresh <= _PARAM_REFRESH_S:
+      return
+    self.blinker_pause_lateral.get_params()
+    if self.CP.lateralTuning.which() == 'torque':
+      self.lat_delay = resolve_steer_delay(self.params, sm["liveDelay"].lateralDelay)
+    self._sync_set_speed = self._want_set_speed_to_limit()
+    self.radar_manager.read_params()
+    self._next_param_refresh = now
+
+  # --- lateral engage gate -----------------------------------------------------
+  def iq_lateral_allowed(self, sm: messaging.SubMaster) -> bool:
+    if self.blinker_pause_lateral.update(sm['carState']):
+      return False
+
+    aol = sm['iqState'].aol
+    stock_active = bool(sm['selfdriveState'].active)
+    if self._maneuver_mode:
+      return stock_active or bool(aol.available and aol.active)
+    if aol.available:
+      return bool(aol.active)
+    return stock_active
+
+  @staticmethod
+  def _lead_snapshot(ld: log.RadarState.LeadData) -> dict:
+    return {field: getattr(ld, field) for field in _LEAD_FIELDS}
+
+  # --- build + publish the IQ car-control message ------------------------------
+  def _compose_iq_carcontrol(self, sm: messaging.SubMaster) -> custom.IQCarControl:
+    CC_IQ = custom.IQCarControl.new_message()
+    lp = sm['liveParameters']
+    CC_IQ.angleOffsetDeg = float(getattr(lp, 'angleOffsetDeg', 0.0))
+    CC_IQ.aol = sm['iqState'].aol
+
+    if self._needs_iq_lead_data:
+      CC_IQ.leadOne = self._lead_snapshot(sm['radarState'].leadOne)
+      CC_IQ.leadTwo = self._lead_snapshot(sm['radarState'].leadTwo)
+
+    if self.CP.openpilotLongitudinalControl:
+      cruise = getattr(sm['carState'], 'cruiseState', None)
+      set_speed_ms = float(max(getattr(cruise, 'speedCluster', 0.0), getattr(cruise, 'speed', 0.0), 0.0))
+      set_speed_kph = set_speed_ms * CV.MS_TO_KPH
+      if self._sync_set_speed:
+        CC_IQ.params, self._slc_limit_kph, self._slc_limit_pending_kph = build_iq_control_params_from_plan(
+          self.CP, sm['iqPlan'], bool(sm['selfdriveState'].enabled), set_speed_kph,
+          self._slc_limit_kph, self._slc_limit_pending_kph)
+      else:
+        self._slc_limit_kph = self._slc_limit_pending_kph = None
+      self.radar_manager.update(CC_IQ, sm, set_speed_kph)
+    return CC_IQ
+
+  @staticmethod
+  def _emit(CC_IQ: custom.IQCarControl, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    envelope = messaging.new_message('iqCarControl')
+    envelope.valid = sm['carState'].canValid
+    envelope.iqCarControl = CC_IQ
+    pm.send('iqCarControl', envelope)
+
+  def publish_iq_state(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    fresh = sm.updated['iqState'] or sm.updated['iqPlan'] or (self._needs_iq_lead_data and sm.updated['radarState'])
+    if not fresh:
+      return
+    self._emit(self._compose_iq_carcontrol(sm), sm, pm)

iqpilot/selfdrive/controls/lib/__init__.py

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

iqpilot/selfdrive/controls/lib/custom_stop_distance.py

@@ -0,0 +1,111 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb), ported to
+IQ.Pilot and made bidirectional.
+
+Custom Stop Distance: nudge how far back IQ.Pilot stops behind a stopped lead vehicle or a
+model-held stop (red light). Independent of IQ Force Stops -- works whether Force Stops is on
+or off.
+
+IQCustomStopDistance (meters, -2..2): positive stops further back, negative settles in closer.
+0 is stock.
+
+Two mechanisms share the param:
+
+- Lead stops (radard): the reported lead distance is nudged by the offset, faded back out as the
+  lead gets up to speed so normal following distance is unaffected. Works in chill and end-to-end.
+
+- Model-held stops (planner, end-to-end mode): when the model's trajectory ends at ~zero velocity
+  (it plans to remain stopped, e.g. a red light), a positive offset brakes toward a point short of
+  its predicted stop and holds there instead of creeping forward -- it only ever adds braking on
+  top of the model's own plan, never relaxes below it. A negative offset is a no-op here: there's
+  no safe way to coax the car past the model's own conservative stop point this way.
+"""
+import numpy as np
+
+from iqdbc.car.interfaces import ACCEL_MIN
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+CUSTOM_STOP_DISTANCE_PARAM = "IQCustomStopDistance"
+MIN_DISTANCE_M = -2
+MAX_DISTANCE_M = 2
+
+# Fade the offset back out as the lead gets up to speed
+STOPPED_DISTANCE_FADE_BP = [0., 3.]  # m/s, lead speed
+MIN_ADJUSTED_D_REL = 1.0  # m
+
+E2E_STOP_PLAN_VEL_THRESHOLD = 1.0  # m/s, model plan ending below this implies a held stop
+E2E_STOP_MIN_BRAKING = -0.1  # m/s^2, only deepen braking the model has already started
+E2E_STOP_MIN_DIST = 2.0  # m, never target a stop point closer than this
+E2E_STOP_HOLD_MAX_V = 0.5  # m/s, below this the car is considered stopped
+E2E_STOP_HOLD_BUFFER = 2.0  # m, hold until the model's stop point moves beyond offset + buffer
+
+
+def get_sanitize_int_param(key, min_val, max_val, params):
+  stored = params.get(key, return_default=True)
+  bounded = min(max(stored, min_val), max_val)
+  if bounded != stored:
+    params.put(key, bounded)
+  return bounded
+
+
+class CustomStopDistance:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.distance = 0.
+    self.read_params()
+
+  def read_params(self) -> None:
+    self.distance = float(get_sanitize_int_param(CUSTOM_STOP_DISTANCE_PARAM, MIN_DISTANCE_M, MAX_DISTANCE_M, self.params))
+
+  def update(self) -> None:
+    if self.frame % int(3 / DT_MDL) == 0:
+      self.read_params()
+    self.frame += 1
+
+  def apply_lead(self, lead_dict: dict) -> dict:
+    if self.distance == 0. or not lead_dict.get('status', False):
+      return lead_dict
+
+    offset = self.distance * float(np.interp(lead_dict['vLead'], STOPPED_DISTANCE_FADE_BP, [1., 0.]))
+    adjusted = lead_dict['dRel'] - offset
+    if self.distance > 0:
+      # stop further back: never reduce the reported distance below the floor, and never report further than reality
+      lead_dict['dRel'] = min(lead_dict['dRel'], max(adjusted, MIN_ADJUSTED_D_REL))
+    else:
+      # stop closer in: never report closer than reality
+      lead_dict['dRel'] = max(lead_dict['dRel'], adjusted)
+    return lead_dict
+
+  def adjust_e2e_stop(self, a_target: float, should_stop: bool, v_ego: float, model_msg) -> tuple[float, bool]:
+    if self.distance <= 0.:
+      return a_target, should_stop
+
+    x = model_msg.position.x
+    v = model_msg.velocity.x
+    if len(x) != ModelConstants.IDX_N or len(v) != ModelConstants.IDX_N:
+      return a_target, should_stop
+
+    # only stops the model plans to hold (red lights) can be shifted -- stop signs are left alone:
+    # forcing an early stop makes the model treat the stop as completed and roll through the sign
+    if float(v[-1]) > E2E_STOP_PLAN_VEL_THRESHOLD:
+      return a_target, should_stop
+
+    stop_distance = float(x[-1])
+
+    if v_ego < E2E_STOP_HOLD_MAX_V:
+      # stopped short of the model's stop point: hold instead of creeping up to it
+      if stop_distance <= self.distance + E2E_STOP_HOLD_BUFFER:
+        should_stop = True
+    elif a_target < E2E_STOP_MIN_BRAKING:
+      # deepen braking that has already started, targeting a stop short of the model's stop point
+      adjusted_distance = max(stop_distance - self.distance, E2E_STOP_MIN_DIST)
+      a_required = max(-(v_ego ** 2) / (2 * adjusted_distance), ACCEL_MIN)
+      if a_required < a_target:
+        a_target = float(a_required)
+
+    return a_target, should_stop

iqpilot/selfdrive/controls/lib/helpers/__init__.py

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

iqpilot/selfdrive/controls/lib/helpers/blinker_pause.py

@@ -0,0 +1,82 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from cereal import car
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+
+class SignalPauseEngine:
+  _KEY_ON = "IQBlinkerPauseLateral"
+  _KEY_UNIT = "IsMetric"
+  _KEY_GATE = "IQBlinkerMinLateralSpeed"
+
+  def __init__(self):
+    self._kv = Params()
+    self._state = {"on": False, "metric": False, "gate": 0.0}
+    self.reload_setup()
+
+  @staticmethod
+  def _one_signal(cs: car.CarState) -> bool:
+    return bool(cs.leftBlinker) ^ bool(cs.rightBlinker)
+
+  @staticmethod
+  def _as_float(raw) -> float:
+    try:
+      return float(raw) if raw is not None else 0.0
+    except (TypeError, ValueError):
+      return 0.0
+
+  def _pull_setup(self) -> None:
+    self._state["on"] = self._kv.get_bool(self._KEY_ON)
+    self._state["metric"] = self._kv.get_bool(self._KEY_UNIT)
+    self._state["gate"] = self._as_float(self._kv.get(self._KEY_GATE))
+
+  def _gate_mps(self) -> float:
+    factor = CV.KPH_TO_MS if self._state["metric"] else CV.MPH_TO_MS
+    return self._state["gate"] * factor
+
+  def reload_setup(self) -> None:
+    self._pull_setup()
+
+  def heartbeat(self) -> None:
+    self._pull_setup()
+
+  def is_paused(self, cs: car.CarState) -> bool:
+    return bool(self._state["on"] and self._one_signal(cs) and cs.vEgo < self._gate_mps())
+
+  @property
+  def enabled(self):
+    return self._state["on"]
+
+  @enabled.setter
+  def enabled(self, value):
+    self._state["on"] = bool(value)
+
+  @property
+  def is_metric(self):
+    return self._state["metric"]
+
+  @is_metric.setter
+  def is_metric(self, value):
+    self._state["metric"] = bool(value)
+
+  @property
+  def min_speed(self):
+    return self._state["gate"]
+
+  @min_speed.setter
+  def min_speed(self, value):
+    self._state["gate"] = float(value)
+
+
+class IQSignalPauseController(SignalPauseEngine):
+  def __init__(self):
+    super().__init__()
+
+  def get_params(self) -> None:
+    self.reload_setup()
+
+  def update(self, cs: car.CarState) -> bool:
+    return self.is_paused(cs)

iqpilot/selfdrive/controls/lib/helpers/curvature.py

@@ -0,0 +1,48 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from numpy import clip, interp
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MAX_LATERAL_JERK, MIN_SPEED
+
+
+def _sanitize_plan(headings, curvatures):
+  valid_shape = len(headings) == CONTROL_N and len(curvatures) >= CONTROL_N
+  if valid_shape:
+    return headings, curvatures
+  placeholder = [0.0] * CONTROL_N
+  return placeholder, placeholder
+
+
+def _project_future_heading(delay_s: float, headings) -> float:
+  return float(interp(delay_s, ModelConstants.T_IDXS[:CONTROL_N], headings))
+
+
+def _convert_heading_to_curvature(projected_heading: float, speed_mps: float, current_curvature: float, delay_s: float) -> float:
+  turning_arc = projected_heading / (speed_mps * delay_s)
+  return (2.0 * turning_arc) - current_curvature
+
+
+def _limit_curvature_rate(target_curvature: float, current_curvature: float, speed_mps: float) -> float:
+  curvature_step = MAX_LATERAL_JERK / (speed_mps ** 2)
+  lower = current_curvature - (curvature_step * DT_MDL)
+  upper = current_curvature + (curvature_step * DT_MDL)
+  return float(clip(target_curvature, lower, upper))
+
+
+def solve_lag_curvature(steer_delay, v_ego, psis, curvatures):
+  headings, curvature_track = _sanitize_plan(psis, curvatures)
+  speed_mps = max(MIN_SPEED, v_ego)
+  delay_s = max(float(steer_delay), 1e-3)
+  current_curvature = float(curvature_track[0])
+  projected_heading = _project_future_heading(delay_s, headings)
+  target_curvature = _convert_heading_to_curvature(projected_heading, speed_mps, current_curvature, delay_s)
+  return _limit_curvature_rate(target_curvature, current_curvature, speed_mps)
+
+
+get_lag_adjusted_curvature = solve_lag_curvature

iqpilot/selfdrive/controls/lib/helpers/e2e_alerts.py

@@ -0,0 +1,143 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from cereal import messaging, custom
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+
+PARAM_PATH = "EndToEndAlert"
+PARAM_LEAD = "EndToEndLeadAlert"
+PARAM_STRIDE_S = 2.0
+
+SETTLE_S = 1.0
+CONFIRM_S = 0.4
+ROLL_MPS = 0.3
+
+HORIZON_TAIL = 5
+PATH_SPEED_MPS = 3.0
+
+LEAD_QUEUE_M = 12.0
+LEAD_SPEED_MPS = 1.0
+LEAD_GAP_M = 0.5
+
+
+class _Confirm:
+  def __init__(self, window_s: float):
+    self._window = window_s
+    self._held = 0.0
+    self._spent = False
+
+  def clear(self) -> None:
+    self._held = 0.0
+    self._spent = False
+
+  def poll(self, holds: bool) -> bool:
+    if self._spent:
+      return False
+    self._held = self._held + DT_MDL if holds else 0.0
+    if self._held < self._window:
+      return False
+    self._spent = True
+    return True
+
+
+class _Dwell:
+  def __init__(self):
+    self.seconds = 0.0
+    self.lead_floor = float('inf')
+
+  def clear(self) -> None:
+    self.seconds = 0.0
+    self.lead_floor = float('inf')
+
+  def tick(self, lead_range: float | None) -> None:
+    self.seconds += DT_MDL
+    if lead_range is not None:
+      self.lead_floor = min(self.lead_floor, lead_range)
+
+  @property
+  def settled(self) -> bool:
+    return self.seconds >= SETTLE_S
+
+  @property
+  def queued(self) -> bool:
+    return self.lead_floor < LEAD_QUEUE_M
+
+
+class EndToEndAlertEngine:
+  def __init__(self):
+    self._params = Params()
+    self._on = {"path": False, "lead": False}
+    self._elapsed_since_read = PARAM_STRIDE_S
+    self._dwell = _Dwell()
+    self._confirm = {"path": _Confirm(CONFIRM_S), "lead": _Confirm(CONFIRM_S)}
+    self._fired = {"path": False, "lead": False}
+
+  def _refresh_params(self) -> None:
+    self._elapsed_since_read += DT_MDL
+    if self._elapsed_since_read < PARAM_STRIDE_S:
+      return
+    self._elapsed_since_read = 0.0
+    self._on["path"] = self._params.get_bool(PARAM_PATH)
+    self._on["lead"] = self._params.get_bool(PARAM_LEAD)
+
+  @staticmethod
+  def _car_holds_long(sm: messaging.SubMaster) -> bool:
+    # AOL steers without raising selfdriveState.enabled, so the pair reads as long authority
+    return bool(sm['selfdriveState'].enabled or sm['carState'].cruiseState.enabled)
+
+  @staticmethod
+  def _halted(cs) -> bool:
+    return bool(cs.standstill) or abs(cs.vEgo) < ROLL_MPS
+
+  @staticmethod
+  def _horizon_speed(model) -> float:
+    # capnp list readers reject slices
+    samples = model.velocity.x
+    count = len(samples)
+    if count < HORIZON_TAIL:
+      return 0.0
+    return sum(samples[i] for i in range(count - HORIZON_TAIL, count)) / HORIZON_TAIL
+
+  def _rearm(self) -> None:
+    self._dwell.clear()
+    for gate in self._confirm.values():
+      gate.clear()
+
+  def update(self, sm: messaging.SubMaster, iq_events: IQEvents) -> None:
+    self._refresh_params()
+    self._fired["path"] = self._fired["lead"] = False
+
+    cs = sm['carState']
+    lead = sm['radarState'].leadOne
+    lead_range = float(lead.dRel) if lead.status else None
+
+    if not self._halted(cs) or cs.gasPressed or self._car_holds_long(sm):
+      self._rearm()
+      return
+
+    self._dwell.tick(lead_range)
+    if not self._dwell.settled:
+      return
+
+    if self._on["path"] and lead_range is None:
+      opened = self._horizon_speed(sm['modelV2']) > PATH_SPEED_MPS
+      self._fired["path"] = self._confirm["path"].poll(opened)
+
+    if self._on["lead"] and lead_range is not None and self._dwell.queued:
+      pulling = lead.vLead > LEAD_SPEED_MPS and (lead_range - self._dwell.lead_floor) > LEAD_GAP_M
+      self._fired["lead"] = self._confirm["lead"].poll(pulling)
+
+    if self._fired["path"] or self._fired["lead"]:
+      iq_events.add(custom.IQOnroadEvent.EventName.e2eChime)
+
+  @property
+  def path_alert(self) -> bool:
+    return self._fired["path"]
+
+  @property
+  def lead_alert(self) -> bool:
+    return self._fired["lead"]

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 cereal import custom, log
+
+from openpilot.common.params import Params
+from openpilot.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

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 cereal import custom
+
+from openpilot.common.constants import CV
+from openpilot.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

iqpilot/selfdrive/controls/lib/helpers/nav_torque_pulse.py

@@ -0,0 +1,83 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Short, decaying steering-torque nudges that lean the car through navigation
+turns and highway exits. This is a lateral-control add-on driven by iqNavState;
+it is independent of the feed-forward model and is off by default.
+"""
+import numpy as np
+import cereal.messaging as messaging
+from cereal import custom
+
+TURN_NUDGE_TORQUE = 0.8
+EXIT_NUDGE_TORQUE = 0.6
+TURN_PULSE_FRAMES = 50
+EXIT_PULSE_FRAMES = 75
+
+# Master switch — nav torque influence is experimental and shipped off.
+IQP_NAV_TORQUE_INFLUENCE_ENABLED = False
+
+_LEFT = 1   # turnDesireDirection / lanePositioningDirection: 1 == left
+
+
+class NavTorquePulseBrain:
+  def __init__(self, lac_torque):
+    self._controller = lac_torque
+    self._nav_sm = messaging.SubMaster(["iqNavState"], poll="iqNavState")
+    self._nav_key = ""
+    self._nav_pulse_sign = 0.0
+    self._nav_pulse_frames = 0
+
+  def _lookup_nav_pulse(self):
+    if not IQP_NAV_TORQUE_INFLUENCE_ENABLED:
+      return "", 0.0, 0
+
+    self._nav_sm.update(0)
+    nav_state = self._nav_sm["iqNavState"]
+    phase = getattr(nav_state, "maneuverPhase", custom.IQNavState.ManeuverPhase.none)
+    maneuver_direction = getattr(nav_state, "maneuverDirection", custom.NavDirection.none)
+
+    # left nudges negative, otherwise positive
+    def turn(tag, direction):
+      return f"turn{tag}:{direction}", -TURN_NUDGE_TORQUE if direction == _LEFT else TURN_NUDGE_TORQUE, TURN_PULSE_FRAMES
+
+    def keep(tag, direction):
+      return f"{tag}:{direction}", -EXIT_NUDGE_TORQUE if direction == _LEFT else EXIT_NUDGE_TORQUE, EXIT_PULSE_FRAMES
+
+    if phase == custom.IQNavState.ManeuverPhase.turnActive:
+      return turn("-phase", getattr(nav_state, "turnDesireDirection", 0))
+    if phase == custom.IQNavState.ManeuverPhase.highwayCommit and maneuver_direction in (custom.NavDirection.left, custom.NavDirection.right):
+      return keep("highway-phase", getattr(nav_state, "lanePositioningDirection", 0))
+    if getattr(nav_state, "shouldSendTurnDesire", False):
+      return turn("", getattr(nav_state, "turnDesireDirection", 0))
+    if getattr(nav_state, "shouldSendLanePositioning", False):
+      return keep("keep", getattr(nav_state, "lanePositioningDirection", 0))
+    return "", 0.0, 0
+
+  def nudge_output_torque(self, active: bool, car_state, output_torque: float) -> float:
+    if not IQP_NAV_TORQUE_INFLUENCE_ENABLED:
+      self._nav_pulse_frames = 0
+      self._nav_key = ""
+      return output_torque
+
+    nav_key, pulse_sign, pulse_frames = self._lookup_nav_pulse()
+
+    if not active or getattr(car_state, "steeringPressed", False):
+      self._nav_pulse_frames = 0
+      if not nav_key:
+        self._nav_key = ""
+      return output_torque
+
+    if nav_key and nav_key != self._nav_key:
+      self._nav_key = nav_key
+      self._nav_pulse_sign = pulse_sign
+      self._nav_pulse_frames = pulse_frames
+    elif not nav_key and self._nav_pulse_frames == 0:
+      self._nav_key = ""
+
+    if self._nav_pulse_frames > 0:
+      self._nav_pulse_frames -= 1
+      steer_max = float(getattr(self._controller, "steer_max", 1.0))
+      output_torque = float(np.clip(output_torque + self._nav_pulse_sign, -steer_max, steer_max))
+
+    return output_torque

iqpilot/selfdrive/controls/lib/helpers/tests/__init__.py

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

iqpilot/selfdrive/controls/lib/helpers/tests/test_e2e_alerts.py

@@ -0,0 +1,133 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import pytest
+
+import cereal.messaging as messaging
+from cereal import custom
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+  EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
+
+E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime
+
+
+class _Events(list):
+  def add(self, name):
+    self.append(name)
+
+
+def _model(horizon):
+  msg = messaging.new_message('modelV2')
+  msg.modelV2.velocity.x = [0.0] * (33 - HORIZON_TAIL) + [horizon] * HORIZON_TAIL
+  return msg.as_reader().modelV2
+
+
+def _sm(*, v_ego=0.0, standstill=True, gas=False, enabled=False, cruise=False,
+        horizon=0.0, lead=None):
+  lead = lead or SimpleNamespace(status=False, dRel=0.0, vLead=0.0)
+  return {
+    'carState': SimpleNamespace(vEgo=v_ego, standstill=standstill, gasPressed=gas,
+                                cruiseState=SimpleNamespace(enabled=cruise)),
+    'selfdriveState': SimpleNamespace(enabled=enabled),
+    'radarState': SimpleNamespace(leadOne=lead),
+    'modelV2': _model(horizon),
+  }
+
+
+def _engine(path=True, lead=True):
+  engine = EndToEndAlertEngine()
+  engine._refresh_params = lambda: None
+  engine._on = {"path": path, "lead": lead}
+  return engine
+
+
+def _run(engine, sm, seconds):
+  events = _Events()
+  chimes = 0
+  for _ in range(int(seconds / DT_MDL)):
+    engine.update(sm, events)
+    chimes += events.count(E2E_CHIME)
+    events.clear()
+  return chimes
+
+
+def _lead(d_rel, v_lead=0.0):
+  return SimpleNamespace(status=True, dRel=d_rel, vLead=v_lead)
+
+
+def test_path_opens_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+
+
+def test_path_needs_the_settle_dwell():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+
+
+def test_path_silent_while_openpilot_long_is_engaged():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, enabled=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, enabled=True), 5.0) == 0
+
+
+def test_path_silent_while_stock_acc_holds_the_car():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, cruise=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, cruise=True), 5.0) == 0
+
+
+def test_path_chimes_under_aol():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+def test_path_ignores_a_visible_lead():
+  engine = _engine(lead=False)
+  _run(engine, _sm(horizon=0.0, lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, lead=_lead(6.0)), 5.0) == 0
+
+
+def test_lead_pullaway_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0) == 0
+  moving = _sm(lead=_lead(6.0 + LEAD_GAP_M + 0.5, LEAD_SPEED_MPS + 1.0))
+  assert _run(engine, moving, CONFIRM_S + 1.0) == 1
+  assert _run(engine, moving, 5.0) == 0
+
+
+def test_lead_creep_inside_the_gap_stays_silent():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(6.0 + LEAD_GAP_M / 2, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_lead_far_ahead_is_not_a_queue():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(40.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(44.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_gas_and_motion_rearm_the_dwell():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  _run(engine, _sm(v_ego=5.0, standstill=False, horizon=8.0), 2.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+@pytest.mark.parametrize("param_off", ["path", "lead"])
+def test_each_param_gates_only_its_own_trigger(param_off):
+  engine = _engine(path=param_off != "path", lead=param_off != "lead")
+  if param_off == "path":
+    _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+    assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+  else:
+    _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+    assert _run(engine, _sm(lead=_lead(8.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0

iqpilot/selfdrive/controls/lib/helpers/tests/test_nav_torque_pulse.py

@@ -0,0 +1,105 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import custom
+import openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+  NavTorquePulseBrain, TURN_PULSE_FRAMES, EXIT_PULSE_FRAMES)
+
+
+@pytest.fixture
+def influence_on():
+  nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = True
+  try:
+    yield
+  finally:
+    nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = False
+
+
+def _fixed_nav_sm(**fields):
+  nav = SimpleNamespace(
+    maneuverPhase=custom.IQNavState.ManeuverPhase.none,
+    maneuverDirection=custom.NavDirection.none,
+    shouldSendTurnDesire=False,
+    turnDesireDirection=0,
+    shouldSendLanePositioning=False,
+    lanePositioningDirection=0,
+  )
+  for k, v in fields.items():
+    setattr(nav, k, v)
+
+  class SM:
+    def update(self, _):
+      return None
+
+    def __getitem__(self, _):
+      return nav
+  return SM()
+
+
+def _brain(nav_sm=None, steer_max=1.0):
+  brain = NavTorquePulseBrain(SimpleNamespace(steer_max=steer_max))
+  if nav_sm is not None:
+    brain._nav_sm = nav_sm
+  return brain
+
+
+def test_passthrough_when_disabled():
+  brain = _brain()
+  cs = SimpleNamespace(steeringPressed=False)
+  assert brain.nudge_output_torque(True, cs, 0.42) == 0.42
+
+
+class TestPulseSign:
+  @pytest.mark.parametrize("direction,expect_negative", [(1, True), (2, False)])
+  def test_turn_desire_direction(self, influence_on, direction, expect_negative):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=direction))
+    cs = SimpleNamespace(steeringPressed=False)
+    first = brain.nudge_output_torque(True, cs, 0.0)
+    assert (first < 0.0) == expect_negative
+
+  def test_turn_active_phase_uses_turn_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(maneuverPhase=custom.IQNavState.ManeuverPhase.turnActive,
+                                 turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 2)]
+    assert outs[TURN_PULSE_FRAMES - 1] < 0.0
+    assert outs[TURN_PULSE_FRAMES] == 0.0
+
+
+class TestPulseLifecycle:
+  def test_pulse_expires_after_its_frame_count(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 3)]
+    assert all(o < 0.0 for o in outs[:TURN_PULSE_FRAMES])
+    assert all(o == 0.0 for o in outs[TURN_PULSE_FRAMES:])
+    assert all(np.isfinite(o) for o in outs)
+
+  def test_steering_press_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=True)
+    assert brain.nudge_output_torque(True, cs, 0.4) == 0.4
+
+  def test_inactive_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    assert brain.nudge_output_torque(False, cs, 0.4) == 0.4
+
+  def test_output_clamped_to_steer_max(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=2), steer_max=0.5)
+    cs = SimpleNamespace(steeringPressed=False)
+    out = brain.nudge_output_torque(True, cs, 0.4)  # 0.4 + 0.8 nudge, clamped to 0.5
+    assert out == pytest.approx(0.5)
+
+  def test_lane_positioning_uses_exit_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendLanePositioning=True, lanePositioningDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(EXIT_PULSE_FRAMES + 2)]
+    assert outs[EXIT_PULSE_FRAMES - 1] < 0.0
+    assert outs[EXIT_PULSE_FRAMES] == 0.0

iqpilot/selfdrive/controls/lib/iq_dynamic/engine.py

@@ -0,0 +1,249 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from cereal import messaging
+from numpy import interp
+from iqdbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
+  IQConstants,
+  IQFilterEngine,
+  IQModeEngine,
+  IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM,
+  IQ_DYNAMIC_MODE_PARAM,
+  IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM,
+  IQ_DYNAMIC_MODEL_STOP_TIME_PARAM,
+  IQ_FORCE_STOPS_PARAM,
+  compute_slowdown_need,
+)
+
+S_Y = 33
+
+
+class IQDynamicController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self.IQS = CP
+    self._mpc = mpc
+    self.IQParams = params or Params()
+
+    self.IQDynamicStatus = False
+    self.IQDynamicA = False
+    self.IQDynamicF = 0
+    self.IQDynamicU = 0.0
+    self.IQEngineManager = IQModeEngine()
+
+    self.IQFilterL = IQFilterEngine(measurement_noise=0.17, process_noise=0.04, process_decay=1.03, smoothing_floor=0.9)
+    self.IQFilterSDL = IQFilterEngine(measurement_noise=0.12, process_noise=0.098, process_decay=1.01, smoothing_floor=0.8)
+    self.IQFilterSFL = IQFilterEngine(measurement_noise=0.11, process_noise=0.06, process_decay=1.000, smoothing_floor=0.90)
+    self.IQFilterFCW = IQFilterEngine(measurement_noise=0.19, process_noise=0.11, process_decay=1.11, smoothing_floor=0.4)
+    self.IQFilterSlowLead = IQFilterEngine(measurement_noise=0.15, process_noise=0.08, process_decay=1.02, smoothing_floor=0.75)
+    self.IQFilterModelStop = IQFilterEngine(measurement_noise=0.15, process_noise=0.06, process_decay=1.01, smoothing_floor=0.7)
+
+    self.hasIQFilterLED = False
+    self.hasIQSDL = False
+    self.hasIQSFL = False
+    self.hasIQL = False
+    self.curve_detected = False
+    self.slow_lead_detected = False
+    self.stop_light_detected = False
+    self.low_speed_detected = False
+    self.low_speed_lead_detected = False
+    self.model_stopped = False
+    self.tracking_lead = False
+    self.force_stops_enabled = True
+    self.slc_experimental_mode = False
+
+    self.kph = 0.0
+    self.cruise_kph = 0.0
+    self.aeb = 0
+    self.aeb_c = 0
+    self.ss_c = 0
+    self.e_x = float('inf')
+    self.e_d = 0.0
+    self.model_length = 0.0
+    self.lead_speed = 0.0
+
+    self.conditional_curves = True
+    self.conditional_slower_lead = True
+    self.conditional_stopped_lead = True
+    self.conditional_model_stops = True
+    self.conditional_slc_fallback = True
+    self.conditional_speed = IQConstants.CONDITIONAL_SPEED_DEFAULT
+    self.conditional_lead_speed = IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT
+    self.model_stop_time = IQConstants.MODEL_STOP_TIME_DEFAULT
+    self.minimum_force_stop_length = IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT
+
+  def _read_bool(self, key: str, default: bool) -> bool:
+    value = self.IQParams.get_bool(key)
+    return default if value is None else bool(value)
+
+  def _read_float(self, key: str, default: float) -> float:
+    value = self.IQParams.get(key)
+    if value is None:
+      return default
+    if isinstance(value, bytes):
+      value = value.decode('utf-8')
+    try:
+      return float(value)
+    except (TypeError, ValueError):
+      return default
+
+  def _readIQParams(self) -> None:
+    if self.IQDynamicF % int(1. / DT_MDL) != 0:
+      return
+    self.IQDynamicStatus = self._read_bool(IQ_DYNAMIC_MODE_PARAM, False)
+    self.conditional_curves = self._read_bool(IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM, True)
+    self.conditional_slower_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM, True)
+    self.conditional_stopped_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM, True)
+    self.conditional_model_stops = self._read_bool(IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM, True)
+    self.conditional_slc_fallback = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM, True)
+    self.conditional_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM, IQConstants.CONDITIONAL_SPEED_DEFAULT)
+    self.conditional_lead_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM, IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT)
+    self.model_stop_time = self._read_float(IQ_DYNAMIC_MODEL_STOP_TIME_PARAM, IQConstants.MODEL_STOP_TIME_DEFAULT)
+    self.minimum_force_stop_length = self._read_float(IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM, IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT)
+    self.force_stops_enabled = self._read_bool(IQ_FORCE_STOPS_PARAM, True)
+
+  def set_slc_experimental_mode(self, active: bool) -> None:
+    self.slc_experimental_mode = bool(active)
+
+  def mode(self) -> str:
+    return self.IQEngineManager.get_mode()
+
+  def enabled(self) -> bool:
+    return self.IQDynamicStatus
+
+  def active(self) -> bool:
+    return self.IQDynamicA
+
+  def force_stop_requested(self) -> bool:
+    return bool(self.force_stops_enabled and self.stop_light_detected and self.model_stopped and not self.tracking_lead)
+
+  def setaeb(self) -> None:
+    self.aeb = self.aeb_c
+
+  def IQDynamicEngine(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    radar_state = sm['radarState']
+    model = sm['modelV2']
+
+    self.kph = car_state.vEgo * 3.6
+    self.cruise_kph = car_state.vCruise
+    self.ss_c = min(20, self.ss_c + 1) if car_state.standstill else max(0, self.ss_c - 1)
+
+    lead_status = float(getattr(radar_state.leadOne, "status", False))
+    self.IQFilterL.push(lead_status)
+    self.hasIQFilterLED = (self.IQFilterL.value() or 0.0) > IQConstants.LEAD_LOCK_GATE
+    self.tracking_lead = self.hasIQFilterLED
+    self.lead_speed = float(getattr(radar_state.leadOne, "vLead", 0.0))
+
+    prev_fcw = self.IQFilterFCW.value() or 0.0
+    self.IQFilterFCW.push(float(self.aeb > 0))
+    self.hasIQL = prev_fcw > 0.5
+
+    valid_model = len(model.position.x) == S_Y and len(model.orientation.x) == S_Y
+    if valid_model:
+      self.model_length = float(model.position.x[S_Y - 1])
+      self.e_x = self.model_length
+      self.e_d = interp(self.kph, IQConstants.BRAKE_CURVE_SPEED_AXIS, IQConstants.BRAKE_CURVE_DISTANCE_AXIS)
+      need = compute_slowdown_need(self.kph, self.model_length, self.e_d)
+    else:
+      self.model_length = 0.0
+      self.e_x = float('inf')
+      self.e_d = 0.0
+      need = 0.3 if self.kph > 20.0 else 0.0
+    self.IQFilterSDL.push(need)
+    self.IQDynamicU = self.IQFilterSDL.value() or 0.0
+    self.hasIQSDL = self.IQDynamicU > (IQConstants.BRAKE_CURVE_GATE * 0.8)
+    self.curve_detected = self.hasIQSDL
+
+    if self.ss_c <= 5 and not self.hasIQSDL:
+      slowness_observed = float(self.kph <= (self.cruise_kph * IQConstants.CRUISE_LAG_RATIO_GATE))
+      self.IQFilterSFL.push(slowness_observed)
+      threshold = IQConstants.CRUISE_LAG_GATE * (0.8 if self.hasIQSFL else 1.1)
+      self.hasIQSFL = (self.IQFilterSFL.value() or 0.0) > threshold
+
+    v_ego = float(car_state.vEgo)
+    self.low_speed_detected = not self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_speed
+    self.low_speed_lead_detected = self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_lead_speed
+
+    if self.tracking_lead:
+      slower_lead = (v_ego - self.lead_speed) > IQConstants.CRUISING_SPEED and self.conditional_slower_lead
+      stopped_lead = self.lead_speed < 1.0 and self.conditional_stopped_lead
+      self.IQFilterSlowLead.push(float(slower_lead or stopped_lead))
+      self.slow_lead_detected = (self.IQFilterSlowLead.value() or 0.0) >= IQConstants.SLOW_LEAD_THRESHOLD
+    else:
+      self.IQFilterSlowLead.reset()
+      self.slow_lead_detected = False
+
+    should_stop = bool(getattr(getattr(model, "action", None), "shouldStop", False))
+    model_stopping = self.model_length > 0.0 and self.model_length < max(v_ego * self.model_stop_time, IQConstants.CRUISING_SPEED)
+    self.model_stopped = bool(should_stop or model_stopping)
+    self.IQFilterModelStop.push(float(self.model_stopped and not self.tracking_lead))
+    self.stop_light_detected = (self.IQFilterModelStop.value() or 0.0) >= IQConstants.MODEL_STOP_THRESHOLD
+
+  def _request_blended(self, urgency: float = 1.0, emergency: bool = False) -> None:
+    self.IQEngineManager.request('blended', urgency=urgency, emergency=emergency)
+
+  def _request_acc(self, urgency: float = 0.8) -> None:
+    self.IQEngineManager.request('acc', urgency=urgency)
+
+  def IQStateEngine(self) -> None:
+    if self.hasIQL:
+      self._request_blended(1.0, True)
+    elif self.stop_light_detected and self.conditional_model_stops:
+      self._request_blended(1.0, self.model_stopped)
+    elif self.low_speed_detected or self.low_speed_lead_detected:
+      self._request_blended(0.95)
+    elif self.slow_lead_detected:
+      self._request_blended(0.9)
+    elif self.conditional_curves and self.hasIQSDL:
+      self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.5)))
+    elif self.conditional_slc_fallback and self.slc_experimental_mode:
+      self._request_blended(0.8)
+    elif self.ss_c > 3:
+      self._request_blended(0.9)
+    elif self.hasIQSFL and not self.hasIQSDL:
+      self._request_acc(0.8)
+    else:
+      self._request_acc(0.7)
+
+  def IQStateEngine_R(self) -> None:
+    if self.hasIQL:
+      self._request_blended(1.0, True)
+    elif self.stop_light_detected and self.conditional_model_stops:
+      self._request_blended(1.0, self.model_stopped)
+    elif self.low_speed_detected or self.low_speed_lead_detected:
+      self._request_blended(0.95)
+    elif self.slow_lead_detected:
+      self._request_blended(0.9)
+    elif self.conditional_curves and self.hasIQSDL:
+      self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.3)))
+    elif self.conditional_slc_fallback and self.slc_experimental_mode:
+      self._request_blended(0.8)
+    elif self.hasIQFilterLED and not (self.ss_c > 3):
+      self._request_acc(1.0)
+    elif self.ss_c > 3:
+      self._request_blended(0.9)
+    elif self.hasIQSFL and not self.hasIQSDL:
+      self._request_acc(0.8)
+    else:
+      self._request_acc(0.7)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._readIQParams()
+    self.setaeb()
+    self.IQDynamicEngine(sm)
+    if self.IQS.radarUnavailable:
+      self.IQStateEngine()
+    else:
+      self.IQStateEngine_R()
+    self.IQEngineManager.update()
+    self.IQDynamicA = sm['selfdriveState'].experimentalMode and self.IQDynamicStatus
+    self.IQDynamicF += 1

iqpilot/selfdrive/controls/lib/iq_dynamic/imahelper.py

@@ -0,0 +1,148 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from typing import Literal
+
+ModeType = Literal['acc', 'blended']
+IQ_DYNAMIC_MODE_PARAM = "IQDynamicMode"
+IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM = "IQDynamicConditionalCurves"
+IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM = "IQDynamicConditionalSlowerLead"
+IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM = "IQDynamicConditionalStoppedLead"
+IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM = "IQDynamicConditionalModelStops"
+IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM = "IQDynamicConditionalSLCFallback"
+IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM = "IQDynamicConditionalSpeed"
+IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM = "IQDynamicConditionalLeadSpeed"
+IQ_DYNAMIC_MODEL_STOP_TIME_PARAM = "IQDynamicModelStopTime"
+IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM = "IQDynamicMinimumForceStopLength"
+IQ_FORCE_STOPS_PARAM = "IQForceStops"
+
+
+class IQConstants:
+  CRUISING_SPEED = 3.0
+  SIGNAL_QUEUE_DEPTH = 6
+  LEAD_LOCK_GATE = 0.45
+  BRAKE_CURVE_QUEUE_DEPTH = 5
+  BRAKE_CURVE_GATE = 0.3
+  BRAKE_CURVE_SPEED_AXIS = [0., 10., 20., 30., 40., 50., 55., 60.]
+  BRAKE_CURVE_DISTANCE_AXIS = [32., 46., 64., 86., 108., 130., 145., 165.]
+  CRUISE_LAG_QUEUE_DEPTH = 10
+  CRUISE_LAG_GATE = 0.55
+  CRUISE_LAG_RATIO_GATE = 1.025
+  CONDITIONAL_SPEED_DEFAULT = 18.0
+  CONDITIONAL_LEAD_SPEED_DEFAULT = 24.0
+  MODEL_STOP_TIME_DEFAULT = 3.0
+  MODEL_STOP_THRESHOLD = 0.55
+  SLOW_LEAD_THRESHOLD = 0.55
+  FORCE_STOP_PLANNER_TIME = 3.0
+  MINIMUM_FORCE_STOP_LENGTH_DEFAULT = 0.0
+
+
+def compute_slowdown_need(kph: float, horizon_dist: float, desired_dist: float) -> float:
+  if horizon_dist >= desired_dist or desired_dist <= 0.0:
+    return 0.0
+
+  shortage = desired_dist - horizon_dist
+  shortage_ratio = shortage / desired_dist
+  need = min(1.0, shortage_ratio * 2.0)
+
+  if horizon_dist < desired_dist * 0.3:
+    need = min(1.0, need * 2.0)
+
+  if kph > 25.0:
+    need = min(1.0, need * (1.0 + (kph - 25.0) / 80.0))
+
+  return need
+
+
+class IQFilterEngine:
+  def __init__(self, initial=0.0, measurement_noise=0.1, process_noise=0.01, process_decay=1.0, smoothing_floor=0.85):
+    self._value = initial
+    self._variance = 1.0
+    self._measurement_noise = measurement_noise
+    self._process_noise = process_noise
+    self._process_decay = process_decay
+    self._smoothing_floor = smoothing_floor
+    self._initialized = False
+    self._samples = []
+    self._sample_limit = 10
+    self._confidence = 0.0
+
+  def push(self, measurement: float) -> None:
+    if len(self._samples) >= self._sample_limit:
+      self._samples.pop(0)
+    self._samples.append(measurement)
+
+    if not self._initialized:
+      self._value = measurement
+      self._initialized = True
+      self._confidence = 0.1
+      return
+
+    self._variance = self._process_decay * self._variance + self._process_noise
+    gain = self._variance / (self._variance + self._measurement_noise)
+    effective_gain = gain * (1.0 - self._smoothing_floor) + self._smoothing_floor * 0.1
+
+    innovation = measurement - self._value
+    self._value = self._value + effective_gain * innovation
+    self._variance = (1.0 - effective_gain) * self._variance
+
+    if abs(innovation) < 0.1:
+      self._confidence = min(1.0, self._confidence + 0.05)
+    else:
+      self._confidence = max(0.1, self._confidence - 0.02)
+
+  def value(self):
+    return self._value if self._initialized else None
+
+  def confidence(self) -> float:
+    return self._confidence
+
+  def reset(self) -> None:
+    self._initialized = False
+    self._samples = []
+    self._confidence = 0.0
+
+
+class IQModeEngine:
+  def __init__(self):
+    self._state: ModeType = 'acc'
+    self._scores = {'acc': 1.0, 'blended': 0.0}
+    self._switching_timer = 0
+    self._mode_age = 0
+    self._forced_takeover = False
+
+  def request(self, mode: ModeType, urgency: float = 1.0, emergency: bool = False) -> None:
+    if emergency:
+      self._forced_takeover = True
+      self._state = mode
+      self._switching_timer = 15
+      self._mode_age = 0
+      return
+
+    self._scores[mode] = min(1.0, self._scores[mode] + 0.1 * urgency)
+    for key in self._scores:
+      if key != mode:
+        self._scores[key] = max(0.0, self._scores[key] - 0.05)
+
+    if self._mode_age < 10 and not self._forced_takeover:
+      return
+
+    threshold = 0.6 if mode != self._state else 0.3
+    if self._scores[mode] > threshold and mode != self._state and self._switching_timer == 0:
+      self._switching_timer = 15
+      self._state = mode
+      self._mode_age = 0
+
+  def update(self) -> None:
+    if self._switching_timer > 0:
+      self._switching_timer -= 1
+
+    self._mode_age += 1
+    if self._forced_takeover and self._mode_age > 20:
+      self._forced_takeover = False
+
+    for key in self._scores:
+      self._scores[key] *= 0.98
+
+  def get_mode(self) -> ModeType:
+    return self._state

iqpilot/selfdrive/controls/lib/iq_dynamic/radar_manager.py

@@ -0,0 +1,68 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+RadarManager — IQ.Dynamics side of "Blend IQ.Pilot + Stock ACC Radar" (VW PQ only).
+
+Decides the high-level intent for the stock ACC radar and writes it onto iqCarControl for the iqdbc
+PQRadarHandler to execute on CAN. It does NOT touch CAN or read radar feedback directly: the handler
+(car process) owns the bus and the failure latch, and the carcontroller gates radar-accel passthrough
+on the live ACS_Sta_ADR. That keeps the desync guard automatic — if chill is requested but the radar
+isn't active, the carcontroller simply uses the planner's VoACC accel (standard IQ long).
+
+Intent produced:
+  radarBlendActive  feature enabled + PQ + alpha long available
+  radarEngageReq    want the radar's cruise engaged (engage same time long control engages)
+  radarCancelReq    cancel now (1 kph stop / driver brake / long disengaged / teardown)
+  useRadarAccel     chill (acc) mode + engaged -> carcontroller passes radar ACS_Sollbeschl through
+  radarSetSpeedKph  OP set speed to sync the radar's ACA_V_Wunsch toward
+  radarGapBars      OP follow-distance bars to mirror to the radar
+"""
+from openpilot.common.constants import CV
+
+CANCEL_CEIL_MS = 1.0 * CV.KPH_TO_MS  # cancel the radar at/below 1 kph (it can still see speed -> would fault)
+
+
+class RadarManager:
+  def __init__(self, CP, params):
+    self.CP = CP
+    self.params = params
+    self.is_pq = self._detect_pq(CP)
+    self.enabled_param = False
+
+  @staticmethod
+  def _detect_pq(CP) -> bool:
+    if getattr(CP, "brand", "") != "volkswagen":
+      return False
+    try:
+      from iqdbc.car.volkswagen.values import VolkswagenFlags
+      return bool(CP.flags & VolkswagenFlags.PQ)
+    except Exception:
+      return False
+
+  def read_params(self) -> None:
+    if self.is_pq:
+      self.enabled_param = self.params.get_bool("IQDynamicBlendStockRadar")
+
+  def update(self, CC_IQ, sm, set_speed_kph: float) -> None:
+    blend = bool(self.is_pq and self.enabled_param and self.CP.openpilotLongitudinalControl)
+    CC_IQ.radarBlendActive = blend
+    if not blend:
+      return
+
+    ss = sm['selfdriveState']
+    cs = sm['carState']
+    iq = sm['iqPlan'].iqDynamic
+
+    long_engaged = bool(ss.enabled) and self.CP.openpilotLongitudinalControl
+    iq_engaged = long_engaged and bool(iq.enabled)
+    chill = iq_engaged and bool(iq.active) and (iq.state == 'acc')
+    brake = bool(cs.brakePressed)
+    v_ego = float(cs.vEgo)
+
+    # Engage the radar whenever IQ.Dynamics long is engaged; cancel on stop/brake/teardown.
+    # The handler resolves priority (cancel wins) and applies the 1->2 kph engage hysteresis.
+    CC_IQ.radarEngageReq = iq_engaged and not brake
+    CC_IQ.radarCancelReq = (not iq_engaged) or brake or (v_ego <= CANCEL_CEIL_MS)
+    CC_IQ.useRadarAccel = chill
+    CC_IQ.radarSetSpeedKph = float(max(set_speed_kph, 0.0))
+    CC_IQ.radarGapBars = int(min(3, max(1, ss.personality.raw + 1)))

iqpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,256 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from datetime import datetime
+
+import numpy as np
+
+from cereal import messaging, custom
+from iqdbc.car import structs
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
+from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.engine import IQDynamicController
+from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import IQConstants
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import EndToEndAlertEngine
+from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise
+from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_ADAPT_ACC
+from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+
+IQDynamicState = custom.IQPlan.IQDynamicControl.IQDynamicControlState
+LongitudinalPlanSource = custom.IQPlan.LongitudinalPlanSource
+SpeedLimitAssistState = custom.IQPlan.SpeedLimit.AssistState
+SpeedLimitSource = custom.IQPlan.SpeedLimit.Source
+NavProvider = custom.IQNavState.LongitudinalProvider
+NavLongitudinalState = custom.IQNavState.LongitudinalState
+
+class LongitudinalPlannerIQ:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams, mpc):
+    self.events_iq = IQEvents()
+    self.iq_dynamic = IQDynamicController(CP, mpc)
+    self.custom_stop_distance = CustomStopDistance()
+    self.slimit = SLCVCruise()
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts = EndToEndAlertEngine()
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+    self.speed_limit_last = 0.
+    self.speed_limit_final_last = 0.
+    self.speed_limit_source = SpeedLimitSource.none
+    self.nav_engaged = False
+    self.nav_provider = NavProvider.none
+    self.nav_state = NavLongitudinalState.disabled
+    self.nav_speed_target = 0.
+    self.nav_accel_target = 0.
+    self.nav_valid = False
+    self.force_stop_timer = 0.0
+    self.forcing_stop = False
+    self.override_force_stop = False
+    self.override_force_stop_timer = 0.0
+    self.tracked_model_length = 0.0
+
+  def is_e2e(self, sm: messaging.SubMaster) -> bool:
+    experimental_mode = sm['selfdriveState'].experimentalMode
+    if not self.iq_dynamic.active():
+      return experimental_mode
+
+    return experimental_mode and self.iq_dynamic.mode() == "blended"
+
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    CS = sm['carState']
+    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
+    # SLC should apply whenever IQ.Pilot is engaged, even on stock-longitudinal cars
+    # where carControl.longActive stays false.
+    slc_apply_enabled = bool(getattr(sm['selfdriveState'], "enabled", False))
+
+    nav_state = sm['iqNavState']
+    self.nav_engaged = bool(getattr(nav_state, "longitudinalEngaged", False))
+    self.nav_provider = getattr(nav_state, "longitudinalProvider", NavProvider.none)
+    self.nav_state = getattr(nav_state, "longitudinalState", NavLongitudinalState.disabled)
+    self.nav_speed_target = float(getattr(nav_state, "speedTarget", 0.0))
+    self.nav_accel_target = float(getattr(nav_state, "accelTarget", 0.0))
+    self.nav_valid = bool(getattr(nav_state, "valid", False) and self.nav_engaged)
+
+    # IQ.Pilot custom Speed Limit Controller
+    now = datetime.now()
+    if hasattr(sm, "alive"):
+      time_validated = sm.alive.get('clocks', False) and getattr(sm['clocks'], 'timeValid', False)
+    else:
+      clocks = sm.get('clocks', None) if isinstance(sm, dict) else None
+      time_validated = bool(getattr(clocks, 'timeValid', False))
+    slc_v_cruise = self.slimit.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
+    self.iq_dynamic.set_slc_experimental_mode(self.slimit.slc_experimental_mode)
+    self.iq_dynamic.update(sm)
+    # Prefer confirmed controller output for UI/planner rendering.
+    # Fall back to active (policy-resolved) target/source when confirmed is unavailable.
+    display_speed_limit = self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target
+    display_source = self.slimit.slc_source if self.slimit.slc_source != "None" else self.slimit.slc_active_source
+
+    if display_speed_limit > 0:
+      self.speed_limit_last = display_speed_limit
+      self.speed_limit_final_last = display_speed_limit + self.slimit.slc_offset
+    elif display_source == "None":
+      self.speed_limit_last = 0.0
+      self.speed_limit_final_last = 0.0
+    # Respect user-defined max cruise speed when applying SLC.
+    if v_cruise_cluster > 0 and self.speed_limit_final_last > 0:
+      self.speed_limit_final_last = min(self.speed_limit_final_last, v_cruise_cluster)
+    source_map = {
+      "Dashboard": SpeedLimitSource.car,
+      "Map Data": SpeedLimitSource.map,
+      "Mapbox": SpeedLimitSource.map,
+      "None": SpeedLimitSource.none,
+    }
+    self.speed_limit_source = source_map.get(display_source, SpeedLimitSource.none)
+
+    targets = {
+      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
+      LongitudinalPlanSource.speedLimitAssist: (slc_v_cruise, a_ego),
+    }
+    if self.nav_valid:
+      targets[LongitudinalPlanSource.nav] = (self.nav_speed_target, self.nav_accel_target)
+
+    self.source = min(targets, key=lambda k: targets[k][0])
+    self.output_v_target, self.output_a_target = targets[self.source]
+    self.output_v_target = self._apply_force_stop(self.output_v_target, v_ego, sm, slc_apply_enabled)
+    # envelope shaping only in Assist mode: info/warn must never change the plan
+    self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slimit, "controller_enabled", False))
+                              and bool(getattr(self.slimit, "mode_assist", False)))
+    return self.output_v_target, self.output_a_target
+
+  def cruise_envelope(self, v_target: float, v_ego: float, t_idxs) -> np.ndarray:
+    """Per-timestep cruise speed over the MPC horizon: the scalar target, shaped down
+    ahead of an upcoming lower speed limit so the solver decelerates before the sign
+    instead of at it."""
+    env = np.full(len(t_idxs), max(float(v_target), 0.0))
+    if not getattr(self, "_envelope_enabled", False):
+      return env
+    slc = getattr(self.slimit, "slc", None)
+    next_limit = float(getattr(slc, "next_speed_limit", 0.0) or 0.0)
+    next_dist = float(getattr(slc, "next_speed_distance", 0.0) or 0.0)
+    if next_limit <= 0.0 or next_dist <= 0.0:
+      return env
+    next_target = max(next_limit + float(getattr(self.slimit, "slc_offset", 0.0) or 0.0), 0.0)
+    if next_target >= env[0]:
+      return env
+    travel = np.maximum(v_ego, 1.0) * np.asarray(t_idxs)
+    v_allowed = np.sqrt(np.maximum(next_target ** 2 + 2.0 * abs(LIMIT_ADAPT_ACC) * (next_dist - travel), next_target ** 2))
+    return np.minimum(env, v_allowed)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self.events_iq.clear()
+    for event_name in getattr(self.slimit, 'pending_events', []):
+      self.events_iq.add(event_name)
+    self.custom_stop_distance.update()
+    self.e2e_alerts.update(sm, self.events_iq)
+    if bool(getattr(sm["iqCarState"], "alcOverrideAlert", False)):
+      self.events_iq.add(custom.IQOnroadEvent.EventName.steeringOverrideReengageAlc)
+
+  def apply_e2e_stop_distance(self, sm: messaging.SubMaster, v_ego: float, a_target: float, should_stop: bool) -> tuple[float, bool]:
+    if not self.is_e2e(sm):
+      return a_target, should_stop
+    return self.custom_stop_distance.adjust_e2e_stop(a_target, should_stop, v_ego, sm['modelV2'])
+
+  def _apply_force_stop(self, v_target: float, v_ego: float, sm: messaging.SubMaster, apply_enabled: bool) -> float:
+    force_stop = self.iq_dynamic.force_stop_requested() and apply_enabled and self.override_force_stop_timer <= 0.0
+    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0.0
+    force_stop_enabled = self.force_stop_timer >= 1.0
+    force_stop_ramp_time = max(float(getattr(self.iq_dynamic, "model_stop_time", IQConstants.FORCE_STOP_PLANNER_TIME)), DT_MDL)
+
+    accel_pressed = bool(getattr(sm["iqCarState"], "accelPressed", False))
+    self.override_force_stop |= sm["carState"].gasPressed or accel_pressed
+    self.override_force_stop &= force_stop_enabled
+
+    if self.override_force_stop:
+      self.override_force_stop_timer = 10.0
+    elif self.override_force_stop_timer > 0.0:
+      self.override_force_stop_timer = max(0.0, self.override_force_stop_timer - DT_MDL)
+    else:
+      self.override_force_stop = False
+
+    if force_stop_enabled and not self.override_force_stop:
+      self.forcing_stop = True
+      self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
+      if sm["carState"].standstill:
+        return 0.0
+      return min(self.tracked_model_length / force_stop_ramp_time, v_target)
+
+    self.forcing_stop = False
+    self.tracked_model_length = max(
+      float(getattr(self.iq_dynamic, "model_length", 0.0)),
+      float(getattr(self.iq_dynamic, "minimum_force_stop_length", 0.0)),
+      0.0,
+    )
+    return v_target
+
+  def publish_longitudinal_plan_iq(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    def fill_plan(plan_msg) -> None:
+      plan_msg.longitudinalPlanSource = self.source
+      plan_msg.vTarget = float(self.output_v_target)
+      plan_msg.aTarget = float(self.output_a_target)
+      plan_msg.events = self.events_iq.to_msg()
+
+      # IQ.Dynamic control state
+      iq_dynamic = plan_msg.iqDynamic
+      iq_dynamic.state = IQDynamicState.blended if self.iq_dynamic.mode() == 'blended' else IQDynamicState.acc
+      iq_dynamic.enabled = self.iq_dynamic.enabled()
+      iq_dynamic.active = self.iq_dynamic.active()
+
+      nav_summary = plan_msg.iqNavState.nav
+      nav_summary.engaged = self.nav_engaged
+      nav_summary.provider = self.nav_provider
+      nav_summary.state = self.nav_state
+      nav_summary.speedTarget = float(self.nav_speed_target)
+      nav_summary.accelTarget = float(self.nav_accel_target)
+      nav_summary.valid = self.nav_valid
+
+      # Speed Limit
+      speedLimit = plan_msg.speedLimit
+      resolver = speedLimit.resolver
+      speed_limit = float(self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target)
+      speed_limit_offset = float(self.slimit.slc_offset)
+      speed_limit_final = speed_limit + speed_limit_offset if speed_limit > 0 else 0.
+      speed_limit_valid = speed_limit > 0.
+      speed_limit_last_valid = self.speed_limit_last > 0.
+
+      resolver.speedLimit = speed_limit
+      resolver.speedLimitLast = float(self.speed_limit_last)
+      resolver.speedLimitFinal = float(speed_limit_final)
+      resolver.speedLimitFinalLast = float(self.speed_limit_final_last)
+      resolver.speedLimitValid = speed_limit_valid
+      resolver.speedLimitLastValid = speed_limit_last_valid
+      resolver.speedLimitOffset = speed_limit_offset
+      resolver.distToSpeedLimit = 0.
+      resolver.source = self.speed_limit_source
+
+      assist = speedLimit.assist
+      slc_assist_state = self.slimit.assist_state
+      assist.enabled = bool(self.slimit.slc_target > 0 or self.slimit.slc_unconfirmed > 0)
+      assist.active = self.source == LongitudinalPlanSource.speedLimitAssist and self.slimit.slc_target > 0
+      if slc_assist_state is not None:
+        assist.state = slc_assist_state
+      elif not assist.enabled:
+        assist.state = SpeedLimitAssistState.disabled
+      elif self.slimit.slc_unconfirmed > 0:
+        assist.state = SpeedLimitAssistState.preActive
+      elif assist.active:
+        assist.state = SpeedLimitAssistState.active
+      else:
+        assist.state = SpeedLimitAssistState.inactive
+      assist.vTarget = float(self.output_v_target if assist.active else 255.)
+      assist.aTarget = float(self.slimit.slc_a_target if assist.active else 0.)
+
+      e2eAlerts = plan_msg.e2eAlerts
+      e2eAlerts.pathOpen = self.e2e_alerts.path_alert
+      e2eAlerts.leadPullaway = self.e2e_alerts.lead_alert
+
+    valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    plan_iq_send = messaging.new_message('iqPlan')
+    plan_iq_send.valid = valid
+    fill_plan(plan_iq_send.iqPlan)
+    pm.send('iqPlan', plan_iq_send)

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/__init__.py

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

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/__init__.py

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

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_locator.py

@@ -0,0 +1,85 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Candidate-ladder selection checks for get_nn_model_path, driven by a synthetic
+model directory so the assertions don't depend on which cars ship a model.
+"""
+import json
+import os
+
+import pytest
+
+from iqdbc.car import structs
+import openpilot.selfdrive.controls.lib.latcontrol_torque as locator
+
+
+@pytest.fixture
+def model_dir(tmp_path, monkeypatch):
+  """A fake model directory with known fingerprints + a substitute table."""
+  models = tmp_path / "models"
+  models.mkdir()
+  for stem in ("HONDA_CIVIC", "HONDA_CIVIC 12345", "TOYOTA_RAV4_TSS2", "MOCK"):
+    (models / f"{stem}.json").write_text("{}")
+
+  sub = tmp_path / "substitute.toml"
+  sub.write_text('"CHEVROLET_XX" = "TOYOTA_RAV4_TSS2"\n')
+
+  monkeypatch.setattr(locator, "TORQUE_NN_MODEL_PATH", str(models))
+  monkeypatch.setattr(locator, "TORQUE_NN_MODEL_SUBSTITUTE_PATH", str(sub))
+  monkeypatch.setattr(locator, "MOCK_MODEL_PATH", str(models / "MOCK.json"))
+  return models
+
+
+def make_cp(fingerprint, eps_fw=b"", angle=False):
+  cp = structs.CarParams()
+  cp.carFingerprint = fingerprint
+  if eps_fw:
+    fw = structs.CarParams.CarFw()
+    fw.ecu = "eps"
+    fw.fwVersion = eps_fw
+    cp.carFw = [fw]
+  if angle:
+    cp.steerControlType = structs.CarParams.SteerControlType.angle
+  return cp
+
+
+def test_exact_fingerprint_match(model_dir):
+  path, name, exact = locator.get_nn_model_path(make_cp("TOYOTA_RAV4_TSS2"))
+  assert name == "TOYOTA_RAV4_TSS2"
+  assert exact is True
+
+
+def test_fingerprint_plus_eps_fw_prefers_specific_file(model_dir):
+  # eps fw steers selection toward the fw-specific file. Note the resolved match
+  # is fuzzy, not exact: fwVersion is bytes and the candidate stringifies it as
+  # b'12345', so it never scores a perfect 1.0 against the "... 12345" filename.
+  path, name, exact = locator.get_nn_model_path(make_cp("HONDA_CIVIC", eps_fw=b"12345"))
+  assert name == "HONDA_CIVIC 12345"
+  assert exact is False
+
+
+def test_fuzzy_match_flags_non_exact(model_dir):
+  # close but not identical to a shipped fingerprint
+  path, name, exact = locator.get_nn_model_path(make_cp("TOYOTA_RAV4_TSS2_XYZ"))
+  assert name == "TOYOTA_RAV4_TSS2"
+  assert exact is False
+
+
+def test_substitute_fallback(model_dir):
+  # unknown fingerprint that the substitute table redirects
+  path, name, exact = locator.get_nn_model_path(make_cp("CHEVROLET_XX"))
+  assert name == "TOYOTA_RAV4_TSS2"
+  assert exact is False
+
+
+def test_angle_steer_is_always_mock(model_dir):
+  path, name, exact = locator.get_nn_model_path(make_cp("TOYOTA_RAV4_TSS2", angle=True))
+  assert name == "MOCK"
+  assert path == locator.MOCK_MODEL_PATH
+  assert exact is False
+
+
+def test_short_eps_fw_ignored(model_dir):
+  # a 3-char-or-less fw string is not used to build the candidate
+  path, name, _ = locator.get_nn_model_path(make_cp("HONDA_CIVIC", eps_fw=b"ab"))
+  assert name == "HONDA_CIVIC"

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_network.py

@@ -0,0 +1,97 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Unit checks for the NNFF model loader against the shipped model files.
+"""
+import json
+import os
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.latcontrol_torque import NNTorqueModel
+from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+
+# A minimal valid NNFF model (Twilsonco format: column-vector mean/std, dense_N_W/b
+# layers). Used as a fallback so the loader logic is still exercised when no trained
+# models are shipped (they are removed pending retraining and re-added over time).
+_SYNTHETIC_MODEL = {
+  "input_size": 4,
+  "output_size": 1,
+  "input_mean": [[0.0], [0.0], [0.0], [0.0]],
+  "input_std": [[1.0], [1.0], [1.0], [1.0]],
+  "layers": [
+    {"dense_1_W": [[0.5, 0.5, 0.5, 0.5], [0.5, 0.5, 0.5, 0.5]], "dense_1_b": [[0.0], [0.0]], "activation": "sigmoid"},
+    {"dense_2_W": [[2.0, 2.0]], "dense_2_b": [[-1.0]], "activation": "identity"},
+  ],
+}
+
+MODEL_FILES = sorted(f for f in os.listdir(TORQUE_NN_MODEL_PATH) if f.endswith(".json"))
+if MODEL_FILES:
+  _MODEL_DIR = TORQUE_NN_MODEL_PATH
+  _NAMES = MODEL_FILES
+else:
+  import tempfile
+  _MODEL_DIR = tempfile.mkdtemp(prefix="nnff_synthetic_")
+  with open(os.path.join(_MODEL_DIR, "SYNTHETIC.json"), "w") as _f:
+    json.dump(_SYNTHETIC_MODEL, _f)
+  _NAMES = ["SYNTHETIC.json"]
+
+SAMPLE = [f for f in ("HYUNDAI_IONIQ_5.json", "TOYOTA_RAV4_TSS2_2022.json", "MOCK.json") if f in _NAMES] \
+         or _NAMES[:3]
+
+
+def _path(name):
+  return os.path.join(_MODEL_DIR, name)
+
+
+@pytest.mark.parametrize("name", _NAMES, ids=[n[:-5] for n in _NAMES])
+def test_every_model_loads_and_is_finite(name):
+  m = NNTorqueModel(_path(name))
+  assert m.input_size >= 2
+  assert m.output_size >= 1
+  assert m.input_mean.shape == m.input_std.shape
+  out = m.evaluate([0.0] * m.input_size)
+  assert np.isfinite(out)
+  assert isinstance(m.friction_override, (bool, np.bool_))
+
+
+@pytest.mark.parametrize("name", SAMPLE, ids=[n[:-5] for n in SAMPLE])
+class TestModelBehavior:
+  def test_short_input_is_zero_padded(self, name):
+    m = NNTorqueModel(_path(name))
+    padded = m.evaluate([5.0, 1.0])
+    explicit = m.evaluate([5.0, 1.0] + [0.0] * (m.input_size - 2))
+    assert padded == explicit
+
+  def test_too_short_input_raises(self, name):
+    m = NNTorqueModel(_path(name))
+    with pytest.raises(ValueError):
+      m.evaluate([1.0])
+
+  def test_zero_bias_matches_manual_bias_removal(self, name):
+    m = NNTorqueModel(_path(name))
+    mz = NNTorqueModel(_path(name), zero_bias=True)
+    assert all(np.allclose(b, 0.0) for b in mz._biases)
+    # weights and activations are unchanged
+    assert len(mz._weights) == len(m._weights)
+
+  def test_deterministic(self, name):
+    m = NNTorqueModel(_path(name))
+    vec = [float(v) for v in np.linspace(-1.5, 1.5, m.input_size)]
+    assert m.evaluate(vec) == m.evaluate(list(vec))
+
+
+def test_activation_registry_rejects_unknown(tmp_path):
+  base = json.load(open(_path(SAMPLE[0])))
+  base["layers"][-1]["activation"] = "not_a_real_activation"
+  bad = tmp_path / "bad.json"
+  bad.write_text(json.dumps(base))
+  with pytest.raises(ValueError):
+    NNTorqueModel(str(bad))
+
+
+def test_sigmoid_identity_helpers():
+  assert NNTorqueModel.identity(3.5) == 3.5
+  assert 0.0 < float(NNTorqueModel.sigmoid(np.array([0.0]))[0]) < 1.0
+  assert abs(float(NNTorqueModel.sigmoid(np.array([0.0]))[0]) - 0.5) < 1e-6

iqpilot/selfdrive/controls/lib/neural_network_feed_forward/tests/test_nnff_assembly.py

@@ -0,0 +1,106 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Off-device checks for the NNFF controller wiring and the nav torque pulse,
+built on lightweight fakes so they run without a car interface.
+"""
+import os
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from cereal import log
+from openpilot.common.params import Params
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.latcontrol_torque import NeuralNetworkFeedForward
+from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+
+_REAL_MODEL = next((f for f in sorted(os.listdir(TORQUE_NN_MODEL_PATH))
+                    if f.endswith(".json") and f != "MOCK.json"), None)
+
+# Models are shipped separately and re-added as retrained; with none present,
+# NNFF is a no-op (falls back to stock torque FF), so the assembly tests skip.
+pytestmark = pytest.mark.skipif(_REAL_MODEL is None,
+                                reason="no NNFF models present (nuked pending retraining)")
+
+
+def _torque_fn():
+  def fn(inputs, tp, gravity_adjusted=False):
+    base = inputs.lateral_acceleration - (inputs.roll_compensation if gravity_adjusted else 0.0)
+    return base * 0.4
+  return fn
+
+
+class FakeCI:
+  def torque_from_lateral_accel_in_torque_space(self):
+    return _torque_fn()
+
+
+class FakeVM:
+  @staticmethod
+  def calc_curvature(angle, v, roll):
+    return angle / (max(v, 1.0) ** 2 * 0.05 + 2.0)
+
+
+def _model_v2():
+  t = np.array(ModelConstants.T_IDXS)
+  return SimpleNamespace(
+    orientation=SimpleNamespace(x=(0.02 * np.sin(t)).tolist(), y=(0.01 * np.cos(t)).tolist()),
+    acceleration=SimpleNamespace(y=(0.8 * np.sin(2.0 * t)).tolist()))
+
+
+def _make_controller(model_file):
+  Params().put_bool("NeuralNetworkFeedForward", True)
+  path = os.path.join(TORQUE_NN_MODEL_PATH, model_file)
+  cp = SimpleNamespace(steerActuatorDelay=0.15)
+  cp_iq = SimpleNamespace(iqLateralNet=SimpleNamespace(
+    model=SimpleNamespace(path=path, name=os.path.splitext(model_file)[0])))
+  lac = SimpleNamespace(steer_max=1.0, torque_params=SimpleNamespace(
+    latAccelFactor=2.5, latAccelOffset=0.0, friction=0.1, steeringAngleDeadzoneDeg=0.0))
+  return NeuralNetworkFeedForward(lac, cp, cp_iq, FakeCI())
+
+
+def _drive_once(nnff, step=1, pressed=False):
+  nnff.update_model_v2(_model_v2())
+  v = 20.0
+  dla = 1.0
+  cs = SimpleNamespace(vEgo=v, aEgo=0.2, steeringPressed=pressed, steeringRateDeg=1.0)
+  cal = SimpleNamespace(roll=0.02)
+  pose = SimpleNamespace(orientation=SimpleNamespace(pitch=0.01))
+  pid = PIDController([[1, 30], [10.0, 0.8]], 0.15, rate=100)
+  pid.set_limits(1.0, -1.0)
+  pt = log.ControlsState.LateralTorqueState.new_message()
+  return nnff.update(cs, FakeVM(), pid, cal, dla, pt, dla, 0.8 * dla, pose, 0.02 * 9.81,
+                     dla, 0.8 * dla, 0.01, dla - 0.02 * 9.81, dla / v ** 2, 0.8 * dla / v ** 2, False, 0.3)
+
+
+class TestControllerWiring:
+  def test_real_model_reports_present(self):
+    nnff = _make_controller(_REAL_MODEL)
+    assert nnff.has_nn_model is True
+
+  def test_mock_model_reports_absent(self):
+    nnff = _make_controller("MOCK.json")
+    assert nnff.has_nn_model is False
+    assert nnff.model.input_size >= 2  # MOCK still loads as a valid net
+
+  def test_update_returns_finite_torque(self):
+    nnff = _make_controller(_REAL_MODEL)
+    pid_log, torque = _drive_once(nnff)
+    assert np.isfinite(torque)
+    assert np.isfinite(pid_log.error)
+
+  def test_lag_update_refreshes_future_times(self):
+    nnff = _make_controller(_REAL_MODEL)
+    before = list(nnff.nn_future_times)
+    nnff.update_lateral_lag(0.5)
+    after = list(nnff.nn_future_times)
+    assert after != before
+    assert all(a == pytest.approx(f + nnff.desired_lat_jerk_time) for a, f in zip(after, nnff.future_times, strict=True))
+
+  def test_disabled_when_model_invalid(self):
+    nnff = _make_controller(_REAL_MODEL)
+    nnff.model_valid = False
+    assert nnff._nnff_enabled is False

iqpilot/selfdrive/controls/lib/slc_vcruise.py

@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+import time
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
+from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController
+
+CRUISING_SPEED = 7
+
+
+class SLCVCruise:
+
+  def __init__(self):
+    self.params = Params()
+
+    self.slc = SpeedLimitController(self.params)
+    self._last_debug_log_t = 0.0
+    self._last_debug_signature = None
+
+    # Exposed SLC state (for UI/logging)
+    self.controller_enabled = False
+    self.mode_assist = False
+    self.slc_offset = 0
+    self.slc_target = 0
+    self.slc_source = "None"
+    self.slc_unconfirmed = 0
+    self.slc_overridden_speed = 0
+    self.slc_active_target = 0
+    self.slc_active_source = "None"
+    self._user_max_speed = 0.0
+    self.slc_experimental_mode = False
+    self.pending_events = []
+
+  @property
+  def assist_state(self):
+    return getattr(self.slc, 'assist_state', None)
+
+  @property
+  def slc_a_target(self):
+    return float(getattr(self.slc, 'output_a_target', 0.0))
+
+  def _maybe_log_debug(self, slc_params, apply_enabled, v_cruise, v_ego, dashboard_speed_limit, applied_target, returned_v_cruise):
+    map_speed_limit = float(getattr(self.slc, "map_speed_limit", 0.0) or 0.0)
+    mapbox_limit = float(getattr(self.slc, "mapbox_limit", 0.0) or 0.0)
+    next_speed_limit = float(getattr(self.slc, "next_speed_limit", 0.0) or 0.0)
+    gps_valid = bool(getattr(self.slc, "gps_valid", False))
+    signature = (
+      bool(slc_params["speed_limit_controller"]),
+      bool(slc_params["show_speed_limits"]),
+      self.slc.target,
+      self.slc.source,
+      self.slc.active_target,
+      self.slc.active_source,
+      map_speed_limit,
+      mapbox_limit,
+      next_speed_limit,
+      self.slc.overridden_speed,
+      bool(apply_enabled),
+      float(applied_target),
+      float(returned_v_cruise),
+    )
+    now_mono = time.monotonic()
+    if signature == self._last_debug_signature and now_mono - self._last_debug_log_t < 5.0:
+      return
+
+    self._last_debug_signature = signature
+    self._last_debug_log_t = now_mono
+
+    message = (
+      "SLC debug: "
+      f"mode={int(self.params.get('IQSpeedAssistMode', return_default=True))} "
+      f"controller={slc_params['speed_limit_controller']} "
+      f"show={slc_params['show_speed_limits']} "
+      f"apply_enabled={bool(apply_enabled)} "
+      f"dashboard={round(float(dashboard_speed_limit), 2)} "
+      f"map_data={round(map_speed_limit, 2)} "
+      f"mapbox={round(mapbox_limit, 2)} "
+      f"next_map={round(next_speed_limit, 2)} "
+      f"selected_source={self.slc.source} "
+      f"selected_target={round(float(self.slc.target), 2)} "
+      f"active_source={self.slc.active_source} "
+      f"active_target={round(float(self.slc.active_target), 2)} "
+      f"offset={round(float(self.slc_offset), 2)} "
+      f"override={round(float(self.slc.overridden_speed), 2)} "
+      f"gps_valid={gps_valid} "
+      f"applied_target={round(float(applied_target), 2)} "
+      f"returned_v_cruise={round(float(returned_v_cruise), 2)} "
+      f"v_cruise={round(float(v_cruise), 2)} "
+      f"v_ego={round(float(v_ego), 2)}"
+    )
+    cloudlog.info(message)
+    k3_slc_log(message)
+
+  def _get_slc_params(self):
+    """
+    Load SLC parameters from Params.
+
+    Returns:
+      Dictionary of SLC configuration parameters
+    """
+    def get_param_bool(key, default=False):
+      value = self.params.get_bool(key)
+      return value if value is not None else default
+
+    def get_param_float(key, default=0.0):
+      value = self.params.get(key)
+      if value is None:
+        return default
+      if isinstance(value, bytes):
+        try:
+          return float(value.decode('utf-8'))
+        except (ValueError, AttributeError):
+          return default
+      return float(value)
+
+    def get_param_str(key, default=""):
+      value = self.params.get(key)
+      if value is None:
+        return default
+      if isinstance(value, bytes):
+        return value.decode('utf-8')
+      return str(value)
+
+    slc_policy = int(get_param_str("SLCPolicy", "1"))
+
+    override_method = int(get_param_str("SLCOverrideMethod", "0"))
+    override_manual = (override_method == 0)
+    override_set_speed = (override_method == 1)
+
+    speed_limit_mode = int(get_param_str("IQSpeedAssistMode", "1"))  # default: SpeedLimitMode.information
+    speed_limit_controller = get_param_bool("SpeedLimitController")
+    show_speed_limits = get_param_bool("ShowSpeedLimits")
+
+    if speed_limit_mode == 0:  # SpeedLimitMode.off
+      speed_limit_controller = False
+      show_speed_limits = False
+    elif speed_limit_mode == 3:  # SpeedLimitMode.control
+      speed_limit_controller = True
+      show_speed_limits = False
+    else:
+      speed_limit_controller = False
+      show_speed_limits = True
+
+    return {
+      "speed_limit_controller": speed_limit_controller,
+      "speed_limit_mode": speed_limit_mode,
+      "show_speed_limits": show_speed_limits,
+      "slc_policy": slc_policy,
+      "slc_auto_confirm": get_param_bool("SLCAutoConfirm"),
+      "speed_limit_confirmation_higher": get_param_bool("SpeedLimitConfirmationHigher"),
+      "speed_limit_confirmation_lower": get_param_bool("SpeedLimitConfirmationLower"),
+      "map_speed_lookahead_higher": get_param_float("MapSpeedLookaheadHigher", 5.0),
+      "map_speed_lookahead_lower": get_param_float("MapSpeedLookaheadLower", 5.0),
+      "slc_fallback_experimental_mode": get_param_bool("SLCFallbackExperimentalMode"),
+      "slc_fallback_set_speed": get_param_bool("SLCFallbackSetSpeed"),
+      "slc_fallback_previous_speed_limit": get_param_bool("SLCFallbackPreviousSpeedLimit"),
+      "speed_limit_controller_override_manual": override_manual,
+      "speed_limit_controller_override_set_speed": override_set_speed,
+      "slc_online_filler": get_param_bool("SLCOnlineFiller"),
+      "is_metric": get_param_bool("IsMetric"),
+      "construction_zone_assist": get_param_bool("ConstructionZoneAssist"),
+      "construction_zone_speed": get_param_float("ConstructionZoneSpeed", 60.0),
+    }
+
+  @staticmethod
+  def _allow_auto_raise(slc_params):
+    # Reuse the existing "confirm higher" toggle as the gate:
+    # disabled confirm => allow SLC to raise cruise to a higher accepted limit.
+    return not slc_params["speed_limit_confirmation_higher"]
+
+  def update(self, apply_enabled, now, time_validated, v_cruise, v_ego, sm):
+    slc_params = self._get_slc_params()
+    self.controller_enabled = bool(slc_params["speed_limit_controller"])
+    self.mode_assist = int(slc_params["speed_limit_mode"]) == 3  # SpeedLimitMode.control
+    is_metric = slc_params["is_metric"]
+    v_cruise_cluster = max(sm["carState"].vCruiseCluster * CV.KPH_TO_MS, v_cruise)
+    v_cruise_diff = v_cruise_cluster - v_cruise
+
+    v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego)
+    v_ego_diff = v_ego_cluster - v_ego
+    car_state_iq = sm["iqCarState"]
+    dashboard_speed_limit = car_state_iq.speedLimit if hasattr(car_state_iq, "speedLimit") else 0
+    if apply_enabled:
+      if self._user_max_speed <= 0.0:
+        self._user_max_speed = v_cruise_cluster
+      elif v_cruise_cluster > self._user_max_speed:
+        self._user_max_speed = v_cruise_cluster
+    else:
+      self._user_max_speed = 0.0
+    if slc_params["speed_limit_controller"]:
+      self.slc.update_limits(dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params)
+      self.pending_events = list(getattr(self.slc, 'pending_events', []))
+      self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, slc_params, is_metric)
+
+      self.slc_offset = 0 if self.slc.source == "Construction" else self.slc.get_offset(is_metric)
+      self.slc_target = self.slc.target
+      self.slc_source = self.slc.source
+      self.slc_active_target = self.slc.active_target
+      self.slc_active_source = self.slc.active_source
+      self.slc_unconfirmed = self.slc.unconfirmed_speed_limit
+      self.slc_overridden_speed = self.slc.overridden_speed
+
+    elif slc_params["show_speed_limits"]:
+      self.slc.update_limits(dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params)
+      self.pending_events = []
+
+      self.slc_offset = 0
+      self.slc_target = self.slc.target
+      self.slc_source = self.slc.source
+      self.slc_active_target = self.slc.active_target
+      self.slc_active_source = self.slc.active_source
+      self.slc_unconfirmed = self.slc.unconfirmed_speed_limit
+      self.slc_overridden_speed = 0
+
+    else:
+      self.pending_events = []
+      self.slc_offset = 0
+      self.slc_target = 0
+      self.slc_source = "None"
+      self.slc_active_target = 0
+      self.slc_active_source = "None"
+      self.slc_unconfirmed = 0
+      self.slc_overridden_speed = 0
+
+    self.slc_experimental_mode = bool(
+      slc_params["speed_limit_controller"] and
+      slc_params["slc_fallback_experimental_mode"] and
+      self.slc_target <= 0
+    )
+
+    applied_target = 0.0
+
+    if slc_params["speed_limit_controller"] and apply_enabled:
+      slc_target_with_offset = max(self.slc_overridden_speed, self.slc_target + self.slc_offset)
+      allow_auto_raise = self._allow_auto_raise(slc_params)
+      if self._user_max_speed > 0.0 and not allow_auto_raise:
+        slc_target_with_offset = min(slc_target_with_offset, self._user_max_speed)
+      slc_cruise_target = slc_target_with_offset - v_ego_diff
+
+      if slc_cruise_target >= CRUISING_SPEED:
+        applied_target = slc_cruise_target
+        if allow_auto_raise and self.slc_source != "Construction":
+          v_cruise = slc_cruise_target
+        else:
+          v_cruise = min(v_cruise, slc_cruise_target)
+
+    self._maybe_log_debug(slc_params, apply_enabled, v_cruise, v_ego, dashboard_speed_limit, applied_target, v_cruise)
+
+    return v_cruise

iqpilot/selfdrive/controls/lib/smooth_stops.py

@@ -0,0 +1,73 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
+"""
+from iqdbc.car.interfaces import ACCEL_MIN
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+
+STANDSTILL_SPEED = 0.05
+STANDSTILL_HOLD_SPEED = 0.15
+SETTLE_DECEL = 0.80
+TAPER_SPEED = 1.0
+STOP_KISS_DECEL = 0.25
+STOP_GAP_MARGIN = 3.0
+MIN_GAP_BUDGET = 0.5
+PROGRESS_EPS = 0.02
+ANTI_CREEP_RATE = 0.50
+SETTLE_JERK = 2.5
+EMERGENCY_DECEL = 3.0
+
+
+def read_smooth_stops_enabled(params: Params) -> bool:
+  return params.get_bool("IQForceStops")
+
+
+class SmoothStopController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.enabled = False
+    self._v_min = float("inf")
+    self._stall_s = 0.0
+    self.read_params()
+
+  def read_params(self) -> None:
+    self.enabled = read_smooth_stops_enabled(self.params)
+
+  def update(self) -> None:
+    if self.frame % int(3 / DT_CTRL) == 0:
+      self.read_params()
+    self.frame += 1
+
+  def reset(self) -> None:
+    self._v_min = float("inf")
+    self._stall_s = 0.0
+
+  def want_hold(self, should_stop: bool, v_ego: float, standstill: bool) -> bool:
+    return bool(should_stop and (v_ego <= STANDSTILL_SPEED or (standstill and v_ego <= STANDSTILL_HOLD_SPEED)))
+
+  def settle(self, a_target: float, v_ego: float, lead_distance: float, has_lead: bool, last_output: float) -> float:
+    landing = STOP_KISS_DECEL + (SETTLE_DECEL - STOP_KISS_DECEL) * min(v_ego / TAPER_SPEED, 1.0)
+    a_settle = -landing
+
+    if has_lead and lead_distance > 0.0:
+      gap = max(lead_distance - STOP_GAP_MARGIN, MIN_GAP_BUDGET)
+      a_settle = min(a_settle, -(v_ego * v_ego) / (2.0 * gap))
+
+    if v_ego < self._v_min - PROGRESS_EPS:
+      self._v_min = v_ego
+      self._stall_s = 0.0
+    else:
+      self._stall_s += DT_CTRL
+    a_settle -= ANTI_CREEP_RATE * self._stall_s
+
+    a_settle = max(a_settle, ACCEL_MIN)
+    target = min(a_settle, a_target)
+
+    if target <= -EMERGENCY_DECEL:
+      return target
+
+    step = SETTLE_JERK * DT_CTRL
+    return min(max(target, last_output - step), last_output + step)

iqpilot/selfdrive/controls/lib/speed_limit_controller.py

@@ -0,0 +1,825 @@
+#!/usr/bin/env python3
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import calendar
+import json
+import math
+import time
+from concurrent.futures import ThreadPoolExecutor
+
+import numpy as np
+
+from cereal import car, custom
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
+from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
+from openpilot.iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
+from openpilot.iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC, OFFSET_PERCENT_MAX
+
+try:
+  import requests
+except ImportError:
+  requests = None
+
+ButtonType = car.CarState.ButtonEvent.Type
+SpeedLimitAssistState = custom.IQPlan.SpeedLimit.AssistState
+EventNameIQ = custom.IQOnroadEvent.EventName
+
+LIMIT_MIN_ACC = -1.5
+LIMIT_MAX_ACC = 1.0
+LIMIT_MIN_SPEED = 8.33
+LIMIT_SPEED_OFFSET_TH = -1.0
+LIMIT_ADAPT_ACC = -1.0
+CONTROL_HORIZON = 10.0
+
+AUTO_CONFIRM_PERIOD = 5.0
+AUTO_DENY_PERIOD = 30.0
+
+POLICY_MAP_DATA_ONLY = 0
+POLICY_MAP_DATA_PRIORITY = 1
+POLICY_COMBINED = 2
+
+CONFIRM_LOWER_BUTTONS = frozenset({ButtonType.decelCruise, ButtonType.setCruise})
+CONFIRM_HIGHER_BUTTONS = frozenset({ButtonType.accelCruise, ButtonType.resumeCruise})
+
+
+class IQSpeedLimitResolver:
+  def __init__(self):
+    self.map_speed_limit = 0.0
+    self.next_speed_limit = 0.0
+    self.next_speed_distance = 0.0
+
+  @staticmethod
+  def _is_alive(sm, key):
+    if hasattr(sm, "alive"):
+      return bool(sm.alive.get(key, False))
+    return False
+
+  def update_map_data(self, v_ego, sm, lookahead_lower, lookahead_higher):
+    if not self._is_alive(sm, "iqLiveData"):
+      self.map_speed_limit = 0.0
+      self.next_speed_limit = 0.0
+      self.next_speed_distance = 0.0
+      return
+
+    map_data = sm["iqLiveData"]
+    current_limit = float(getattr(map_data, "speedLimit", 0)) if getattr(map_data, "speedLimitValid", False) else 0.0
+    ahead_limit = float(getattr(map_data, "speedLimitAhead", 0)) if getattr(map_data, "speedLimitAheadValid", False) else 0.0
+    ahead_distance = float(getattr(map_data, "speedLimitAheadDistance", 0))
+
+    self.next_speed_limit = ahead_limit
+    self.next_speed_distance = ahead_distance
+
+    if ahead_limit > 0 and ahead_distance > 0:
+      if ahead_limit < v_ego:
+        adapt_time = (ahead_limit - v_ego) / LIMIT_ADAPT_ACC  # positive (LIMIT_ADAPT_ACC negative)
+        adapt_distance = v_ego * adapt_time + 0.5 * LIMIT_ADAPT_ACC * adapt_time**2
+        comfort_distance = lookahead_lower * v_ego
+        if ahead_distance <= max(adapt_distance, comfort_distance):
+          self.map_speed_limit = ahead_limit
+          return
+      elif ahead_limit > current_limit:
+        if ahead_distance <= lookahead_higher * v_ego:
+          self.map_speed_limit = ahead_limit
+          return
+
+    self.map_speed_limit = current_limit
+
+  def resolve(self, dashboard_limit, mapbox_limit, slc_params):
+    policy = slc_params.get("slc_policy", POLICY_MAP_DATA_PRIORITY)
+
+    sources = {}
+    if dashboard_limit >= LIMIT_MIN_SPEED:
+      sources["Dashboard"] = dashboard_limit
+    if mapbox_limit >= LIMIT_MIN_SPEED:
+      sources["Mapbox"] = mapbox_limit
+    if self.map_speed_limit >= LIMIT_MIN_SPEED:
+      sources["Map Data"] = self.map_speed_limit
+
+    if policy == POLICY_MAP_DATA_ONLY:
+      if "Map Data" in sources:
+        return sources["Map Data"], "Map Data"
+      return 0.0, "None"
+
+    if policy == POLICY_MAP_DATA_PRIORITY:
+      for src in ("Map Data", "Dashboard", "Mapbox"):
+        if src in sources:
+          return sources[src], src
+      return 0.0, "None"
+
+    if policy == POLICY_COMBINED:
+      if sources:
+        src = min(sources, key=sources.get)
+        return sources[src], src
+      return 0.0, "None"
+
+    return 0.0, "None"
+
+
+class IQSpeedLimitAssist:
+  def __init__(self, params):
+    self._params = params
+    self._state = SpeedLimitAssistState.inactive
+    self._prev_state = SpeedLimitAssistState.inactive
+
+    self.target = 0.0
+    self.source = "None"
+
+    self.unconfirmed_limit = 0.0
+    self.unconfirmed_source = "None"
+
+    self.previous_target = 0.0
+    self.previous_source = "None"
+    self.denied_target = 0.0
+
+    self._pre_active_timer = 0.0
+
+    self.pending_events = []
+
+    self.output_a_target = 0.0
+
+    self.just_confirmed = False
+
+  @property
+  def state(self):
+    return self._state
+
+  def update(self, enabled, v_ego, resolved_limit, resolved_source, slc_params, sm):
+    self.pending_events = []
+    self.just_confirmed = False
+    self._prev_state = self._state
+
+    if not enabled:
+      if self._state != SpeedLimitAssistState.disabled:
+        self._state = SpeedLimitAssistState.disabled
+        self._reset_confirmed()
+        self._reset_unconfirmed()
+      self.output_a_target = 0.0
+      self._fire_transition_events()
+      return
+
+    if self._state == SpeedLimitAssistState.disabled:
+      self._state = SpeedLimitAssistState.inactive
+
+    has_limit = resolved_limit >= LIMIT_MIN_SPEED
+    v_offset = self.target - v_ego if self.target > 0 else 0.0
+
+    if self._state == SpeedLimitAssistState.inactive:
+      if has_limit:
+        if self._needs_confirmation(resolved_limit, slc_params):
+          self._enter_pre_active(resolved_limit, resolved_source)
+        else:
+          self._apply_limit(resolved_limit, resolved_source, v_ego, fire_changed_event=True)
+
+    elif self._state == SpeedLimitAssistState.preActive:
+      self._pre_active_timer += DT_MDL
+      confirmed, denied = self._check_confirmation(sm, slc_params)
+
+      if denied:
+        self.denied_target = self.unconfirmed_limit
+        self.previous_source = self.unconfirmed_source
+        self.previous_target = self.unconfirmed_limit
+        self._reset_unconfirmed()
+        self._state = SpeedLimitAssistState.inactive
+      elif confirmed:
+        self._confirm(v_ego)
+      elif not has_limit:
+        self._reset_unconfirmed()
+        self._state = SpeedLimitAssistState.inactive
+
+    elif self._state in (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting):
+      if not has_limit:
+        if self.target > 0:
+          self.previous_target = self.target
+          self.previous_source = self.source
+        self._reset_confirmed()
+        self._state = SpeedLimitAssistState.inactive
+      elif abs(resolved_limit - self.target) >= 1.0:
+        if self._needs_confirmation(resolved_limit, slc_params):
+          self._enter_pre_active(resolved_limit, resolved_source)
+        else:
+          self._apply_limit(resolved_limit, resolved_source, v_ego, fire_changed_event=True)
+      elif self._state == SpeedLimitAssistState.adapting:
+        if v_offset >= LIMIT_SPEED_OFFSET_TH:
+          self._state = SpeedLimitAssistState.active
+      elif self._state == SpeedLimitAssistState.active:
+        if v_offset < LIMIT_SPEED_OFFSET_TH:
+          self._state = SpeedLimitAssistState.adapting
+
+    self._update_a_target(v_ego)
+    self._fire_transition_events()
+
+  def _enter_pre_active(self, limit, source):
+    self.unconfirmed_limit = limit
+    self.unconfirmed_source = source
+    self._state = SpeedLimitAssistState.preActive
+    self._pre_active_timer = 0.0
+
+  def _confirm(self, v_ego):
+    self.target = self.unconfirmed_limit
+    self.source = self.unconfirmed_source
+    self.previous_target = self.target
+    self.previous_source = self.source
+    self.denied_target = 0.0
+    self._reset_unconfirmed()
+    self._params.put_nonblocking("PreviousSpeedLimit", float(self.target))
+    self.just_confirmed = True
+    v_offset = self.target - v_ego
+    self._state = SpeedLimitAssistState.adapting if v_offset < LIMIT_SPEED_OFFSET_TH else SpeedLimitAssistState.active
+
+  def _apply_limit(self, limit, source, v_ego, fire_changed_event=False):
+    self.target = limit
+    self.source = source
+    self.previous_target = self.target
+    self.previous_source = self.source
+    self._params.put_nonblocking("PreviousSpeedLimit", float(self.target))
+    if fire_changed_event:
+      self.pending_events.append(EventNameIQ.speedLimitChanged)
+    v_offset = self.target - v_ego
+    self._state = SpeedLimitAssistState.adapting if v_offset < LIMIT_SPEED_OFFSET_TH else SpeedLimitAssistState.active
+
+  def _needs_confirmation(self, new_limit, slc_params):
+    if new_limit < self.target:
+      return slc_params.get("speed_limit_confirmation_lower", False)
+    return slc_params.get("speed_limit_confirmation_higher", False)
+
+  def _check_confirmation(self, sm, slc_params):
+    confirmed = False
+    denied = False
+
+    if slc_params.get("slc_auto_confirm", False) and self._pre_active_timer >= AUTO_CONFIRM_PERIOD:
+      return True, False
+
+    if self._pre_active_timer >= AUTO_DENY_PERIOD:
+      return False, True
+
+    is_lower = (self.target <= 0) or (self.unconfirmed_limit <= self.target)
+    try:
+      for btn in sm["carState"].buttonEvents:
+        if btn.pressed:
+          continue
+        if is_lower and btn.type in CONFIRM_LOWER_BUTTONS:
+          confirmed = True
+          break
+        elif not is_lower and btn.type in CONFIRM_HIGHER_BUTTONS:
+          confirmed = True
+          break
+    except (AttributeError, TypeError):
+      pass
+
+    return confirmed, denied
+
+  def _update_a_target(self, v_ego):
+    if self._state in (SpeedLimitAssistState.adapting, SpeedLimitAssistState.active) and self.target > 0:
+      v_offset = self.target - v_ego
+      self.output_a_target = float(np.clip(v_offset / CONTROL_HORIZON, LIMIT_MIN_ACC, LIMIT_MAX_ACC))
+    else:
+      self.output_a_target = 0.0
+
+  def _fire_transition_events(self):
+    prev = self._prev_state
+    curr = self._state
+    if prev == curr:
+      return
+    if curr == SpeedLimitAssistState.preActive:
+      self.pending_events.append(EventNameIQ.speedLimitPreActive)
+    elif curr in (SpeedLimitAssistState.adapting, SpeedLimitAssistState.active):
+      if prev not in (SpeedLimitAssistState.adapting, SpeedLimitAssistState.active):
+        self.pending_events.append(EventNameIQ.speedLimitActive)
+
+  def _reset_confirmed(self):
+    self.target = 0.0
+    self.source = "None"
+
+  def _reset_unconfirmed(self):
+    self.unconfirmed_limit = 0.0
+    self.unconfirmed_source = "None"
+
+
+class SpeedLimitController:
+  def __init__(self, params):
+    self.params = params
+    self._resolver = IQSpeedLimitResolver()
+    self._assist = IQSpeedLimitAssist(params)
+
+    self.calling_mapbox = False
+    self.mapbox_limit = 0.0
+    self.segment_distance = 0.0
+
+    self.gps_valid = False
+    self.gps_position = {"bearing": 0, "latitude": 0, "longitude": 0}
+
+    self.override_slc = False
+    self.overridden_speed = 0.0
+
+    self._resolved_limit = 0.0
+    self._resolved_source = "None"
+    self._czone_was_limiting = False
+
+    self.pending_events = []
+
+    mapbox_requests_raw = self.params.get("MapBoxRequests")
+    if isinstance(mapbox_requests_raw, dict):
+      self.mapbox_requests = mapbox_requests_raw
+    elif mapbox_requests_raw is not None:
+      try:
+        raw = mapbox_requests_raw
+        if isinstance(raw, bytes):
+          self.mapbox_requests = json.loads(raw.decode("utf-8"))
+        elif isinstance(raw, str):
+          self.mapbox_requests = json.loads(raw)
+        else:
+          self.mapbox_requests = {}
+      except (json.JSONDecodeError, AttributeError, TypeError):
+        self.mapbox_requests = {}
+    else:
+      self.mapbox_requests = {}
+    self.mapbox_requests.setdefault("total_requests", 0)
+    self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100))
+
+    self.mapbox_host = "https://api.mapbox.com"
+    self.mapbox_token = self.params.get("MapboxToken")
+    if self.mapbox_token is not None and isinstance(self.mapbox_token, bytes):
+      self.mapbox_token = self.mapbox_token.decode("utf-8")
+
+    previous_limit = self.params.get("PreviousSpeedLimit")
+    if previous_limit is not None:
+      try:
+        val = previous_limit
+        self._assist.previous_target = float(val.decode("utf-8") if isinstance(val, bytes) else val)
+      except (ValueError, AttributeError):
+        pass
+
+    self.executor = ThreadPoolExecutor(max_workers=1)
+    self._offset_cache = {}
+    self._offset_cache_t = 0.0
+    self._last_mapbox_log_t = 0.0
+    self._last_mapbox_diag_t = 0.0
+    self._last_mapbox_diag_message = None
+
+    self.session = requests.Session() if requests is not None else None
+    if self.session is not None:
+      self.session.headers.update({"Accept-Language": "en"})
+      self.session.headers.update({"User-Agent": "iqpilot-mapbox-speed-limit-retriever/1.0"})
+
+    self.tomtom_host = "https://api.tomtom.com"
+    self.tomtom_token = self._resolve_tomtom_token()
+    self.tomtom_limit = 0.0
+    self.tomtom_segment_distance = 0.0
+    self.calling_tomtom = False
+    self.tomtom_consecutive_failures = 0
+    self.tomtom_backoff_until = 0.0
+
+  def _resolve_tomtom_token(self) -> str:
+    try:
+      from openpilot.iqpilot.navd.runtime_common import resolve_tomtom_token
+      return resolve_tomtom_token(self.params) or ""
+    except Exception:
+      tok = self.params.get("TomTomToken")
+      return (tok.decode("utf-8") if isinstance(tok, bytes) else (tok or "")).strip()
+
+  @property
+  def target(self):
+    return self._assist.target
+
+  @property
+  def source(self):
+    return self._assist.source
+
+  @property
+  def active_target(self):
+    return self._resolved_limit
+
+  @property
+  def active_source(self):
+    return self._resolved_source
+
+  @property
+  def unconfirmed_speed_limit(self):
+    return self._assist.unconfirmed_limit
+
+  @property
+  def map_speed_limit(self):
+    return self._resolver.map_speed_limit
+
+  @property
+  def next_speed_limit(self):
+    return self._resolver.next_speed_limit
+
+  @property
+  def assist_state(self):
+    return self._assist.state
+
+  @property
+  def output_a_target(self):
+    return self._assist.output_a_target
+
+  def get_offset(self, is_metric):
+    target = self._assist.target
+    # offsets only apply to real limit sources: fallback set-speed publishes "None",
+    # construction clamps must never be inflated
+    if target <= 0 or self._assist.source in ("None", "Construction"):
+      return 0.0
+    offset_map = OFFSET_MAP_METRIC if is_metric else OFFSET_MAP_IMPERIAL
+    for low, high, offset_param in offset_map:
+      if low <= target < high:
+        percent = float(np.clip(self._get_offset_percent(offset_param), -OFFSET_PERCENT_MAX, OFFSET_PERCENT_MAX))
+        return target * percent / 100.0
+    return 0.0
+
+  def _get_offset_percent(self, offset_param):
+    now_mono = time.monotonic()
+    if now_mono - self._offset_cache_t >= 5.0:
+      self._offset_cache.clear()
+      self._offset_cache_t = now_mono
+    if offset_param not in self._offset_cache:
+      offset_value = self.params.get(offset_param)
+      try:
+        if isinstance(offset_value, bytes):
+          offset_value = offset_value.decode("utf-8")
+        self._offset_cache[offset_param] = float(offset_value) if offset_value is not None else 0.0
+      except (ValueError, TypeError):
+        self._offset_cache[offset_param] = 0.0
+    return self._offset_cache[offset_param]
+
+  @staticmethod
+  def _is_alive(sm, key):
+    if hasattr(sm, "alive"):
+      return bool(sm.alive.get(key, False))
+    return False
+
+  def update_gps(self, sm):
+    iq_loc_valid = False
+    iq_loc = None
+    if self._is_alive(sm, "iqLiveLocation"):
+      iq_loc = sm["iqLiveLocation"]
+      iq_loc_valid = bool(getattr(iq_loc, "gpsHealthy", False))
+
+    if self._is_alive(sm, "gpsLocationExternal"):
+      gps_location = sm["gpsLocationExternal"]
+    elif self._is_alive(sm, "gpsLocation"):
+      gps_location = sm["gpsLocation"]
+    else:
+      gps_location = None
+
+    gps_has_fix = False
+    if gps_location is not None:
+      gps_has_fix = bool(getattr(gps_location, "hasFix", False))
+      gps_has_fix |= bool(getattr(gps_location, "flags", 0) > 0)
+
+    if gps_location and (gps_has_fix or iq_loc_valid):
+      self.gps_valid = True
+      self.gps_position = {
+        "bearing": getattr(gps_location, "bearingDeg", 0),
+        "latitude": getattr(gps_location, "latitude", 0),
+        "longitude": getattr(gps_location, "longitude", 0),
+      }
+    elif iq_loc_valid and iq_loc is not None and getattr(iq_loc, "geodeticPosition", None) and iq_loc.geodeticPosition.isValid:
+      self.gps_valid = True
+      self.gps_position = {
+        "bearing": math.degrees(iq_loc.alignedOrientationNed.values[2]) if getattr(iq_loc, "alignedOrientationNed", None) else 0,
+        "latitude": iq_loc.geodeticPosition.values[0],
+        "longitude": iq_loc.geodeticPosition.values[1],
+      }
+    else:
+      self.gps_valid = False
+
+  def _log_mapbox_diag(self, message, force=False):
+    now_mono = time.monotonic()
+    if not force and message == self._last_mapbox_diag_message and now_mono - self._last_mapbox_diag_t < 5.0:
+      return
+    if not force and now_mono - self._last_mapbox_diag_t < 2.0:
+      return
+    self._last_mapbox_diag_t = now_mono
+    self._last_mapbox_diag_message = message
+    cloudlog.info(message)
+    k3_slc_log(message)
+
+  def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm):
+    if requests is None or self.session is None:
+      self._log_mapbox_diag("SLC Mapbox skipped: requests session unavailable")
+      self.mapbox_limit = 0.0
+      self.segment_distance = 0.0
+      return
+
+    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    if not self.gps_valid or not self.mapbox_token or steer_angle >= 45:
+      self._log_mapbox_diag(f"SLC Mapbox skipped: gps_valid={self.gps_valid} token={bool(self.mapbox_token)} steer_angle={round(float(steer_angle), 2)}")
+      self.mapbox_limit = 0.0
+      self.segment_distance = 0.0
+      return
+
+    if v_ego < 1:
+      return
+
+    if self.segment_distance > 0:
+      self.segment_distance -= v_ego * DT_MDL
+      return
+
+    if self.calling_mapbox:
+      self.segment_distance = v_ego
+      return
+
+    def make_request():
+      try:
+        self.calling_mapbox = True
+        successful = False
+
+        if not is_url_pingable(self.mapbox_host):
+          self._log_mapbox_diag("SLC Mapbox skipped: host not pingable", force=True)
+          self.segment_distance = 1000
+          return None
+
+        if time_validated:
+          current_month = now.month
+          if current_month != self.mapbox_requests.get("month"):
+            self.mapbox_requests.update(
+              {
+                "month": current_month,
+                "total_requests": 0,
+                "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100,
+              }
+            )
+
+        self.mapbox_requests["total_requests"] += 1
+        self.params.put_nonblocking("MapBoxRequests", self.mapbox_requests)
+
+        lat = self.gps_position.get("latitude")
+        lon = self.gps_position.get("longitude")
+        bearing = self.gps_position.get("bearing")
+        future_lat, future_lon = calculate_bearing_offset(lat, lon, bearing, v_ego)
+
+        self._log_mapbox_diag(
+          f"SLC Mapbox request: lat={round(float(lat), 6)} lon={round(float(lon), 6)} bearing={round(float(bearing), 2)} v_ego={round(float(v_ego), 2)}",
+          force=True,
+        )
+
+        url = f"{self.mapbox_host}/matching/v5/mapbox/driving/{lon},{lat};{future_lon},{future_lat}.json"
+        mapbox_params = {
+          "access_token": self.mapbox_token,
+          "annotations": "maxspeed,distance",
+          "geometries": "polyline6",
+          "overview": "full",
+          "steps": "false",
+          "radiuses": "10;10",
+          "tidy": "true",
+        }
+
+        response = self.session.get(url, params=mapbox_params, timeout=10)
+        response.raise_for_status()
+        successful = True
+        return response.json()
+      except Exception as exception:
+        now_mono = time.monotonic()
+        if now_mono - self._last_mapbox_log_t >= 5.0:
+          self._last_mapbox_log_t = now_mono
+          msg = f"SLC Mapbox request failed: {exception}"
+          cloudlog.warning(msg)
+          k3_slc_log(msg)
+      finally:
+        self.calling_mapbox = False
+        if not successful:
+          self.mapbox_limit = 0.0
+          self.segment_distance = v_ego
+
+    def complete_request(future):
+      try:
+        data = future.result()
+        if data:
+          matchings = data.get("matchings") or []
+          if not matchings:
+            self.mapbox_limit = 0.0
+            self.segment_distance = v_ego
+            return
+          legs = (matchings[0] or {}).get("legs") or []
+          if not legs:
+            self.mapbox_limit = 0.0
+            self.segment_distance = v_ego
+            return
+          annotation = legs[0].get("annotation") or {}
+          distances = annotation.get("distance") or [v_ego]
+          segment_distance = distances[0]
+          speed_data = annotation.get("maxspeed", [])
+          speed_limit_kph = 0
+          if speed_data:
+            first = speed_data[0]
+            speed_limit_kph = (first.get("speed") if first.get("speed") != "none" else 0) or 0
+          if speed_limit_kph > 0:
+            self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS
+            self.segment_distance = segment_distance
+            self._log_mapbox_diag(
+              f"SLC Mapbox callback: speed_limit_kph={round(float(speed_limit_kph), 2)} segment_distance={round(float(segment_distance), 2)}",
+              force=True,
+            )
+            return
+        self.mapbox_limit = 0.0
+        self.segment_distance = v_ego
+      except Exception as exception:
+        now_mono = time.monotonic()
+        if now_mono - self._last_mapbox_log_t >= 5.0:
+          self._last_mapbox_log_t = now_mono
+          msg = f"SLC Mapbox callback failed: {exception}"
+          cloudlog.warning(msg)
+          k3_slc_log(msg)
+        self.mapbox_limit = 0.0
+        self.segment_distance = v_ego
+
+    future = self.executor.submit(make_request)
+    future.add_done_callback(complete_request)
+
+  def get_tomtom_speed_limit(self, now, time_validated, v_ego, sm):
+    if requests is None or self.session is None or not self.tomtom_token:
+      self.tomtom_limit = 0.0
+      self.tomtom_segment_distance = 0.0
+      return
+
+    # backoff: an exhausted-quota key (HTTP 403 InsufficientFunds) otherwise gets
+    # hammered every 250 m for the rest of the drive
+    if time.monotonic() < self.tomtom_backoff_until:
+      self.tomtom_limit = 0.0
+      return
+
+    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    if not self.gps_valid or steer_angle >= 45 or v_ego < 1:
+      self.tomtom_limit = 0.0
+      return
+
+    # re-query at most once per ~250 m of travel
+    if self.tomtom_segment_distance > 0:
+      self.tomtom_segment_distance -= v_ego * DT_MDL
+      return
+    if self.calling_tomtom:
+      self.tomtom_segment_distance = v_ego
+      return
+
+    lat = self.gps_position.get("latitude")
+    lon = self.gps_position.get("longitude")
+    bearing = self.gps_position.get("bearing")
+    future_lat, future_lon = calculate_bearing_offset(lat, lon, bearing, max(v_ego, 12.0) * 12.0)
+
+    def make_request():
+      successful = False
+      try:
+        self.calling_tomtom = True
+        url = f"{self.tomtom_host}/routing/1/calculateRoute/{lat},{lon}:{future_lat},{future_lon}/json"
+        self._log_mapbox_diag(
+          f"SLC TomTom request: lat={round(float(lat), 6)} lon={round(float(lon), 6)} bearing={round(float(bearing), 2)} v_ego={round(float(v_ego), 2)}",
+          force=True,
+        )
+        response = self.session.get(url, params={"key": self.tomtom_token, "sectionType": "speedLimit", "traffic": "false"}, timeout=10)
+        response.raise_for_status()
+        successful = True
+        self.tomtom_consecutive_failures = 0
+        return response.json()
+      except Exception as exception:
+        status = getattr(getattr(exception, "response", None), "status_code", None)
+        if status in (401, 403, 429):
+          # dead/exhausted key: retry hourly in case credits refill, not every 250 m
+          self.tomtom_backoff_until = time.monotonic() + 3600.0
+        else:
+          self.tomtom_consecutive_failures += 1
+          self.tomtom_backoff_until = time.monotonic() + min(600.0, 10.0 * (2 ** min(self.tomtom_consecutive_failures, 6)))
+        now_mono = time.monotonic()
+        if now_mono - self._last_mapbox_log_t >= 5.0:
+          self._last_mapbox_log_t = now_mono
+          msg = f"SLC TomTom request failed (backoff {max(0.0, self.tomtom_backoff_until - now_mono):.0f}s): {exception}"
+          cloudlog.warning(msg)
+          k3_slc_log(msg)
+      finally:
+        self.calling_tomtom = False
+        if not successful:
+          self.tomtom_limit = 0.0
+          self.tomtom_segment_distance = v_ego
+
+    def complete_request(future):
+      try:
+        data = future.result()
+        kmh = 0
+        if data:
+          sections = ((data.get("routes") or [{}])[0]).get("sections") or []
+          speed_secs = [s for s in sections if s.get("sectionType") == "SPEED_LIMIT"]
+          at_start = next((s for s in speed_secs if s.get("startPointIndex") == 0), None)
+          chosen = at_start or (speed_secs[0] if speed_secs else None)
+          if chosen:
+            kmh = chosen.get("maxSpeedLimitInKmh") or 0
+        if kmh and kmh > 0:
+          self.tomtom_limit = float(kmh) * CV.KPH_TO_MS
+          self._log_mapbox_diag(
+            f"SLC TomTom callback: speed_limit_kph={round(float(kmh), 2)}",
+            force=True,
+          )
+        else:
+          self.tomtom_limit = 0.0
+      except Exception as exception:
+        now_mono = time.monotonic()
+        if now_mono - self._last_mapbox_log_t >= 5.0:
+          self._last_mapbox_log_t = now_mono
+          cloudlog.warning(f"SLC TomTom callback failed: {exception}")
+        self.tomtom_limit = 0.0
+      finally:
+        self.tomtom_segment_distance = 250.0
+
+    future = self.executor.submit(make_request)
+    future.add_done_callback(complete_request)
+
+  def _construction_zone_limit(self, sm, slc_params):
+    if not slc_params.get("construction_zone_assist", False):
+      return 0.0
+    if not self._is_alive(sm, "iqConstructionZone"):
+      return 0.0
+    if not bool(getattr(sm["iqConstructionZone"], "active", False)):
+      return 0.0
+    speed = slc_params.get("construction_zone_speed", 60.0)
+    unit = CV.KPH_TO_MS if slc_params.get("is_metric", False) else CV.MPH_TO_MS
+    return max(float(speed), 0.0) * unit
+
+  def _maybe_reset_mapbox_quota(self, now, time_validated):
+    if time_validated:
+      current_month = now.month
+      if current_month != self.mapbox_requests.get("month"):
+        self.mapbox_requests.update(
+          {
+            "month": current_month,
+            "total_requests": 0,
+            "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100,
+          }
+        )
+        self.params.put_nonblocking("MapBoxRequests", self.mapbox_requests)
+
+  def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params):
+    self.update_gps(sm)
+
+    lookahead_lower = slc_params.get("map_speed_lookahead_lower", 5.0)
+    lookahead_higher = slc_params.get("map_speed_lookahead_higher", 5.0)
+    self._resolver.update_map_data(v_ego, sm, lookahead_lower, lookahead_higher)
+
+    use_online = slc_params.get("slc_online_filler", False)
+    if use_online:
+      self._maybe_reset_mapbox_quota(now, time_validated)
+      if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"]:
+        self.get_mapbox_speed_limit(now, time_validated, v_ego, sm)
+      else:
+        self.mapbox_limit = 0.0
+        self.segment_distance = 0.0
+      self.get_tomtom_speed_limit(now, time_validated, v_ego, sm)
+    else:
+      self.mapbox_limit = 0.0
+      self.tomtom_limit = 0.0
+      self.segment_distance = 0.0
+      self.tomtom_segment_distance = 0.0
+
+    online_limit = self.tomtom_limit if self.tomtom_limit > 0 else self.mapbox_limit
+
+    dashboard_limit = float(dashboard_speed_limit) if dashboard_speed_limit else 0.0
+    resolved_limit, resolved_source = self._resolver.resolve(dashboard_limit, online_limit, slc_params)
+
+    enabled = bool(getattr(sm["selfdriveState"], "enabled", False))
+    if resolved_limit <= 0:
+      if self._assist.denied_target != self._assist.previous_target > 0 and slc_params.get("slc_fallback_previous_speed_limit", False):
+        resolved_limit = self._assist.previous_target
+        resolved_source = self._assist.previous_source
+      elif enabled and slc_params.get("slc_fallback_set_speed", False):
+        resolved_limit = v_cruise
+        resolved_source = "None"
+
+    # work-zone clamp: only ever lowers the resolved limit
+    czone_limit = self._construction_zone_limit(sm, slc_params)
+    if czone_limit > 0 and (resolved_limit <= 0 or resolved_limit > czone_limit):
+      resolved_limit = czone_limit
+      resolved_source = "Construction"
+
+    self._resolved_limit = float(resolved_limit)
+    self._resolved_source = resolved_source
+
+    self._assist.update(enabled, v_ego, resolved_limit, resolved_source, slc_params, sm)
+
+    if self._assist.just_confirmed:
+      self.overridden_speed = 0.0
+
+    self.pending_events = list(self._assist.pending_events)
+
+    czone_limiting = resolved_source == "Construction"
+    if czone_limiting and not self._czone_was_limiting:
+      self.pending_events.append(EventNameIQ.constructionZoneDetected)
+    self._czone_was_limiting = czone_limiting
+
+  def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, slc_params, is_metric):
+    offset = self.get_offset(is_metric)
+    target = self._assist.target
+
+    self.override_slc = self.overridden_speed > target + offset > 0
+    self.override_slc |= sm["carState"].gasPressed and v_ego > target + offset > 0
+    self.override_slc &= bool(getattr(sm["selfdriveState"], "enabled", False))
+
+    if self.override_slc:
+      if slc_params.get("speed_limit_controller_override_manual", False):
+        if sm["carState"].gasPressed:
+          self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
+        self.overridden_speed = float(np.clip(self.overridden_speed, target + offset, v_cruise + v_cruise_diff))
+      elif slc_params.get("speed_limit_controller_override_set_speed", False):
+        self.overridden_speed = v_cruise + v_cruise_diff
+    else:
+      self.overridden_speed = 0.0

iqpilot/selfdrive/controls/lib/tests/test_custom_stop_distance.py

@@ -0,0 +1,118 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
+"""
+from types import SimpleNamespace
+
+from iqdbc.car.interfaces import ACCEL_MIN
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import (
+  CustomStopDistance,
+  MIN_ADJUSTED_D_REL,
+)
+
+
+def _build(distance):
+  c = CustomStopDistance.__new__(CustomStopDistance)
+  c.frame = 0
+  c.distance = float(distance)
+  return c
+
+
+def _model_msg(stop_distance, end_velocity):
+  x = [0.0] * (ModelConstants.IDX_N - 1) + [stop_distance]
+  v = [0.0] * (ModelConstants.IDX_N - 1) + [end_velocity]
+  return SimpleNamespace(position=SimpleNamespace(x=x), velocity=SimpleNamespace(x=v))
+
+
+def test_zero_distance_is_a_no_op():
+  c = _build(0)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  assert c.apply_lead(dict(lead)) == lead
+
+
+def test_positive_distance_reduces_reported_lead_distance():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 8.0
+
+
+def test_negative_distance_increases_reported_lead_distance():
+  c = _build(-2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 12.0
+
+
+def test_positive_distance_never_reports_below_floor():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 1.5, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == MIN_ADJUSTED_D_REL
+
+
+def test_positive_distance_never_reports_further_than_reality():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 0.5, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 0.5
+
+
+def test_offset_fades_out_as_lead_speeds_up():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 3.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 10.0
+
+
+def test_no_lead_is_untouched():
+  c = _build(2)
+  lead = {'status': False, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 10.0
+
+
+def test_e2e_negative_distance_is_a_no_op():
+  c = _build(-2)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_zero_distance_is_a_no_op():
+  c = _build(0)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_stop_sign_plans_are_untouched():
+  c = _build(2)
+  # model plan still moving at the end -> proceeding through (stop sign), not held
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 5.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_holds_short_of_model_stop_when_already_stopped():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=3.0, end_velocity=0.0))
+  assert should_stop is True
+
+
+def test_e2e_does_not_hold_once_past_offset_and_buffer():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert should_stop is False
+
+
+def test_e2e_deepens_braking_already_in_progress():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert a_target < -0.5
+  assert a_target >= ACCEL_MIN
+
+
+def test_e2e_never_relaxes_braking():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert a_target == 0.0

iqpilot/selfdrive/controls/lib/tests/test_iq_dynamic_force_stop.py

@@ -0,0 +1,62 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from types import SimpleNamespace
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerIQ
+
+
+class _FakeIQDynamic:
+  def __init__(self, requested=True, model_length=4.0, model_stop_time=5.0, minimum_force_stop_length=15.0):
+    self._requested = requested
+    self.model_length = model_length
+    self.model_stop_time = model_stop_time
+    self.minimum_force_stop_length = minimum_force_stop_length
+
+  def force_stop_requested(self):
+    return self._requested
+
+
+def _build_planner(iq_dynamic):
+  planner = LongitudinalPlannerIQ.__new__(LongitudinalPlannerIQ)
+  planner.iq_dynamic = iq_dynamic
+  planner.force_stop_timer = 0.0
+  planner.forcing_stop = False
+  planner.override_force_stop = False
+  planner.override_force_stop_timer = 0.0
+  planner.tracked_model_length = 0.0
+  return planner
+
+
+def _build_sm(gas_pressed=False, accel_pressed=False, standstill=False):
+  return {
+    "carState": SimpleNamespace(gasPressed=gas_pressed, standstill=standstill),
+    "iqCarState": SimpleNamespace(accelPressed=accel_pressed),
+  }
+
+
+def test_force_stop_uses_model_stop_time_as_ramp():
+  planner = _build_planner(_FakeIQDynamic(model_length=20.0, model_stop_time=5.0, minimum_force_stop_length=0.0))
+  sm = _build_sm()
+
+  output = 12.0
+  for _ in range(int(1.0 / DT_MDL)):
+    output = planner._apply_force_stop(12.0, 0.0, sm, True)
+
+  assert planner.forcing_stop
+  assert output == 4.0
+
+
+def test_force_stop_respects_minimum_force_stop_length():
+  planner = _build_planner(_FakeIQDynamic(model_length=4.0, model_stop_time=5.0, minimum_force_stop_length=15.0))
+  sm = _build_sm()
+
+  output = 12.0
+  for _ in range(int(1.0 / DT_MDL)):
+    output = planner._apply_force_stop(12.0, 0.0, sm, True)
+
+  assert planner.forcing_stop
+  assert planner.tracked_model_length == 15.0
+  assert output == 3.0

iqpilot/selfdrive/controls/lib/tests/test_nav_exit_lane_change.py

@@ -0,0 +1,96 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from cereal import custom, log
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+
+ManeuverType = custom.IQNavState.ManeuverType
+NavDirection = custom.NavDirection
+LaneChangeDirection = log.LaneChangeDirection
+
+
+class DummyCarState:
+  def __init__(self, vEgo=25.0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+               steeringPressed=False, steeringTorque=0, brakePressed=False):
+    self.vEgo = vEgo
+    self.leftBlinker = leftBlinker
+    self.rightBlinker = rightBlinker
+    self.leftBlindspot = leftBlindspot
+    self.rightBlindspot = rightBlindspot
+    self.steeringPressed = steeringPressed
+    self.steeringTorque = steeringTorque
+    self.brakePressed = brakePressed
+
+
+class DummyNavState:
+  def __init__(self, active=True, nextManeuverValid=True, nextManeuverType=int(ManeuverType.exit),
+               nextManeuverDistance=300.0, nextManeuverDirection=int(NavDirection.right)):
+    self.active = active
+    self.nextManeuverValid = nextManeuverValid
+    self.nextManeuverType = nextManeuverType
+    self.nextManeuverDistance = nextManeuverDistance
+    self.nextManeuverDirection = nextManeuverDirection
+
+
+def _make_dh(enabled: bool, enable_bsm: bool):
+  dh = DesireHelper()
+  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  dh.nav_exit._read_enabled = lambda: enabled  # bypass the (unregistered) param in tests
+  dh.nav_exit._enable_bsm = enable_bsm
+  return dh
+
+
+def _run(dh, carstate, nav_state, n=20):
+  for _ in range(n):
+    dh.update(carstate, True, 1.0, nav_state)
+  return dh.desire
+
+
+def test_feature_off_no_exit_lane_change():
+  dh = _make_dh(enabled=False, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+
+
+def test_no_bsm_requires_nudge_holds_without_one():
+  # No blindspot monitor: nav exit must NOT auto-start; without a nudge it stays in preLaneChange.
+  dh = _make_dh(enabled=True, enable_bsm=False)
+  cs = DummyCarState(steeringPressed=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+  assert dh.lane_change_state == LaneChangeState.preLaneChange
+  assert dh.lane_change_direction == LaneChangeDirection.right
+
+
+def test_no_bsm_starts_on_driver_nudge():
+  # Driver nudges the wheel toward the exit (right -> negative torque) -> lane change starts.
+  dh = _make_dh(enabled=True, enable_bsm=False)
+  cs = DummyCarState(steeringPressed=True, steeringTorque=-1)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.laneChangeRight
+
+
+def test_bsm_auto_starts_when_clear():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.laneChangeRight
+
+
+def test_bsm_holds_when_blindspot_occupied():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=True)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+
+
+def test_only_exit_maneuvers_trigger():
+  # A turn maneuver (not an exit) must not trigger the exit lane change.
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  nav = DummyNavState(nextManeuverType=int(ManeuverType.turn))
+  assert _run(dh, cs, nav) == log.Desire.none
+
+
+def test_too_far_does_not_trigger():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  nav = DummyNavState(nextManeuverDistance=900.0)
+  assert _run(dh, cs, nav) == log.Desire.none

iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py

@@ -0,0 +1,463 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from datetime import datetime
+from types import SimpleNamespace
+
+from openpilot.common.constants import CV
+from openpilot.iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
+from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
+from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
+
+
+class FakeParams:
+  def __init__(self):
+    self.values = {}
+
+  def get(self, key, encoding=None):
+    _ = encoding
+    return self.values.get(key)
+
+  def get_bool(self, key):
+    return bool(self.values.get(key, False))
+
+  def put_nonblocking(self, key, value):
+    self.values[key] = value
+
+  def put(self, key, value):
+    self.values[key] = value
+
+
+def _build_sm(v_cruise_cluster=100.0, v_ego_cluster=27.8, gas=False, enabled=True, iq_limit=0.0):
+  # vCruiseCluster is in kph in carState.
+  return {
+    "carState": SimpleNamespace(vCruiseCluster=v_cruise_cluster, vEgoCluster=v_ego_cluster, gasPressed=gas,
+                                steeringAngleDeg=0.0, buttonEvents=[]),
+    "iqCarState": SimpleNamespace(speedLimit=iq_limit, accelPressed=False, decelPressed=False),
+    "selfdriveState": SimpleNamespace(enabled=enabled),
+    "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
+  }
+
+
+class _FakeSLC:
+  def __init__(self):
+    self.target = 0.0
+    self.source = "None"
+    self.active_target = 0.0
+    self.active_source = "None"
+    self.unconfirmed_speed_limit = 0.0
+    self.overridden_speed = 0.0
+    self.pending_events = []
+    self.assist_state = None
+    self.output_a_target = 0.0
+    self.update_limits_calls = 0
+    self.update_override_calls = 0
+    self._offset = 0.0
+
+  def update_limits(self, *_args, **_kwargs):
+    self.update_limits_calls += 1
+
+  def update_override(self, *_args, **_kwargs):
+    self.update_override_calls += 1
+
+  def get_offset(self, _is_metric):
+    return self._offset
+
+
+def _base_slc_params_controller():
+  return {
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "slc_fallback_previous_speed_limit": False,
+    "slc_fallback_set_speed": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "slc_online_filler": True,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+  }
+
+
+def test_speed_limit_controller_resolves_source_by_priority():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  controller.mapbox_limit = 22.0
+  controller._resolver.map_speed_limit = 18.0  # map data wins in map_data_priority policy
+
+  sm = _build_sm(iq_limit=25.0)
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_policy"] = POLICY_MAP_DATA_PRIORITY
+
+  controller.update_limits(25.0, datetime.now(), True, 30.0, 27.0, sm, slc_params)
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 18.0
+
+
+def test_speed_limit_controller_combined_mode_prefers_smallest_limit():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  controller.mapbox_limit = 24.0
+  controller._resolver.map_speed_limit = 16.0  # smallest of: dashboard=28, mapbox=24, map_data=16
+
+  sm = _build_sm(iq_limit=28.0)
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_policy"] = POLICY_COMBINED
+
+  controller.update_limits(28.0, datetime.now(), True, 31.0, 27.0, sm, slc_params)
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 16.0
+
+
+def test_slc_vcruise_applies_target_without_increasing_cruise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 23.0
+  slc.slc.source = "Dashboard"
+  slc.slc.active_target = 23.0
+  slc.slc.active_source = "Dashboard"
+  slc.slc._offset = 1.0
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 30.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=27.0, iq_limit=23.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=27.0, sm=sm)
+
+  assert slc.slc.update_limits_calls == 1
+  assert slc.slc.update_override_calls == 1
+  assert out <= v_cruise
+  assert out >= CRUISING_SPEED
+
+
+def test_slc_vcruise_show_only_does_not_modify_cruise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 21.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 21.0
+  slc.slc.active_source = "Map Data"
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": False,
+    "speed_limit_mode": 1,
+    "show_speed_limits": True,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 29.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=26.0, iq_limit=21.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=26.0, sm=sm)
+
+  assert slc.slc.update_limits_calls == 1
+  assert slc.slc.update_override_calls == 0
+  assert out == v_cruise
+
+
+def test_slc_vcruise_auto_raises_for_higher_limit_when_confirmation_disabled():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 20.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 20.0
+  slc.slc.active_source = "Map Data"
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 13.5
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=13.5, iq_limit=20.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=13.5, sm=sm)
+
+  assert out > v_cruise
+  assert out == 20.0
+
+
+def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 20.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 20.0
+  slc.slc.active_source = "Map Data"
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": True,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 13.5
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=13.5, iq_limit=20.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=13.5, sm=sm)
+
+  assert out == v_cruise
+
+
+class _FakeSM(dict):
+  def __init__(self, services, alive=None):
+    super().__init__(services)
+    self.alive = alive or {}
+
+
+def _construction_sm(active=True, alive=True, iq_limit=0.0):
+  sm = _FakeSM(_build_sm(iq_limit=iq_limit))
+  sm["iqConstructionZone"] = SimpleNamespace(active=active, orangeFraction=0.001, secondsSinceHit=1.0)
+  sm.alive = {"iqConstructionZone": alive}
+  return sm
+
+
+def _construction_controller():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  return controller
+
+
+def _construction_slc_params():
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_online_filler"] = False
+  slc_params["construction_zone_assist"] = True
+  slc_params["construction_zone_speed"] = 60.0
+  slc_params["is_metric"] = False
+  return slc_params
+
+
+def test_construction_zone_clamps_higher_limit():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 31.3  # ~70 mph
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_does_not_raise_lower_limit():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 20.0  # below the 60 mph clamp
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 20.0
+
+
+def test_construction_zone_applies_without_other_sources():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 0.0
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_ignored_when_not_alive_or_inactive_or_disabled():
+  for kwargs, slc_toggle in (
+    (dict(alive=False), True),
+    (dict(active=False), True),
+    (dict(), False),
+  ):
+    controller = _construction_controller()
+    controller._resolver.map_speed_limit = 31.3
+    sm = _construction_sm(**kwargs)
+    slc_params = _construction_slc_params()
+    slc_params["construction_zone_assist"] = slc_toggle
+
+    controller.update_limits(0.0, None, True, 33.0, 30.0, sm, slc_params)
+    assert controller.active_source == "Map Data"
+    assert controller.active_target == 31.3
+
+
+def test_construction_zone_metric_speed_units():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 33.0
+  sm = _construction_sm()
+  slc_params = _construction_slc_params()
+  slc_params["is_metric"] = True
+  slc_params["construction_zone_speed"] = 100.0  # kph
+
+  controller.update_limits(0.0, None, True, 36.0, 33.0, sm, slc_params)
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 100.0 * CV.KPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_never_raises_cruise_even_with_auto_raise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 60.0 * CV.MPH_TO_MS
+  slc.slc.source = "Construction"
+  slc.slc.active_target = slc.slc.target
+  slc.slc.active_source = "Construction"
+  slc.slc._offset = 2.0  # must be ignored for Construction
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": False,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,  # auto-raise allowed
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+    "construction_zone_assist": True,
+    "construction_zone_speed": 60.0,
+  }
+
+  # user cruising below the construction clamp: must not be raised to it
+  v_cruise = 22.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=22.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=22.0, sm=sm)
+  assert out == v_cruise
+  assert slc.slc_offset == 0
+
+  # user cruising above it: clamped down
+  v_cruise = 33.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=33.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=33.0, sm=sm)
+  assert abs(out - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def _offset_controller(pct1=10.0, pct2=5.0, pct3=8.0):
+  params = FakeParams()
+  params.put("speed_limit_offset1", pct1)
+  params.put("speed_limit_offset2", pct2)
+  params.put("speed_limit_offset3", pct3)
+  controller = SpeedLimitController(params)
+  controller._assist.source = "Map Data"
+  return controller
+
+
+def test_get_offset_percent_per_zone():
+  controller = _offset_controller()
+
+  controller._assist.target = 6.7  # ~15 mph -> zone 1
+  assert abs(controller.get_offset(False) - 6.7 * 0.10) < 1e-9
+
+  controller._assist.target = 13.4  # ~30 mph -> zone 2
+  assert abs(controller.get_offset(False) - 13.4 * 0.05) < 1e-9
+
+  controller._assist.target = 31.3  # ~70 mph -> zone 3 (open-ended)
+  assert abs(controller.get_offset(False) - 31.3 * 0.08) < 1e-9
+
+
+def test_get_offset_zone_lower_bound_inclusive():
+  controller = _offset_controller()
+  boundary = OFFSET_MAP_IMPERIAL[1][0]
+  controller._assist.target = boundary
+  assert abs(controller.get_offset(False) - boundary * 0.05) < 1e-9
+
+
+def test_get_offset_zero_without_real_limit_source():
+  for source in ("None", "Construction"):
+    controller = _offset_controller()
+    controller._assist.source = source
+    controller._assist.target = 30.0
+    assert controller.get_offset(False) == 0.0
+
+
+def test_get_offset_percent_clamped():
+  controller = _offset_controller(pct3=500.0)
+  controller._assist.target = 30.0
+  assert abs(controller.get_offset(False) - 30.0 * 0.50) < 1e-9
+
+
+def test_construction_zone_fires_event_once_per_zone_entry():
+  from cereal import custom
+  event = custom.IQOnroadEvent.EventName.constructionZoneDetected
+
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 31.3
+  slc_params = _construction_slc_params()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
+  assert event in controller.pending_events
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
+  assert event not in controller.pending_events
+
+  # zone releases, then a new zone: fires again
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(active=False), slc_params)
+  assert event not in controller.pending_events
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
+  assert event in controller.pending_events

iqpilot/selfdrive/controls/lib/tests/test_smooth_stops.py

@@ -0,0 +1,100 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
+"""
+from openpilot.common.realtime import DT_CTRL
+from openpilot.iqpilot.selfdrive.controls.lib.smooth_stops import (
+  SmoothStopController,
+  read_smooth_stops_enabled,
+  STANDSTILL_SPEED,
+  STANDSTILL_HOLD_SPEED,
+  SETTLE_DECEL,
+  TAPER_SPEED,
+  STOP_KISS_DECEL,
+  SETTLE_JERK,
+  EMERGENCY_DECEL,
+)
+
+JERK_STEP = SETTLE_JERK * DT_CTRL
+
+
+def _build(enabled=True):
+  c = SmoothStopController.__new__(SmoothStopController)
+  c.enabled = enabled
+  c._v_min = float("inf")
+  c._stall_s = 0.0
+  return c
+
+
+def test_unified_toggle_reads_force_stops():
+  seen = {}
+
+  class FakeParams:
+    def get_bool(self, key):
+      seen["key"] = key
+      return True
+
+  assert read_smooth_stops_enabled(FakeParams()) is True
+  assert seen["key"] == "IQForceStops"
+
+
+def test_hold_only_arms_at_standstill():
+  c = _build()
+  assert not c.want_hold(True, 0.5, False)
+  assert not c.want_hold(True, STANDSTILL_SPEED + 0.05, False)
+  assert not c.want_hold(True, 1.0, True)
+  assert not c.want_hold(True, STANDSTILL_HOLD_SPEED + 0.05, True)
+  assert c.want_hold(True, STANDSTILL_SPEED - 0.01, False)
+  assert c.want_hold(True, STANDSTILL_HOLD_SPEED - 0.01, True)
+  assert not c.want_hold(False, 0.0, True)
+
+
+def test_settle_feathers_toward_baseline():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -JERK_STEP
+
+
+def test_settle_never_softer_than_mpc():
+  c = _build()
+  out = c.settle(a_target=-2.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=-1.0)
+  assert out == -1.0 - JERK_STEP
+  assert out < -1.0
+
+
+def test_settle_emergency_bypasses_jerk_limit():
+  c = _build()
+  out = c.settle(a_target=-3.4, v_ego=2.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -3.4
+  assert out <= -EMERGENCY_DECEL
+
+
+def test_settle_lead_firms_up_when_close():
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=50.0, has_lead=True, last_output=-SETTLE_DECEL) == -SETTLE_DECEL
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=3.0, has_lead=True, last_output=-1.0) == -1.0
+
+
+def test_settle_anti_creep_firms_up_when_not_slowing():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=-SETTLE_DECEL)
+  for _ in range(60):
+    out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=out)
+  assert out < -SETTLE_DECEL
+
+
+def test_settle_eases_off_near_stop():
+  c = _build()
+  near = c.settle(a_target=0.0, v_ego=0.1, lead_distance=0.0, has_lead=False, last_output=-0.305)
+  c = _build()
+  high = c.settle(a_target=0.0, v_ego=0.9, lead_distance=0.0, has_lead=False, last_output=-0.745)
+  assert near > high
+  assert near == -(STOP_KISS_DECEL + (SETTLE_DECEL - STOP_KISS_DECEL) * (0.1 / TAPER_SPEED))
+
+
+def test_settle_kiss_decel_at_stop():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.0, lead_distance=0.0, has_lead=False, last_output=-STOP_KISS_DECEL)
+  assert out == -STOP_KISS_DECEL

iqpilot/selfdrive/iqlocd/SConscript

@@ -0,0 +1,37 @@
+Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
+
+loc_libs = [messaging, common, 'pthread', 'dl']
+
+# build ekf models
+rednose_gen_dir = 'models/generated'
+rednose_gen_deps = [
+  "models/constants.py",
+]
+orbit_filter = env.RednoseCompileFilter(
+  target='orbit',
+  filter_gen_script='models/orbit_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=['orbit_state_constants.h'],
+  gen_script_deps=rednose_gen_deps,
+)
+car_ekf = env.RednoseCompileFilter(
+  target='car',
+  filter_gen_script='models/car_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=[],
+  gen_script_deps=rednose_gen_deps,
+)
+
+# iqlocd build
+iqlocd_sources = ["atlas_loc_core.cc", "models/orbit_kf.cc"]
+
+lenv = env.Clone()
+# ekf filter libraries need to be linked, even if no symbols are used
+if arch != "Darwin":
+  lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
+
+lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
+lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
+iqlocd = lenv.Program("iqlocd", iqlocd_sources, LIBS=["orbit", "ekf_sym"] + loc_libs + transformations)
+lenv.Depends(iqlocd, rednose)
+lenv.Depends(iqlocd, orbit_filter)

iqpilot/selfdrive/iqlocd/atlas_loc_core.cc

@@ -0,0 +1,751 @@
+#include "iqpilot/selfdrive/iqlocd/atlas_loc_core.h"
+
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <algorithm>
+#include <cmath>
+#include <vector>
+
+using namespace EKFS;
+using namespace Eigen;
+
+ExitHandler do_exit;
+const double ACCEL_SANITY_CHECK = 100.0;  // m/s^2
+const double ROTATION_SANITY_CHECK = 10.0;  // rad/s
+const double TRANS_SANITY_CHECK = 200.0;  // m/s
+const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
+const double ALTITUDE_SANITY_CHECK = 10000; // m
+const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
+const double VALID_TIME_SINCE_RESET = 1.0; // s
+const double VALID_POS_STD = 50.0; // m
+const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
+const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
+const double RESET_TRACKER_DECAY = 0.99995;
+const double DECAY = 0.9993; // ~10 secs to resume after a bad input
+const double MAX_FILTER_REWIND_TIME = 0.8; // s
+const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
+
+// TODO: GPS sensor time offsets are empirically calculated
+// They should be replaced with synced time from a real clock
+const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
+const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
+const float  GPS_POS_STD_THRESHOLD = 50.0;
+const float  GPS_VEL_STD_THRESHOLD = 5.0;
+const float  GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
+const float  GPS_POS_STD_RESET_THRESHOLD = 2.0;
+const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
+const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
+const int    GPS_ORIENTATION_ERROR_RESET_CNT = 3;
+
+const bool   DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
+
+static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
+  VectorXd res(floatlist.size());
+  for (int i = 0; i < floatlist.size(); i++) {
+    res[i] = floatlist[i];
+  }
+  return res;
+}
+
+static Vector4d quat2vector(const Quaterniond& quat) {
+  return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
+}
+
+static Quaterniond vector2quat(const VectorXd& vec) {
+  return Quaterniond(vec(0), vec(1), vec(2), vec(3));
+}
+
+static void fill_vector_sample(cereal::IQLiveLocation::VectorSample::Builder sample,
+                               const VectorXd& values, const VectorXd& deviations, bool is_valid) {
+  sample.setValues(kj::arrayPtr(values.data(), values.size()));
+  sample.setDeviations(kj::arrayPtr(deviations.data(), deviations.size()));
+  sample.setIsValid(is_valid);
+}
+
+
+static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
+  // To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
+  return ((rot_matrix *  cov_in) * rot_matrix.transpose());
+}
+
+static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
+  // Stds cannot be rotated like values, only covariances can be rotated
+  return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
+}
+
+AtlasLocator::AtlasLocator(AtlasGnssMode gnss_source) {
+  this->kf = std::make_unique<OrbitKalman>();
+  this->reset_kalman();
+
+  this->calib = Vector3d(0.0, 0.0, 0.0);
+  this->device_from_calib = MatrixXdr::Identity(3, 3);
+  this->calib_from_device = MatrixXdr::Identity(3, 3);
+
+  for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
+    this->posenet_stds.push_back(10.0);
+  }
+
+  VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  this->tune_gnss_source(gnss_source);
+}
+
+void AtlasLocator::populate_location_packet(cereal::IQLiveLocation::Builder& fix) {
+  VectorXd predicted_state = this->kf->get_x();
+  MatrixXdr predicted_cov = this->kf->get_P();
+  VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
+
+  VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
+  VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
+  VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
+  VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
+  VectorXd fix_pos_geo_vec = this->current_geodetic();
+  VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
+  MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
+  VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
+
+  VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
+  MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
+  VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
+  VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
+
+  MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
+  VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
+
+  VectorXd vel_device = device_from_ecef * vel_ecef;
+  VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
+  MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+  condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
+  condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
+  condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
+    predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
+  VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
+  H_input << device_from_ecef_eul, vel_ecef;
+  MatrixXdr HH = this->kf->H(H_input);
+  MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
+  VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
+
+  VectorXd vel_calib = this->calib_from_device * vel_device;
+  VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
+
+  VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
+  VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
+  VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
+  VectorXd nextfix_ecef = fix_ecef + vel_ecef;
+  VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
+
+  VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
+  VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
+
+  VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
+  VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
+
+  Vector3d nans = Vector3d(NAN, NAN, NAN);
+
+  // TODO fill in NED and Calibrated stds
+  // write measurements to msg
+  fill_vector_sample(fix.initGeodeticPosition(), fix_pos_geo_vec, nans, this->gps_mode);
+  fill_vector_sample(fix.initEcefPosition(), fix_ecef, fix_ecef_std, this->gps_mode);
+  fill_vector_sample(fix.initEcefVelocity(), vel_ecef, vel_ecef_std, this->gps_mode);
+  fill_vector_sample(fix.initNedVelocity(), ned_vel, nans, this->gps_mode);
+  fill_vector_sample(fix.initBodyVelocity(), vel_device, vel_device_std, true);
+  fill_vector_sample(fix.initBodyAcceleration(), accDevice, accDeviceErr, true);
+  fill_vector_sample(fix.initEcefOrientation(), orientation_ecef, orientation_ecef_std, this->gps_mode);
+  fill_vector_sample(fix.initAlignedOrientationEcef(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
+  fill_vector_sample(fix.initNedOrientation(), orientation_ned, orientation_ned_std, this->gps_mode);
+  fill_vector_sample(fix.initAlignedOrientationNed(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
+  fill_vector_sample(fix.initBodyAngularRate(), angVelocityDevice, angVelocityDeviceErr, true);
+  fill_vector_sample(fix.initAlignedVelocity(), vel_calib, vel_calib_std, this->calibrated);
+  fill_vector_sample(fix.initAlignedAngularRate(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
+  fill_vector_sample(fix.initAlignedAcceleration(), acc_calib, acc_calib_std, this->calibrated);
+  if (DEBUG) {
+    fill_vector_sample(fix.initDebugState(), predicted_state, predicted_std, true);
+  }
+
+  double old_mean = 0.0, new_mean = 0.0;
+  int i = 0;
+  for (double x : this->posenet_stds) {
+    if (i < POSENET_STD_HIST_HALF) {
+      old_mean += x;
+    } else {
+      new_mean += x;
+    }
+    i++;
+  }
+  old_mean /= POSENET_STD_HIST_HALF;
+  new_mean /= POSENET_STD_HIST_HALF;
+  // experimentally found these values, no false positives in 20k minutes of driving
+  bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
+
+  fix.setVisionHealthy(!(std_spike && this->car_speed > 5.0));
+  fix.setDeviceStable(!this->device_fell);
+  fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
+  fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
+  this->device_fell = false;
+
+  //fix.setGpsWeek(this->time.week);
+  //fix.setGpsTimeOfWeek(this->time.tow);
+  fix.setUnixTimestampMillis(this->unix_timestamp_millis);
+
+  double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
+  fix.setSecondsSinceReset(time_since_reset);
+  if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
+    fix.setSolutionState(cereal::IQLiveLocation::SolutionState::READY);
+  } else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
+    fix.setSolutionState(cereal::IQLiveLocation::SolutionState::COARSE);
+  } else {
+    fix.setSolutionState(cereal::IQLiveLocation::SolutionState::BOOTING);
+  }
+}
+
+VectorXd AtlasLocator::current_geodetic() {
+  VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
+  Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
+  return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
+}
+
+VectorXd AtlasLocator::current_state_vector() {
+  return this->kf->get_x();
+}
+
+VectorXd AtlasLocator::current_sigma_vector() {
+  return this->kf->get_P().diagonal().array().sqrt();
+}
+
+bool AtlasLocator::inputs_are_ready() {
+  return this->critical_services_ok(this->observation_values_invalid) && !this->observation_timings_invalid;
+}
+
+void AtlasLocator::clear_observation_timing_fault(){
+  this->observation_timings_invalid = false;
+}
+
+void AtlasLocator::consume_sensor_frame(double current_time, const cereal::SensorEventData::Reader& log) {
+  // TODO does not yet account for double sensor readings in the log
+
+  // Ignore empty readings (e.g. in case the magnetometer had no data ready)
+  if (log.getTimestamp() == 0) {
+    return;
+  }
+
+  double sensor_time = 1e-9 * log.getTimestamp();
+
+  // sensor time and log time should be close
+  if (std::abs(current_time - sensor_time) > 0.1) {
+    LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
+    this->observation_timings_invalid = true;
+    return;
+  } else if (!this->timestamp_ok(sensor_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  // TODO: handle messages from two IMUs at the same time
+  if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
+    return;
+  }
+
+  // Gyro Uncalibrated
+  if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
+    auto v = log.getGyroUncalibrated().getV();
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+
+    VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
+    float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
+    float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
+    bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
+
+    if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
+      this->observation_values_invalid["gyroscope"] *= DECAY;
+    } else {
+      this->observation_values_invalid["gyroscope"] += 1.0;
+    }
+  }
+
+  // Accelerometer
+  if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
+    auto v = log.getAcceleration().getV();
+
+    // TODO: reduce false positives and re-enable this check
+    // check if device fell, estimate 10 for g
+    // 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
+    // this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
+
+    auto meas = Vector3d(-v[2], -v[1], -v[0]);
+    if (meas.norm() < ACCEL_SANITY_CHECK) {
+      this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
+      this->observation_values_invalid["accelerometer"] *= DECAY;
+    } else {
+      this->observation_values_invalid["accelerometer"] += 1.0;
+    }
+  }
+}
+
+void AtlasLocator::seed_fake_gps_observations(double current_time) {
+  // This is done to make sure that the error estimate of the position does not blow up
+  // when the filter is in no-gps mode
+  // Steps : first predict -> observe current obs with reasonable STD
+  this->kf->predict(current_time);
+
+  VectorXd current_x = this->kf->get_x();
+  VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
+  VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
+  const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+  const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void AtlasLocator::consume_gps_frame(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
+  bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+  bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
+  bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
+  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
+
+  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
+    //this->gps_valid = false;
+    this->refresh_gps_mode(current_time);
+    return;
+  }
+
+  double sensor_time = current_time - sensor_time_offset;
+
+  // Process message
+  //this->gps_valid = true;
+  this->gps_mode = true;
+  Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
+  this->converter = std::make_unique<LocalCoord>(geodetic);
+
+  VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
+  VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
+  float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
+
+  this->unix_timestamp_millis = log.getUnixTimestampMillis();
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
+
+  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
+  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
+  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
+  for (int i = 0; i < orientation_error.size(); i++) {
+    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
+    if (orientation_error(i) < 0.0) {
+      orientation_error(i) += 2.0 * M_PI;
+    }
+    orientation_error(i) -= M_PI;
+  }
+  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
+
+  if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
+    LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+  } else if (gps_est_error > 100.0) {
+    LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+  }
+
+  this->last_gps_msg = sensor_time;
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void AtlasLocator::consume_gnss_frame(double current_time, const cereal::GnssMeasurements::Reader& log) {
+
+  if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
+    this->refresh_gps_mode(current_time);
+    return;
+  }
+
+  double sensor_time = log.getMeasTime() * 1e-9;
+  sensor_time -= this->gps_time_offset;
+
+  auto ecef_pos_v = log.getPositionECEF().getValue();
+  VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_pos_std = log.getPositionECEF().getStd()[0];
+  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
+
+  auto ecef_vel_v = log.getVelocityECEF().getValue();
+  VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
+  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
+
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
+
+  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
+
+  LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
+  VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
+  double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
+
+  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
+  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
+  for (int i = 0; i < orientation_error.size(); i++) {
+    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
+    if (orientation_error(i) < 0.0) {
+      orientation_error(i) += 2.0 * M_PI;
+    }
+    orientation_error(i) -= M_PI;
+  }
+  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
+
+  if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
+    this->refresh_gps_mode(current_time);
+    return;
+  }
+
+  // prevent jumping gnss measurements (covered lots, standstill...)
+  bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
+  orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
+  orientation_reset &= !this->standstill;
+  if (orientation_reset) {
+    this->orientation_reset_count++;
+  } else {
+    this->orientation_reset_count = 0;
+  }
+
+  if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
+    // always reset on first gps message and if the location is off but the accuracy is high
+    LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+  } else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
+    LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+    this->orientation_reset_count = 0;
+  }
+
+  this->gps_mode = true;
+  this->last_gps_msg = sensor_time;
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
+void AtlasLocator::consume_car_state_frame(double current_time, const cereal::CarState::Reader& log) {
+  this->car_speed = std::abs(log.getVEgo());
+  this->standstill = log.getStandstill();
+  if (this->standstill) {
+    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
+    this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
+  }
+}
+
+void AtlasLocator::consume_camera_odometry(double current_time, const cereal::CameraOdometry::Reader& log) {
+  VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
+  VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
+
+  if (!this->timestamp_ok(current_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
+  VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
+
+  if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
+  this->posenet_stds.pop_front();
+  this->posenet_stds.push_back(trans_calib_std[0]);
+
+  // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
+  trans_calib_std *= 10.0;
+  rot_calib_std *= 10.0;
+  MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
+  MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
+  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
+    { rot_device }, { rot_device_cov });
+  this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
+    { trans_device }, { trans_device_cov });
+  this->observation_values_invalid["cameraOdometry"] *= DECAY;
+  this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
+}
+
+void AtlasLocator::consume_live_calibration(double current_time, const cereal::LiveCalibrationData::Reader& log) {
+  if (!this->timestamp_ok(current_time)) {
+    this->observation_timings_invalid = true;
+    return;
+  }
+
+  if (log.getRpyCalib().size() > 0) {
+    auto live_calib = floatlist2vector(log.getRpyCalib());
+    if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
+      this->observation_values_invalid["liveCalibration"] += 1.0;
+      return;
+    }
+
+    this->calib = live_calib;
+    this->device_from_calib = euler2rot(this->calib);
+    this->calib_from_device = this->device_from_calib.transpose();
+    this->calibrated = log.getCalStatus() == cereal::LiveCalibrationData::Status::CALIBRATED;
+    this->observation_values_invalid["liveCalibration"] *= DECAY;
+  }
+}
+
+void AtlasLocator::reset_kalman(double current_time) {
+  const VectorXd &init_x = this->kf->get_initial_x();
+  const MatrixXdr &init_P = this->kf->get_initial_P();
+  this->reset_kalman(current_time, init_x, init_P);
+}
+
+void AtlasLocator::run_finite_guard(double current_time) {
+  bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
+  if (!all_finite) {
+    LOGE("Non-finite values detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
+void AtlasLocator::run_time_guard(double current_time) {
+  if (std::isnan(this->last_reset_time)) {
+    this->last_reset_time = current_time;
+  }
+  if (std::isnan(this->first_valid_log_time)) {
+    this->first_valid_log_time = current_time;
+  }
+  double filter_time = this->kf->get_filter_time();
+  bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
+  if (big_time_gap) {
+    LOGE("Time gap of over 10s detected, kalman reset");
+    this->reset_kalman(current_time);
+  }
+}
+
+void AtlasLocator::cool_reset_tracker() {
+  // reset tracker is tuned to trigger when over 1reset/10s over 2min period
+  if (this->gps_ready()) {
+    this->reset_tracker *= RESET_TRACKER_DECAY;
+  } else {
+    this->reset_tracker = 0.0;
+  }
+}
+
+void AtlasLocator::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
+  // too nonlinear to init on completely wrong
+  VectorXd current_x = this->kf->get_x();
+  MatrixXdr current_P = this->kf->get_P();
+  MatrixXdr init_P = this->kf->get_initial_P();
+  const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
+  int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+
+  current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
+  current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
+  current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
+
+  init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
+  init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
+  init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
+  init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
+    STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
+
+  this->reset_kalman(current_time, current_x, init_P);
+}
+
+void AtlasLocator::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
+  this->kf->init_state(init_x, init_P, current_time);
+  this->last_reset_time = current_time;
+  this->reset_tracker += 1.0;
+}
+
+void AtlasLocator::consume_bytes(const char *data, const size_t size) {
+  AlignedBuffer aligned_buf;
+
+  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
+  cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+  this->consume_event(event);
+}
+
+void AtlasLocator::consume_event(const cereal::Event::Reader& log) {
+  double t = log.getLogMonoTime() * 1e-9;
+  this->run_time_guard(t);
+  if (log.isAccelerometer()) {
+    this->consume_sensor_frame(t, log.getAccelerometer());
+  } else if (log.isGyroscope()) {
+    this->consume_sensor_frame(t, log.getGyroscope());
+  } else if (log.isGpsLocation()) {
+    this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+  } else if (log.isGpsLocationExternal()) {
+    this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+  //} else if (log.isGnssMeasurements()) {
+  //  this->consume_gnss_frame(t, log.getGnssMeasurements());
+  } else if (log.isCarState()) {
+    this->consume_car_state_frame(t, log.getCarState());
+  } else if (log.isCameraOdometry()) {
+    this->consume_camera_odometry(t, log.getCameraOdometry());
+  } else if (log.isLiveCalibration()) {
+    this->consume_live_calibration(t, log.getLiveCalibration());
+  }
+  this->run_finite_guard();
+  this->cool_reset_tracker();
+}
+
+kj::ArrayPtr<capnp::byte> AtlasLocator::pack_state_message(MessageBuilder& msg_builder, bool inputsOK,
+                                                       bool sensorsOK, bool gpsOK, bool msgValid) {
+  cereal::Event::Builder evt = msg_builder.initEvent();
+  evt.setValid(msgValid);
+  cereal::IQLiveLocation::Builder iq_loc = evt.initIqLiveLocation();
+  this->populate_location_packet(iq_loc);
+  iq_loc.setSensorsHealthy(sensorsOK);
+  iq_loc.setGpsHealthy(gpsOK);
+  iq_loc.setInputsHealthy(inputsOK);
+  return msg_builder.toBytes();
+}
+
+bool AtlasLocator::gps_ready() {
+  return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
+}
+
+bool AtlasLocator::critical_services_ok(const std::map<std::string, double> &critical_services) {
+  for (auto &kv : critical_services){
+    if (kv.second >= INPUT_INVALID_THRESHOLD){
+      return false;
+    }
+  }
+  return true;
+}
+
+bool AtlasLocator::timestamp_ok(double current_time) {
+  double filter_time = this->kf->get_filter_time();
+  if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
+    LOGE("Observation timestamp is older than the max rewind threshold of the filter");
+    return false;
+  }
+  return true;
+}
+
+void AtlasLocator::refresh_gps_mode(double current_time) {
+  // 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
+  // 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
+  // 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
+  VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
+  if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
+    if (this->gps_mode){
+      this->gps_mode = false;
+      this->reset_kalman(current_time);
+    } else {
+      this->seed_fake_gps_observations(current_time);
+    }
+  }
+}
+
+void AtlasLocator::tune_gnss_source(const AtlasGnssMode &source) {
+  this->gnss_source = source;
+  if (source == AtlasGnssMode::UBLOX) {
+    this->gps_std_factor = 10.0;
+    this->gps_variance_factor = 1.0;
+    this->gps_vertical_variance_factor = 1.0;
+    this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
+  } else {
+    this->gps_std_factor = 2.0;
+    this->gps_variance_factor = 0.0;
+    this->gps_vertical_variance_factor = 3.0;
+    this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
+  }
+}
+
+int AtlasLocator::run() {
+  Params params;
+  AtlasGnssMode source;
+  const char* gps_location_socket;
+  if (params.getBool("UbloxAvailable")) {
+    source = AtlasGnssMode::UBLOX;
+    gps_location_socket = "gpsLocationExternal";
+  } else {
+    source = AtlasGnssMode::QCOM;
+    gps_location_socket = "gpsLocation";
+  }
+
+  this->tune_gnss_source(source);
+  const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
+                                                          "carState", "accelerometer", "gyroscope"};
+
+  SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
+  PubMaster pm({"iqLiveLocation"});
+
+  uint64_t cnt = 0;
+  bool filterInitialized = false;
+  const std::vector<std::string> critical_input_services = {"cameraOdometry", "liveCalibration", "accelerometer", "gyroscope"};
+  for (std::string service : critical_input_services) {
+    this->observation_values_invalid.insert({service, 0.0});
+  }
+
+  while (!do_exit) {
+    sm.update();
+    if (filterInitialized){
+      this->clear_observation_timing_fault();
+      for (const char* service : service_list) {
+        if (sm.updated(service) && sm.valid(service)){
+          const cereal::Event::Reader log = sm[service];
+          this->consume_event(log);
+        }
+      }
+    } else {
+      filterInitialized = sm.allAliveAndValid();
+    }
+
+    const char* trigger_msg = "cameraOdometry";
+    if (sm.updated(trigger_msg)) {
+      bool inputsOK = sm.allValid() && this->inputs_are_ready();
+      bool gpsOK = this->gps_ready();
+      bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
+
+      // Log time to first fix
+      if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
+        this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
+      }
+
+      MessageBuilder msg_builder;
+      kj::ArrayPtr<capnp::byte> bytes = this->pack_state_message(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
+      pm.send("iqLiveLocation", bytes.begin(), bytes.size());
+
+      if (cnt % 1200 == 0 && gpsOK) {  // once a minute
+        VectorXd posGeo = this->current_geodetic();
+        std::string lastGPSPosJSON = util::string_format(
+          "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
+        params.putNonBlocking("LastGPSPositionIQLoc", lastGPSPosJSON);
+      }
+      cnt++;
+    }
+  }
+  return 0;
+}
+
+int main() {
+  util::set_realtime_priority(5);
+
+  AtlasLocator engine;
+  return engine.run();
+}

iqpilot/selfdrive/iqlocd/atlas_loc_core.h

@@ -0,0 +1,100 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+#include <deque>
+#include <fstream>
+#include <memory>
+#include <map>
+#include <string>
+
+#include "cereal/messaging/messaging.h"
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+#include "iqpilot/common/transformations/coordinates.hpp"
+#include "iqpilot/common/transformations/orientation.hpp"
+#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
+#include "iqpilot/selfdrive/iqlocd/sensor_event_constants.h"
+
+#define VISION_DECIMATION 2
+#define SENSOR_DECIMATION 10
+#define POSENET_STD_HIST_HALF 20
+
+enum AtlasGnssMode {
+  UBLOX, QCOM
+};
+
+class AtlasLocator {
+public:
+  AtlasLocator(AtlasGnssMode gnss_source = AtlasGnssMode::UBLOX);
+
+  int run();
+
+  void reset_kalman(double current_time = NAN);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
+  void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
+  void run_finite_guard(double current_time = NAN);
+  void run_time_guard(double current_time = NAN);
+  void cool_reset_tracker();
+  bool gps_ready();
+  bool critical_services_ok(const std::map<std::string, double> &critical_services);
+  bool timestamp_ok(double current_time);
+  void refresh_gps_mode(double current_time);
+  bool inputs_are_ready();
+  void clear_observation_timing_fault();
+
+  kj::ArrayPtr<capnp::byte> pack_state_message(MessageBuilder& msg_builder,
+    bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
+  void populate_location_packet(cereal::IQLiveLocation::Builder& fix);
+
+  Eigen::VectorXd current_geodetic();
+  Eigen::VectorXd current_state_vector();
+  Eigen::VectorXd current_sigma_vector();
+
+  void consume_bytes(const char *data, const size_t size);
+  void consume_event(const cereal::Event::Reader& log);
+  void consume_sensor_frame(double current_time, const cereal::SensorEventData::Reader& log);
+  void consume_gps_frame(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
+  void consume_gnss_frame(double current_time, const cereal::GnssMeasurements::Reader& log);
+  void consume_car_state_frame(double current_time, const cereal::CarState::Reader& log);
+  void consume_camera_odometry(double current_time, const cereal::CameraOdometry::Reader& log);
+  void consume_live_calibration(double current_time, const cereal::LiveCalibrationData::Reader& log);
+
+  void seed_fake_gps_observations(double current_time);
+
+private:
+  std::unique_ptr<OrbitKalman> kf;
+
+  Eigen::VectorXd calib;
+  MatrixXdr device_from_calib;
+  MatrixXdr calib_from_device;
+  bool calibrated = false;
+
+  double car_speed = 0.0;
+  double last_reset_time = NAN;
+  std::deque<double> posenet_stds;
+
+  std::unique_ptr<LocalCoord> converter;
+
+  int64_t unix_timestamp_millis = 0;
+  double reset_tracker = 0.0;
+  bool device_fell = false;
+  bool gps_mode = false;
+  double first_valid_log_time = NAN;
+  double ttff = NAN;
+  double last_gps_msg = 0;
+  AtlasGnssMode gnss_source;
+  bool observation_timings_invalid = false;
+  std::map<std::string, double> observation_values_invalid;
+  bool standstill = true;
+  int32_t orientation_reset_count = 0;
+  float gps_std_factor;
+  float gps_variance_factor;
+  float gps_vertical_variance_factor;
+  double gps_time_offset;
+  Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
+
+  void tune_gnss_source(const AtlasGnssMode &source);
+};

iqpilot/selfdrive/iqlocd/models/car_kf.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+import math
+import sys
+from typing import Any
+
+import numpy as np
+
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
+from openpilot.common.swaglog import cloudlog
+
+from rednose.helpers.kalmanfilter import KalmanFilter
+
+if __name__ == '__main__':  # Generating sympy
+  import sympy as sp
+  from rednose.helpers.ekf_sym import gen_code
+else:
+  from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
+
+
+i = 0
+
+def _slice(n):
+  global i
+  s = slice(i, i + n)
+  i += n
+
+  return s
+
+
+class States:
+  # Vehicle model params
+  STIFFNESS = _slice(1)  # [-]
+  STEER_RATIO = _slice(1)  # [-]
+  ANGLE_OFFSET = _slice(1)  # [rad]
+  ANGLE_OFFSET_FAST = _slice(1)  # [rad]
+
+  VELOCITY = _slice(2)  # (x, y) [m/s]
+  YAW_RATE = _slice(1)  # [rad/s]
+  STEER_ANGLE = _slice(1)  # [rad]
+  ROAD_ROLL = _slice(1)  # [rad]
+
+
+class CarKalman(KalmanFilter):
+  name = 'car'
+
+  initial_x = np.array([
+    1.0,
+    15.0,
+    0.0,
+    0.0,
+
+    10.0, 0.0,
+    0.0,
+    0.0,
+    0.0
+  ])
+
+  # process noise
+  Q = np.diag([
+    (.05 / 100)**2,
+    .01**2,
+    math.radians(0.02)**2,
+    math.radians(0.25)**2,
+
+    .1**2, .01**2,
+    math.radians(0.1)**2,
+    math.radians(0.1)**2,
+    math.radians(1)**2,
+  ])
+  P_initial = Q.copy()
+
+  obs_noise: dict[int, Any] = {
+    ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
+    ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
+    ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
+    ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
+    ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
+  }
+
+  global_vars = [
+    'mass',
+    'rotational_inertia',
+    'center_to_front',
+    'center_to_rear',
+    'stiffness_front',
+    'stiffness_rear',
+  ]
+
+  @staticmethod
+  def generate_code(generated_dir):
+    dim_state = CarKalman.initial_x.shape[0]
+    name = CarKalman.name
+
+    # Linearized single-track lateral dynamics, equations 7.211-7.213
+    # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
+    # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
+
+    # globals
+    global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
+    m, j, aF, aR, cF_orig, cR_orig = global_vars
+
+    # make functions and jacobians with sympy
+    # state variables
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+
+    # Vehicle model constants
+    sf = state[States.STIFFNESS, :][0, 0]
+
+    cF, cR = sf * cF_orig, sf * cR_orig
+    angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
+    angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
+    theta = state[States.ROAD_ROLL, :][0, 0]
+    sa = state[States.STEER_ANGLE, :][0, 0]
+
+    sR = state[States.STEER_RATIO, :][0, 0]
+    u, v = state[States.VELOCITY, :]
+    r = state[States.YAW_RATE, :][0, 0]
+
+    A = sp.Matrix(np.zeros((2, 2)))
+    A[0, 0] = -(cF + cR) / (m * u)
+    A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
+    A[1, 0] = -(cF * aF - cR * aR) / (j * u)
+    A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
+
+    B = sp.Matrix(np.zeros((2, 1)))
+    B[0, 0] = cF / m / sR
+    B[1, 0] = (cF * aF) / j / sR
+
+    C = sp.Matrix(np.zeros((2, 1)))
+    C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
+    C[1, 0] = 0
+
+    x = sp.Matrix([v, r])  # lateral velocity, yaw rate
+    x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
+
+    dt = sp.Symbol('dt')
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
+    state_dot[States.YAW_RATE.start, 0] = x_dot[1]
+
+    # Basic descretization, 1st order integrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    #
+    # Observation functions
+    #
+    obs_eqs = [
+      [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
+      [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
+      [sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
+      [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
+      [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
+      [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
+      [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
+      [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+    ]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
+
+  def __init__(self, generated_dir):
+    dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
+    self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
+                              dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
+
+  def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
+    self.filter.set_global("mass", mass)
+    self.filter.set_global("rotational_inertia", rotational_inertia)
+    self.filter.set_global("center_to_front", center_to_front)
+    self.filter.set_global("center_to_rear", center_to_rear)
+    self.filter.set_global("stiffness_front", stiffness_front)
+    self.filter.set_global("stiffness_rear", stiffness_rear)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  CarKalman.generate_code(generated_dir)

iqpilot/selfdrive/iqlocd/models/constants.py

@@ -0,0 +1,92 @@
+import os
+
+GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
+
+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]

iqpilot/selfdrive/iqlocd/models/orbit_kf.cc

@@ -0,0 +1,122 @@
+#include "iqpilot/selfdrive/iqlocd/models/orbit_kf.h"
+
+using namespace EKFS;
+using namespace Eigen;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
+  return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
+}
+
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
+  return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
+}
+
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
+  std::vector<Eigen::Map<Eigen::VectorXd>> res;
+  for (const Eigen::VectorXd &vec : vec_vec) {
+    res.push_back(get_mapvec(vec));
+  }
+  return res;
+}
+
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
+  std::vector<Eigen::Map<MatrixXdr>> res;
+  for (const MatrixXdr &mat : mat_vec) {
+    res.push_back(get_mapmat(mat));
+  }
+  return res;
+}
+
+OrbitKalman::OrbitKalman() {
+  this->dim_state = orbit_initial_x.rows();
+  this->dim_state_err = orbit_initial_P_diag.rows();
+
+  this->initial_x = orbit_initial_x;
+  this->initial_P = orbit_initial_P_diag.asDiagonal();
+  this->fake_gps_pos_cov = orbit_fake_gps_pos_cov_diag.asDiagonal();
+  this->fake_gps_vel_cov = orbit_fake_gps_vel_cov_diag.asDiagonal();
+  this->reset_orientation_P = orbit_reset_orientation_diag.asDiagonal();
+  this->Q = orbit_Q_diag.asDiagonal();
+  for (auto& pair : orbit_obs_noise_diag) {
+    this->obs_noise[pair.first] = pair.second.asDiagonal();
+  }
+
+  // init filter
+  this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
+    get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
+    std::vector<int>{3}, std::vector<std::string>(), 0.8);
+}
+
+void OrbitKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
+  MatrixXdr covs = covs_diag.asDiagonal();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void OrbitKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+void OrbitKalman::init_state(const VectorXd &state, double filter_time) {
+  MatrixXdr covs = this->filter->covs();
+  this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
+}
+
+VectorXd OrbitKalman::get_x() {
+  return this->filter->state();
+}
+
+MatrixXdr OrbitKalman::get_P() {
+  return this->filter->covs();
+}
+
+double OrbitKalman::get_filter_time() {
+  return this->filter->get_filter_time();
+}
+
+std::vector<MatrixXdr> OrbitKalman::get_R(int kind, int n) {
+  std::vector<MatrixXdr> R;
+  for (int i = 0; i < n; i++) {
+    R.push_back(this->obs_noise[kind]);
+  }
+  return R;
+}
+
+std::optional<Estimate> OrbitKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
+  std::optional<Estimate> r;
+  if (R.size() == 0) {
+    R = this->get_R(kind, meas.size());
+  }
+  r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
+  return r;
+}
+
+void OrbitKalman::predict(double t) {
+  this->filter->predict(t);
+}
+
+const Eigen::VectorXd &OrbitKalman::get_initial_x() {
+  return this->initial_x;
+}
+
+const MatrixXdr &OrbitKalman::get_initial_P() {
+  return this->initial_P;
+}
+
+const MatrixXdr &OrbitKalman::get_fake_gps_pos_cov() {
+  return this->fake_gps_pos_cov;
+}
+
+const MatrixXdr &OrbitKalman::get_fake_gps_vel_cov() {
+  return this->fake_gps_vel_cov;
+}
+
+const MatrixXdr &OrbitKalman::get_reset_orientation_P() {
+  return this->reset_orientation_P;
+}
+
+MatrixXdr OrbitKalman::H(const VectorXd &in) {
+  assert(in.size() == 6);
+  Matrix<double, 3, 6, Eigen::RowMajor> res;
+  this->filter->get_extra_routine("H")((double*)in.data(), res.data());
+  return res;
+}

iqpilot/selfdrive/iqlocd/models/orbit_kf.h

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <string>
+#include <cmath>
+#include <memory>
+#include <unordered_map>
+#include <vector>
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+
+#include "generated/orbit_state_constants.h"
+#include "rednose/helpers/ekf_sym.h"
+
+#define EARTH_GM 3.986005e14  // m^3/s^2 (gravitational constant * mass of earth)
+
+using namespace EKFS;
+
+Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
+Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
+std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
+std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
+
+class OrbitKalman {
+public:
+  OrbitKalman();
+
+  void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
+  void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
+  void init_state(const Eigen::VectorXd &state, double filter_time);
+
+  Eigen::VectorXd get_x();
+  MatrixXdr get_P();
+  double get_filter_time();
+  std::vector<MatrixXdr> get_R(int kind, int n);
+
+  std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
+  std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
+  std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
+  void predict(double t);
+
+  const Eigen::VectorXd &get_initial_x();
+  const MatrixXdr &get_initial_P();
+  const MatrixXdr &get_fake_gps_pos_cov();
+  const MatrixXdr &get_fake_gps_vel_cov();
+  const MatrixXdr &get_reset_orientation_P();
+
+  MatrixXdr H(const Eigen::VectorXd &in);
+
+private:
+  std::string name = "orbit";
+
+  std::shared_ptr<EKFSym> filter;
+
+  int dim_state;
+  int dim_state_err;
+
+  Eigen::VectorXd initial_x;
+  MatrixXdr initial_P;
+  MatrixXdr fake_gps_pos_cov;
+  MatrixXdr fake_gps_vel_cov;
+  MatrixXdr reset_orientation_P;
+  MatrixXdr Q;  // process noise
+  std::unordered_map<int, MatrixXdr> obs_noise;
+};

iqpilot/selfdrive/iqlocd/models/orbit_kf.py

@@ -0,0 +1,242 @@
+#!/usr/bin/env python3
+
+import sys
+import os
+import numpy as np
+
+from openpilot.iqpilot.selfdrive.iqlocd.models.constants import ObservationKind
+
+import sympy as sp
+import inspect
+from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
+from rednose.helpers.ekf_sym import gen_code
+
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+
+
+def numpy2eigenstring(arr):
+  assert(len(arr.shape) == 1)
+  arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
+  return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
+
+
+class States:
+  ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters
+  ECEF_ORIENTATION = slice(3, 7)  # quat for pose of phone in ecef
+  ECEF_VELOCITY = slice(7, 10)  # ecef velocity in m/s
+  ANGULAR_VELOCITY = slice(10, 13)  # roll, pitch and yaw rates in device frame in radians/s
+  GYRO_BIAS = slice(13, 16)  # roll, pitch and yaw biases
+  ACCELERATION = slice(16, 19)  # Acceleration in device frame in m/s**2
+  ACC_BIAS = slice(19, 22)  # Acceletometer bias in m/s**2
+
+  # Error-state has different slices because it is an ESKF
+  ECEF_POS_ERR = slice(0, 3)
+  ECEF_ORIENTATION_ERR = slice(3, 6)  # euler angles for orientation error
+  ECEF_VELOCITY_ERR = slice(6, 9)
+  ANGULAR_VELOCITY_ERR = slice(9, 12)
+  GYRO_BIAS_ERR = slice(12, 15)
+  ACCELERATION_ERR = slice(15, 18)
+  ACC_BIAS_ERR = slice(18, 21)
+
+
+class OrbitScopeModel:
+  name = 'orbit'
+
+  initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
+                        0.42254641, -0.31238054, -0.83602975, -0.15788347,  # NED [0,0,0] -> ECEF Quat
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0,
+                        0, 0, 0])
+
+  # state covariance
+  initial_P_diag = np.array([10**2, 10**2, 10**2,
+                             0.01**2, 0.01**2, 0.01**2,
+                             10**2, 10**2, 10**2,
+                             1**2, 1**2, 1**2,
+                             1**2, 1**2, 1**2,
+                             100**2, 100**2, 100**2,
+                             0.01**2, 0.01**2, 0.01**2])
+
+  # state covariance when resetting midway in a segment
+  reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+  # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+  fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+  fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
+  # process noise
+  Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
+                     0.001**2, 0.001**2, 0.001**2,
+                     0.01**2, 0.01**2, 0.01**2,
+                     0.1**2, 0.1**2, 0.1**2,
+                     (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+                     3**2, 3**2, 3**2,
+                     0.005**2, 0.005**2, 0.005**2])
+
+  obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+                    ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
+                    ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
+                    ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
+                    ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
+                    ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
+
+  @staticmethod
+  def generate_code(generated_dir):
+    name = OrbitScopeModel.name
+    dim_state = OrbitScopeModel.initial_x.shape[0]
+    dim_state_err = OrbitScopeModel.initial_P_diag.shape[0]
+
+    state_sym = sp.MatrixSymbol('state', dim_state, 1)
+    state = sp.Matrix(state_sym)
+    x, y, z = state[States.ECEF_POS, :]
+    q = state[States.ECEF_ORIENTATION, :]
+    v = state[States.ECEF_VELOCITY, :]
+    vx, vy, vz = v
+    omega = state[States.ANGULAR_VELOCITY, :]
+    vroll, vpitch, vyaw = omega
+    roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
+    acceleration = state[States.ACCELERATION, :]
+    acc_bias = state[States.ACC_BIAS, :]
+
+    dt = sp.Symbol('dt')
+
+    # calibration and attitude rotation matrices
+    quat_rot = quat_rotate(*q)
+
+    # Got the quat predict equations from here
+    # A New Quaternion-Based Kalman Filter for
+    # Real-Time Attitude Estimation Using the Two-Step
+    # Geometrically-Intuitive Correction Algorithm
+    A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
+                         [vroll, 0, vyaw, -vpitch],
+                         [vpitch, -vyaw, 0, vroll],
+                         [vyaw, vpitch, -vroll, 0]])
+    q_dot = A * q
+
+    # Time derivative of the state as a function of state
+    state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+    state_dot[States.ECEF_POS, :] = v
+    state_dot[States.ECEF_ORIENTATION, :] = q_dot
+    state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
+
+    # Basic descretization, 1st order intergrator
+    # Can be pretty bad if dt is big
+    f_sym = state + dt * state_dot
+
+    state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
+    state_err = sp.Matrix(state_err_sym)
+    quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
+    v_err = state_err[States.ECEF_VELOCITY_ERR, :]
+    omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
+    acceleration_err = state_err[States.ACCELERATION_ERR, :]
+
+    # Time derivative of the state error as a function of state error and state
+    quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
+    q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
+    state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
+    state_err_dot[States.ECEF_POS_ERR, :] = v_err
+    state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
+    state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
+    f_err_sym = state_err + dt * state_err_dot
+
+    # Observation matrix modifier
+    H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
+    H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
+    H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
+    H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
+
+    # these error functions are defined so that say there
+    # is a nominal x and true x:
+    # true x = err_function(nominal x, delta x)
+    # delta x = inv_err_function(nominal x, true x)
+    nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
+    true_x = sp.MatrixSymbol('true_x', dim_state, 1)
+    delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
+
+    err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
+    delta_quat = sp.Matrix(np.ones(4))
+    delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
+    err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
+    err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
+    err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
+
+    inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
+    inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
+    delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
+    inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
+
+    eskf_params = [[err_function_sym, nom_x, delta_x],
+                   [inv_err_function_sym, nom_x, true_x],
+                   H_mod_sym, f_err_sym, state_err_sym]
+    #
+    # Observation functions
+    #
+    h_gyro_sym = sp.Matrix([
+      vroll + roll_bias,
+      vpitch + pitch_bias,
+      vyaw + yaw_bias])
+
+    pos = sp.Matrix([x, y, z])
+    gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
+    h_acc_sym = (gravity + acceleration + acc_bias)
+    h_acc_stationary_sym = acceleration
+    h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
+    h_pos_sym = sp.Matrix([x, y, z])
+    h_vel_sym = sp.Matrix([vx, vy, vz])
+    h_orientation_sym = q
+    h_relative_motion = sp.Matrix(quat_rot.T * v)
+
+    obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+               [h_phone_rot_sym, ObservationKind.NO_ROT, None],
+               [h_acc_sym, ObservationKind.PHONE_ACCEL, None],
+               [h_pos_sym, ObservationKind.ECEF_POS, None],
+               [h_vel_sym, ObservationKind.ECEF_VEL, None],
+               [h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
+               [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+               [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
+               [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
+
+    # this returns a sympy routine for the jacobian of the observation function of the local vel
+    in_vec = sp.MatrixSymbol('in_vec', 6, 1)  # roll, pitch, yaw, vx, vy, vz
+    h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
+    extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
+
+    gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
+
+    # write constants to extra header file for use in cpp
+    orbit_header = "#pragma once\n\n"
+    orbit_header += "#include <unordered_map>\n"
+    orbit_header += "#include <eigen3/Eigen/Dense>\n\n"
+    for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
+      assert(slc.step is None)  # unsupported
+      orbit_header += f'#define STATE_{state}_START {slc.start}\n'
+      orbit_header += f'#define STATE_{state}_END {slc.stop}\n'
+      orbit_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
+    orbit_header += "\n"
+
+    for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
+      orbit_header += f'#define OBSERVATION_{kind} {val}\n'
+    orbit_header += "\n"
+
+    orbit_header += f"static const Eigen::VectorXd orbit_initial_x = {numpy2eigenstring(OrbitScopeModel.initial_x)};\n"
+    orbit_header += f"static const Eigen::VectorXd orbit_initial_P_diag = {numpy2eigenstring(OrbitScopeModel.initial_P_diag)};\n"
+    orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_pos_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_pos_cov_diag)};\n"
+    orbit_header += f"static const Eigen::VectorXd orbit_fake_gps_vel_cov_diag = {numpy2eigenstring(OrbitScopeModel.fake_gps_vel_cov_diag)};\n"
+    orbit_header += f"static const Eigen::VectorXd orbit_reset_orientation_diag = {numpy2eigenstring(OrbitScopeModel.reset_orientation_diag)};\n"
+    orbit_header += f"static const Eigen::VectorXd orbit_Q_diag = {numpy2eigenstring(OrbitScopeModel.Q_diag)};\n"
+    orbit_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> orbit_obs_noise_diag = {\n"
+    for kind, noise in OrbitScopeModel.obs_noise_diag.items():
+      orbit_header += f"  {{ {kind}, {numpy2eigenstring(noise)} }},\n"
+    orbit_header += "};\n\n"
+
+    open(os.path.join(generated_dir, "orbit_state_constants.h"), 'w').write(orbit_header)
+
+
+if __name__ == "__main__":
+  generated_dir = sys.argv[2]
+  OrbitScopeModel.generate_code(generated_dir)

iqpilot/selfdrive/iqlocd/sensor_event_constants.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#define SENSOR_ACCELEROMETER 1
+#define SENSOR_MAGNETOMETER 2
+#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
+#define SENSOR_GYRO 4
+#define SENSOR_GYRO_UNCALIBRATED 5
+#define SENSOR_LIGHT 7
+
+#define SENSOR_TYPE_ACCELEROMETER 1
+#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
+#define SENSOR_TYPE_GYROSCOPE 4
+#define SENSOR_TYPE_LIGHT 5
+#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
+#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
+#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
+#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16

iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py

@@ -0,0 +1,94 @@
+import pytest
+import platform
+import json
+import random
+import time
+import capnp
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.params import Params
+from openpilot.common.transformations.coordinates import ecef2geodetic
+
+from openpilot.system.manager.process_config import managed_processes
+
+
+if platform.system() == 'Darwin':
+  pytest.skip("Skipping locationd test on macOS due to unsupported msgq.", allow_module_level=True)
+
+
+class TestIQLocdProc:
+  LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
+              'accelerometer', 'gyroscope', 'magnetometer']
+
+  def setup_method(self):
+    self.pm = messaging.PubMaster(self.LLD_MSGS)
+
+    self.params = Params()
+    self.params.put_bool("UbloxAvailable", True)
+    managed_processes['iqlocd'].prepare()
+    managed_processes['iqlocd'].start()
+
+  def teardown_method(self):
+    managed_processes['iqlocd'].stop()
+
+  def get_msg(self, name, t):
+    try:
+      msg = messaging.new_message(name)
+    except capnp.lib.capnp.KjException:
+      msg = messaging.new_message(name, 0)
+
+    if name == "gpsLocationExternal":
+      msg.gpsLocationExternal.flags = 1
+      msg.gpsLocationExternal.hasFix = True
+      msg.gpsLocationExternal.verticalAccuracy = 1.0
+      msg.gpsLocationExternal.speedAccuracy = 1.0
+      msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
+      msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
+      msg.gpsLocationExternal.latitude = float(self.lat)
+      msg.gpsLocationExternal.longitude = float(self.lon)
+      msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
+      msg.gpsLocationExternal.altitude = float(self.alt)
+    #if name == "gnssMeasurements":
+    #  msg.gnssMeasurements.measTime = t
+    #  msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
+    #  msg.gnssMeasurements.positionECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.positionECEF.valid = True
+    #  msg.gnssMeasurements.velocityECEF.value = []
+    #  msg.gnssMeasurements.velocityECEF.std = [0,0,0]
+    #  msg.gnssMeasurements.velocityECEF.valid = True
+    elif name == 'cameraOdometry':
+      msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
+      msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
+    msg.logMonoTime = t
+    msg.valid = True
+    return msg
+
+  def test_params_gps(self):
+    random.seed(123489234)
+    self.params.remove('LastGPSPositionIQLoc')
+
+    self.x = -2710700 + (random.random() * 1e5)
+    self.y = -4280600 + (random.random() * 1e5)
+    self.z = 3850300 + (random.random() * 1e5)
+    self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
+
+    # get fake messages at the correct frequency, listed in services.py
+    msgs = []
+    for sec in range(65):
+      for name in self.LLD_MSGS:
+        for j in range(int(SERVICE_LIST[name].frequency)):
+          msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
+
+    for msg in sorted(msgs, key=lambda x: x.logMonoTime):
+      self.pm.send(msg.which(), msg)
+      if msg.which() == "cameraOdometry":
+        self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005)
+    time.sleep(1)  # wait for async params write
+
+    lastGPS = json.loads(self.params.get('LastGPSPositionIQLoc'))
+    assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
+    assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
+    assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001)

iqpilot/selfdrive/iqmodeld/.gitignore

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

iqpilot/selfdrive/iqmodeld/SConscript

@@ -0,0 +1,131 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import glob
+import os
+
+Import("env", "envCython", "arch", "cereal", "messaging", "common", "visionipc")
+lenv = env.Clone()
+lenvCython = envCython.Clone()
+
+libs = [cereal, messaging, visionipc, common, "capnp", "kj", "pthread"]
+frameworks = []
+core_sources = ["native/iqmodel.cc", "transforms/yuv.cc", "transforms/warp_geometry.cc"]
+SMALL_MODEL_NAMES = ["supercombo", "driving_vision", "driving_off_policy", "driving_on_policy", "driving_policy"]
+FUSED_TRIPLET = ["driving_vision", "driving_off_policy", "driving_on_policy"]
+PC = not os.path.isfile("/TICI")
+
+
+def _inject_path_define(symbol, filename):
+  quoted = f'-D{symbol}_PATH=\\"{File(filename).abspath}\\"'
+  for active_env in (lenv, lenvCython):
+    active_env["CXXFLAGS"].append(quoted)
+
+
+def _tinygrad_sources():
+  root = env.Dir("#tinygrad_repo").relpath
+  workspace = env.Dir("#").abspath
+  return ["#" + path for path in glob.glob(root + "/**", recursive=True, root_dir=workspace) if "pycache" not in path]
+
+
+def _present_models():
+  return [name for name in SMALL_MODEL_NAMES if File(f"models/{name}.onnx").exists()]
+
+
+def _tinygrad_flags():
+  if arch == "larch64":
+    return "DEV=QCOM IMAGE=2 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+  if arch == "Darwin":
+    return f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0'
+  if arch == "x86_64":
+    return "DEV=CPU:LLVM IMAGE=0"
+  return "DEV=CPU:LLVM IMAGE=0"
+
+
+def _fused_flags():
+  if arch == "larch64":
+    return "DEV=QCOM IMAGE=2 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1"
+  if arch == "Darwin":
+    return f'DEV=CPU HOME={os.path.expanduser("~")} IMAGE=0 FLOAT16=1'
+  if arch == "x86_64":
+    return "DEV=CPU:LLVM IMAGE=0 FLOAT16=1"
+  return "DEV=CPU:LLVM IMAGE=0 FLOAT16=1"
+
+
+def _queue_metadata_generation(model_names, tinygrad_files):
+  if not PC:
+    return
+  inputs = tinygrad_files + [File(Dir("#iqpilot/selfdrive/iqmodeld/tools").File("install_models_pc.py").abspath)]
+  outputs = []
+  for model_name in model_names:
+    inputs.extend([File(f"models/{model_name}.onnx"), File(f"models/{model_name}_tinygrad.pkl")])
+    outputs.append(File(f"models/{model_name}_metadata.pkl"))
+  if outputs:
+    tool_dir = Dir("#iqpilot/selfdrive/iqmodeld/tools").abspath
+    model_dir = Dir("models").abspath
+    lenv.Command(outputs, inputs,
+                 lenv.PrettyAction(f"${{PYWARN}} python3 {tool_dir}/install_models_pc.py {model_dir}", 'META'))
+
+
+if arch == "Darwin":
+  frameworks += ["OpenCL"]
+else:
+  libs += ["OpenCL"]
+
+for symbol, filename in {"TRANSFORM": "transforms/warp_geometry.cl", "LOADYUV": "transforms/yuv.cl"}.items():
+  _inject_path_define(symbol, filename)
+
+cython_libs = envCython["LIBS"] + libs
+iqmodel_lib = lenv.Library("iqmodel", core_sources)
+lenvCython.Program("native/iqmodel_pyx.so", "native/iqmodel_pyx.pyx", LIBS=[iqmodel_lib, *cython_libs], FRAMEWORKS=frameworks)
+tinygrad_files = _tinygrad_sources()
+present_models = _present_models()
+_queue_metadata_generation(present_models, tinygrad_files)
+
+
+def tg_compile(flags, model_name):
+  pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
+  fn = File(f"models/{model_name}").abspath
+  return lenv.Command(
+    fn + "_tinygrad.pkl",
+    [fn + ".onnx"] + tinygrad_files,
+    lenv.PrettyAction(
+      f'${{PYWARN}} {pythonpath_string} {flags} python3 {Dir("#tinygrad_repo").abspath}/examples/openpilot/compile3.py {fn}.onnx {fn}_tinygrad.pkl',
+      'MODEL', logfile='${TARGET}.log')
+  )
+
+
+for model_name in present_models:
+  tg_compile(_tinygrad_flags(), model_name)
+
+from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
+from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE
+
+FUSED_CAMERA_CONFIGS = [(_ar_ox_fisheye.width, _ar_ox_fisheye.height), (_os_fisheye.width, _os_fisheye.height)]
+FUSED_FRAME_SKIP = 4
+
+
+def tg_compile_fused(file_prefix, flags):
+  pythonpath_string = 'PYTHONPATH="${PYTHONPATH}:' + env.Dir("#tinygrad_repo").abspath + '"'
+  model_dir = Dir("models").abspath
+  model_w, model_h = MEDMODEL_INPUT_SIZE
+  camera_args = " ".join(f"{cw}x{ch}" for cw, ch in FUSED_CAMERA_CONFIGS)
+  out_pkl = File(f"models/{file_prefix}driving_fused_tinygrad.pkl").abspath
+  onnx_deps = [File(f"models/{file_prefix}{model_name}.onnx") for model_name in FUSED_TRIPLET]
+  cmd = (
+    f'${{PYWARN}} {pythonpath_string} {flags} python3 {Dir("#iqpilot/selfdrive/iqmodeld/tools").abspath}/compile_daemon.py '
+    f'--model-size {model_w}x{model_h} --camera-resolutions {camera_args} '
+    f'--vision-onnx {model_dir}/{file_prefix}driving_vision.onnx '
+    f'--off-policy-onnx {model_dir}/{file_prefix}driving_off_policy.onnx '
+    f'--on-policy-onnx {model_dir}/{file_prefix}driving_on_policy.onnx '
+    f'--output {out_pkl} --frame-skip {FUSED_FRAME_SKIP}'
+  )
+  return lenv.Command(out_pkl, onnx_deps + tinygrad_files,
+                      lenv.PrettyAction(cmd, 'MODEL', logfile='${TARGET}.log'))
+
+
+if all(File(f"models/{model_name}.onnx").exists() for model_name in FUSED_TRIPLET):
+  tg_compile_fused("", _fused_flags())
+
+if all(File(f"models/big_{model_name}.onnx").exists() for model_name in FUSED_TRIPLET):
+  tg_compile_fused("big_", "DEV=USB+AMD:LLVM WARP_DEV=QCOM FLOAT16=1 JIT_BATCH_SIZE=0 GMMU=0")

iqpilot/selfdrive/iqmodeld/__init__.py

@@ -0,0 +1,23 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from pathlib import Path
+
+
+def _models_dir() -> Path:
+  return Path(__file__).resolve().parent / "models"
+
+
+def _artifact_path(stem: str, suffix: str) -> Path:
+  return _models_dir() / f"{stem}{suffix}"
+
+
+MODEL_ASSETS = {
+  "onnx": _artifact_path("supercombo", ".onnx"),
+  "tinygrad": _artifact_path("supercombo", "_tinygrad.pkl"),
+  "metadata": _artifact_path("supercombo", "_metadata.pkl"),
+}
+
+MODEL_PATH = MODEL_ASSETS["onnx"]
+MODEL_PKL_PATH = MODEL_ASSETS["tinygrad"]
+METADATA_PATH = MODEL_ASSETS["metadata"]

iqpilot/selfdrive/iqmodeld/camera.py

@@ -0,0 +1,68 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import numpy as np
+
+from openpilot.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:
+  return sm["liveCalibration"].height[0] if sm["liveCalibration"].height 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),
+    )

iqpilot/selfdrive/iqmodeld/config.py

@@ -0,0 +1,130 @@
+"""
+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 _quadratic_series(limit: float, steps: int) -> list[float]:
+  peak_index = steps - 1
+  return [limit * ((index / peak_index) ** 2) 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)

iqpilot/selfdrive/iqmodeld/daemon.py

@@ -0,0 +1,716 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import time
+from dataclasses import dataclass
+from typing import Any
+
+import cereal.messaging as messaging
+import numpy as np
+
+from cereal import car, custom, log
+from cereal.messaging import PubMaster, SubMaster
+from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
+from iqdbc.car.car_helpers import get_demo_car_params
+from setproctitle import setproctitle
+
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL, config_realtime_process
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.selfdrive.controls.lib.drive_helpers import (
+  MODEL_SMOOTHING_MAX_TOTAL_SEC,
+  dynamic_lat_smooth_extra_seconds,
+  get_accel_from_plan,
+  smooth_value,
+)
+from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
+from openpilot.system import sentry
+
+from openpilot.iqpilot.common.steer_delay import resolve_steer_delay
+from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+from openpilot.iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from openpilot.iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
+from openpilot.iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
+from openpilot.iqpilot.selfdrive.iqmodeld.config import Plan
+from openpilot.iqpilot.selfdrive.iqmodeld.messaging import (
+  DrivePacketMemory,
+  pick_curvature,
+  populate_drive_messages,
+  populate_odometry_message,
+)
+from openpilot.iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
+
+try:
+  from openpilot.iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
+except ModuleNotFoundError:
+  class WarpContext:
+    def __init__(self, *args, **kwargs):
+      raise ModuleNotFoundError("openpilot.iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
+
+  class RoadProjector:
+    def __init__(self, *args, **kwargs):
+      raise ModuleNotFoundError("openpilot.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
+
+
+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["liveCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
+      return self.main_warp, self.extra_warp, self.ready
+
+    rpy = get_calibrated_rpy(sm["liveCalibration"])
+    if rpy is None:
+      live_calib = sm["liveCalibration"]
+      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):
+    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._cameras = CameraIngress(self._gpu)
+    self._pub = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"])
+    self._sub = SubMaster([
+      "deviceState", "carState", "roadCameraState", "liveCalibration",
+      "driverMonitoringState", "carControl", "liveDelay", "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
+    self._runtime.lat_delay = resolve_steer_delay(self._params, self._sub["liveDelay"].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
+
+    populate_odometry_message(
+      pose_msg,
+      outputs,
+      main_stamp.frame_id,
+      dropped_frames,
+      main_stamp.timestamp_eof,
+      live_calib_seen,
+    )
+
+    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
+    while True:
+      frame_pair = self._cameras.pull()
+      if frame_pair is None:
+        cloudlog.debug("visionipc frame missing")
+        continue
+
+      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):
+  InferenceDaemon(demo=demo).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

iqpilot/selfdrive/iqmodeld/default_model/bundle.json

@@ -0,0 +1,62 @@
+{
+  "displayName": "Default (CD210)",
+  "environment": "development",
+  "generation": 12,
+  "index": 56,
+  "internalName": "C210M",
+  "is20hz": true,
+  "minimumSelectorVersion": 14,
+  "models": [
+    {
+      "artifact": {
+        "downloadUri": {
+          "sha256": "ba5c459412310a8c65a11e02cb1f522fe439d515589754451561268f028f4fb0",
+          "uri": "https://git.konn3kt.com/teal/IQModels/raw/branch/main/models/recompiled16/model-CD210%20Model%20%28January%2031%2C%202026%29-101/driving_policy_c210m_tinygrad.pkl"
+        },
+        "fileName": "driving_policy_c210m_tinygrad.pkl"
+      },
+      "metadata": {
+        "downloadUri": {
+          "sha256": "15c8c1ad9073424ee1101b1f7170421140ed308ebaa7c917001130fb1760420b",
+          "uri": "https://git.konn3kt.com/teal/IQModels/raw/branch/main/models/recompiled16/model-CD210%20Model%20%28January%2031%2C%202026%29-101/driving_policy_c210m_metadata.pkl"
+        },
+        "fileName": "driving_policy_c210m_metadata.pkl"
+      },
+      "type": "policy"
+    },
+    {
+      "artifact": {
+        "downloadUri": {
+          "sha256": "c6c9cfba6c618a361d474d2fe11c4d8e7a249e9311dd9bcb42b287067ef75ae7",
+          "uri": "https://git.konn3kt.com/teal/IQModels/raw/branch/main/models/recompiled16/model-CD210%20Model%20%28January%2031%2C%202026%29-101/driving_vision_c210m_tinygrad.pkl"
+        },
+        "fileName": "driving_vision_c210m_tinygrad.pkl"
+      },
+      "metadata": {
+        "downloadUri": {
+          "sha256": "a2be39088d38550e818f5ac1c6300a64605ba0bd0676a27fe7bbea9fddae90a1",
+          "uri": "https://git.konn3kt.com/teal/IQModels/raw/branch/main/models/recompiled16/model-CD210%20Model%20%28January%2031%2C%202026%29-101/driving_vision_c210m_metadata.pkl"
+        },
+        "fileName": "driving_vision_c210m_metadata.pkl"
+      },
+      "type": "vision"
+    }
+  ],
+  "overrides": [
+    {
+      "key": "folder",
+      "value": "Master Models"
+    },
+    {
+      "key": "lat",
+      "value": ".0"
+    },
+    {
+      "key": "long",
+      "value": ".3"
+    }
+  ],
+  "ref": "default",
+  "runner": "tinygrad",
+  "status": "notDownloading"
+}

iqpilot/selfdrive/iqmodeld/iqmodeld

@@ -0,0 +1,16 @@
+#!/usr/bin/env bash
+# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+set -euo pipefail
+
+script_home() {
+  cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd
+}
+
+main() {
+  local here
+  here="$(script_home)"
+  exec "$here/daemon.py" "$@"
+}
+
+main "$@"