IQ.Pilot Prebuilt Release @ ab07000

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