IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/.gitignore

@@ -0,0 +1,2 @@
+calibration_param
+traces

iqpilot/selfdrive/controls/controlsd.py

@@ -0,0 +1,443 @@
+#!/usr/bin/env python3
+import math
+import time
+from numbers import Number
+
+from iqpilot.cereal import car, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.constants import CV
+from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import config_realtime_process, lock_memory, DT_CTRL, Priority, Ratekeeper
+from iqpilot.common.swaglog import cloudlog
+
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.steer_delay import lateral_action_delay
+from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from iqpilot.selfdrive.controls.lib.curvature_lookahead import get_lookahead_curvature
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl
+from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+
+from iqpilot.selfdrive.controls.iq_controls_layer import IQControlsLayer
+
+NavTurnSignalController = import_verified_module(
+  "iqpilot_navd_private", "iqpilot_private.navd.nav_turn_signals"
+).NavTurnSignalController
+
+State = log.SelfdriveState.OpenpilotState
+EventName = log.OnroadEvent.EventName
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+
+ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
+CTRL_ESSENTIAL_SERVICES = ("carState", "modelV2", "longitudinalPlan", "vehicleParameters", "selfdriveState")
+CTRL_LOOP_WARN_US = 15_000
+CTRL_LOOP_BAD_US = 25_000
+CTRL_LOOP_SEVERE_US = 50_000
+CTRL_PHASE_WARN_US = 8_000
+CTRL_TAIL_WARN_US = 8_000
+CTRL_FLAG_MISSING_INPUTS = 1 << 0
+CTRL_FLAG_RK_OVERRUN = 1 << 1
+LAT_SMOOTH_SECONDS = 0.0
+
+
+class Controls(IQControlsLayer):
+  def __init__(self) -> None:
+    self.params = Params()
+    cloudlog.info("controlsd is waiting for CarParams")
+    self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
+    cloudlog.info("controlsd got CarParams")
+
+    # Initialize iqpilot controlsd extension and base model state
+    IQControlsLayer.__init__(self, self.CP, self.params)
+
+    self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_IQ)
+
+    self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState',
+                                   'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan',
+                                   'carState', 'carOutput', 'driverMonitoringState', 'onroadEvents',
+                                   'driverAssistance', 'lateralDelay'] + self.iq_sub_services,
+                                  poll='selfdriveState')
+    self.pm = messaging.PubMaster(['carControl', 'controlsState', 'iqPerfTrace'] + self.iq_pub_services)
+
+    self.steer_limited_by_safety = False
+    self.curvature = 0.0
+    self.desired_curvature = 0.0
+    self.roll_compensation = 0.0
+
+    self._perf = PerfTraceEmitter("controlsd", pubmaster=self.pm)
+    self._perf_ring = PerfTraceRing()
+
+    self._param_update_time = 0.0
+    self.enable_curvature_controller = False
+    self.enable_speed_limit_control = False
+    self.enable_speed_limit_predicative = False
+    self.enable_pred_react_to_speed_limits = False
+    self.enable_pred_react_to_curves = False
+    self.enable_long_comfort_mode = False
+    self.force_rhd_for_bsm = False
+    self.navigation_enabled = False
+    self.nav_exit_lane_change = False
+    self._update_params()
+
+    self.nav_turn_signal_controller = NavTurnSignalController(self.CP)
+
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None
+
+    self.LoC = LongControl(self.CP, self.CP_IQ)
+    self.VM = VehicleModel(self.CP)
+    self.LaC: LatControl
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      self.LaC = LatControlAngle(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+    elif self.CP.lateralTuning.which() == 'pid':
+      self.LaC = LatControlPID(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+      if self._use_pq_torque():
+        try:
+          self.LaC = LatControlTorquePQ(self.CP, self.CP_IQ, self.CI, DT_CTRL)
+        except Exception:
+          cloudlog.exception("LatControlTorquePQ init failed; using generic torque")
+
+  def _use_pq_torque(self) -> bool:
+    try:
+      if self.CP.brand != 'volkswagen':
+        return False
+      from iqdbc.car.volkswagen.values import VolkswagenFlags
+      return bool(self.CP.flags & VolkswagenFlags.PQ)
+    except Exception:
+      cloudlog.exception("pq torque selection failed; using generic torque")
+      return False
+
+  def _update_params(self) -> None:
+    self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController")
+    self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl")
+    self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative")
+    self.enable_pred_react_to_speed_limits = self.params.get_bool("EnableSLPredReactToSL")
+    self.enable_pred_react_to_curves = self.params.get_bool("EnableSLPredReactToCurves")
+    self.enable_long_comfort_mode = self.params.get_bool("EnableLongComfortMode")
+    self.force_rhd_for_bsm = self.params.get_bool("ForceRHDForBSM")
+    self.navigation_enabled = self.params.get_bool("NavigationEnabled")
+    self.nav_exit_lane_change = self.params.get_bool("NavExitLaneChange")
+
+  def update(self):
+    self.sm.update(15)
+    if self.sm.updated["extrinsicsCalibration"]:
+      self.pose_calibrator.feed_live_calib(self.sm['extrinsicsCalibration'])
+    if self.sm.updated["deviceMotion"]:
+      device_pose = Pose.from_live_pose(self.sm['deviceMotion'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
+    if time.monotonic() - self._param_update_time > 3:
+      self._update_params()
+      self._param_update_time = time.monotonic()
+
+  def state_control(self):
+    CS = self.sm['carState']
+
+    # Update VehicleModel
+    lp = self.sm['vehicleParameters']
+    x = max(lp.stiffnessFactor, 0.1)
+    sr = max(lp.steerRatio, 0.1)
+    self.VM.update_params(x, sr)
+
+    steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
+    self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
+    self.roll_compensation = -self.VM.roll_compensation(lp.roll, CS.vEgo)
+
+    lac_ext = getattr(self.LaC, "extension", None)
+    if self.CP.lateralTuning.which() == 'torque' and hasattr(self.LaC, "update_live_torque_params"):
+      torque_params = self.sm['lateralTorqueParameters']
+      if self.sm.all_checks(['lateralTorqueParameters']) and torque_params.useParams:
+        self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
+                                           torque_params.frictionCoefficientFiltered)
+
+        if lac_ext is not None:
+          lac_ext.update_limits()
+
+      if lac_ext is not None:
+        lac_ext.update_model_v2(self.sm['modelV2'])
+
+      if lac_ext is not None:
+        lac_ext.update_lateral_lag(self.CP.steerActuatorDelay)
+
+    long_plan = self.sm['longitudinalPlan']
+    model_v2 = self.sm['modelV2']
+
+    CC = car.CarControl.new_message()
+    CC.enabled = self.sm['selfdriveState'].enabled
+
+    # Check which actuators can be enabled
+    standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill
+
+    # Get which state to use for active lateral control
+    _lat_active = self.iq_lateral_allowed(self.sm)
+
+    CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
+                   (not standstill or self.CP.steerAtStandstill)
+    # long control may stay active through a gas override on platforms that opt in
+    override_longitudinal = any(e.overrideLongitudinal for e in self.sm['onroadEvents'])
+    long_through_override = self.CP_IQ.longActiveWithGasOverride and self.CP.openpilotLongitudinalControl
+    CC.longActive = CC.enabled and not getattr(CS, 'cruiseFaultLateralMode', False) and \
+                    (not override_longitudinal or long_through_override) and \
+                    (self.CP.openpilotLongitudinalControl or not self.CP_IQ.pcmCruiseSpeed)
+
+    CC.leftBlinker, CC.rightBlinker = self.nav_turn_signal_controller.update(
+      self.sm['iqNavState'] if self.sm.alive['iqNavState'] else None,
+      self.navigation_enabled,
+      self.nav_exit_lane_change,
+      CC.enabled and CC.latActive,
+      CS.leftBlinker,
+      CS.rightBlinker,
+      CS.vEgo,
+      CS.yawRate,
+      CS.steeringAngleDeg,
+      self.sm.alive['iqNavState'] and self.sm.valid['iqNavState'],
+    )
+
+    actuators = CC.actuators
+    actuators.longControlState = self.LoC.long_control_state
+
+    if not CC.latActive:
+      self.LaC.reset()
+
+    if not CC.longActive:
+      self.LoC.reset()
+
+    # accel PID loop
+    pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, self.CP_IQ, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
+    actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
+                                            long_plan.leadDistance, long_plan.hasLead, gas_override=override_longitudinal))
+
+    # Steering PID loop and lateral MPC
+    # Reset desired curvature to current to avoid violating the limits on engage
+    if not CC.latActive:
+      new_desired_curvature = self.curvature
+    elif self.sm.valid['lateralManeuverPlan']:
+      new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature
+    else:
+      new_desired_curvature = model_v2.action.desiredCurvature
+
+    lat_accel_override = bool(CS.gasPressed) or bool(self.sm['iqState'].aol.active)
+    self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, lat_accel_override)
+    lat_delay = lateral_action_delay(self.params, self.CP, self.sm["lateralDelay"].lateralDelay) + LAT_SMOOTH_SECONDS
+    lookahead_curvature = None
+    if not self.sm.valid['lateralManeuverPlan']:
+      lookahead_curvature = get_lookahead_curvature(model_v2, CS.vEgo, lat_delay)
+
+    actuators.curvature = self.desired_curvature
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                       self.steer_limited_by_safety, self.desired_curvature,
+                                                       self.calibrated_pose, curvature_limited, lat_delay, lookahead_curvature)
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      actuators.torque = 0.0
+      actuators.steeringAngleDeg = float(steeringAngleDeg)
+    else:
+      actuators.torque = float(steer)
+      actuators.steeringAngleDeg = float(steeringAngleDeg)
+    # Ensure no NaNs/Infs
+    for p in ACTUATOR_FIELDS:
+      attr = getattr(actuators, p)
+      if not isinstance(attr, Number):
+        continue
+
+      if not math.isfinite(attr):
+        cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
+        setattr(actuators, p, 0.0)
+
+    return CC, lac_log
+
+  def publish(self, CC, lac_log):
+    CS = self.sm['carState']
+
+    CC.curvatureControllerActive = self.enable_curvature_controller
+    CC.steerLimited = self.steer_limited_by_safety
+    CC.forceRHDForBSM = self.force_rhd_for_bsm
+    CC.longComfortMode = self.enable_long_comfort_mode
+
+    # Orientation and angle rates can be useful for carcontroller
+    # Only calibrated (car) frame is relevant for the carcontroller
+    CC.currentCurvature = self.curvature
+    CC.rollCompensation = self.roll_compensation
+    if self.calibrated_pose is not None:
+      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
+
+    CC.cruiseControl.override = CC.enabled and not CC.longActive and (self.CP.openpilotLongitudinalControl or not self.CP_IQ.pcmCruiseSpeed)
+    dm_lockout = self.CP.pcmCruise and any(e.name == EventName.tooDistracted for e in self.sm['onroadEvents'])
+    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) and not dm_lockout
+    CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop
+    CC.cruiseControl.speedLimit = self.enable_speed_limit_control
+    CC.cruiseControl.speedLimitPredicative = self.enable_speed_limit_predicative
+    CC.cruiseControl.speedLimitPredReactToSL = self.enable_pred_react_to_speed_limits
+    CC.cruiseControl.speedLimitPredReactToCurves = self.enable_pred_react_to_curves
+
+    hudControl = CC.hudControl
+    hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
+    hudControl.speedVisible = CC.enabled
+    hudControl.lanesVisible = CC.enabled
+    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
+    hudControl.leadDistance = self.sm['longitudinalPlan'].leadDistance
+    hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
+    if self.sm['selfdriveState'].personality.raw == log.LongitudinalPersonality.relaxed:
+      hudControl.leadFollowTime = 1.75
+    elif self.sm['selfdriveState'].personality.raw == log.LongitudinalPersonality.aggressive:
+      hudControl.leadFollowTime = 1.25
+    else:
+      hudControl.leadFollowTime = 1.45
+    hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
+    hudControl.audibleAlert = self.sm['selfdriveState'].alertSound
+    hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive'
+
+    hudControl.rightLaneVisible = True
+    hudControl.leftLaneVisible = True
+    if self.sm.valid['driverAssistance']:
+      hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture
+      hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture
+
+    if self.sm['selfdriveState'].active:
+      CO = self.sm['carOutput']
+      if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+        self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
+                                              STEER_ANGLE_SATURATION_THRESHOLD
+      else:
+        self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
+
+    # TODO: both controlsState and carControl valids should be set by
+    #       sm.all_checks(), but this creates a circular dependency
+
+    # controlsState
+    dat = messaging.new_message('controlsState')
+    dat.valid = CS.canValid
+    cs = dat.controlsState
+
+    cs.curvature = self.curvature
+    cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan']
+    cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2']
+    cs.desiredCurvature = self.desired_curvature
+    cs.longControlState = self.LoC.long_control_state
+    cs.upAccelCmd = float(self.LoC.pid.p)
+    cs.uiAccelCmd = float(self.LoC.pid.i)
+    cs.ufAccelCmd = float(self.LoC.pid.f)
+    cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
+                         (self.sm['selfdriveState'].state == State.softDisabling))
+
+    lat_tuning = self.CP.lateralTuning.which()
+    if self.CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED):
+      cs.lateralControlState.angleState = lac_log
+    elif lat_tuning == 'pid':
+      cs.lateralControlState.pidState = lac_log
+    elif lat_tuning == 'torque':
+      cs.lateralControlState.torqueState = lac_log
+
+    self.pm.send('controlsState', dat)
+
+    # carControl
+    cc_send = messaging.new_message('carControl')
+    cc_send.valid = CS.canValid
+    cc_send.carControl = CC
+    self.pm.send('carControl', cc_send)
+
+  def _tail_work(self) -> int:
+    started_ns = time.monotonic_ns()
+    self.refresh_iq_params(self.sm)
+    self.publish_iq_state(self.sm, self.pm)
+    return (time.monotonic_ns() - started_ns) // 1000
+
+  def _missing_services(self) -> list[str]:
+    missing = []
+    for service in CTRL_ESSENTIAL_SERVICES:
+      if not self.sm.alive[service] or not self.sm.freq_ok[service] or not self.sm.valid[service]:
+        missing.append(service)
+    return missing
+
+  def _emit_perf_trace(self, frame_id: int, loop_dt_us: int, update_us: int, state_control_us: int,
+                       publish_us: int, tail_work_us: int, rk_remaining_us: int) -> None:
+    missing_services = self._missing_services()
+    flags = 0
+    if missing_services:
+      flags |= CTRL_FLAG_MISSING_INPUTS
+    if rk_remaining_us < 0:
+      flags |= CTRL_FLAG_RK_OVERRUN
+
+    sample = PerfSample(
+      frame_id=frame_id,
+      loop_dt_us=loop_dt_us,
+      update_us=update_us,
+      state_control_us=state_control_us,
+      publish_us=publish_us,
+      tail_work_us=tail_work_us,
+      rk_remaining_us=rk_remaining_us,
+      flags=flags,
+    )
+    self._perf_ring.push(sample)
+
+    slow_phase = max(update_us, state_control_us, publish_us, tail_work_us)
+    if loop_dt_us < CTRL_LOOP_WARN_US and slow_phase < CTRL_PHASE_WARN_US and tail_work_us < CTRL_TAIL_WARN_US and flags == 0:
+      return
+
+    if loop_dt_us >= CTRL_LOOP_SEVERE_US:
+      severity = "critical"
+    elif loop_dt_us >= CTRL_LOOP_BAD_US or rk_remaining_us < -10_000:
+      severity = "error"
+    else:
+      severity = "warning"
+
+    detail = (
+      f"update_us={update_us} state_control_us={state_control_us} publish_us={publish_us} "
+      f"tail_work_us={tail_work_us} rk_remaining_us={rk_remaining_us}"
+    )
+    self._perf.emit(
+      "controlsd_slow_loop",
+      severity=severity,
+      frame_id=frame_id,
+      total_time_us=loop_dt_us,
+      rk_remaining_us=rk_remaining_us,
+      flags=flags,
+      samples=self._perf_ring.snapshot(),
+      missing_services=missing_services,
+      detail=detail,
+      min_interval_s=0.25,
+    )
+
+  def run(self):
+    rk = Ratekeeper(100, print_delay_threshold=None)
+    while True:
+      started_ns = time.monotonic_ns()
+      checkpoint_ns = started_ns
+      self.update()
+      update_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      CC, lac_log = self.state_control()
+      state_control_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      self.publish(CC, lac_log)
+      publish_us = (time.monotonic_ns() - checkpoint_ns) // 1000
+      checkpoint_ns = time.monotonic_ns()
+
+      tail_work_us = self._tail_work()
+      loop_dt_us = (time.monotonic_ns() - started_ns) // 1000
+      rk_remaining_us = int(rk.remaining * 1_000_000)
+      self._emit_perf_trace(self.sm.frame, int(loop_dt_us), int(update_us), int(state_control_us),
+                            int(publish_us), int(tail_work_us), rk_remaining_us)
+      rk.monitor_time()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  lock_memory()
+  controls = Controls()
+  controls.run()
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/iq_controls_layer.py

@@ -0,0 +1,119 @@
+"""
+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 iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log, custom
+
+from iqdbc.car import structs
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
+from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
+from iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
+from 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 = sm["lateralDelay"].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['vehicleParameters']
+    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/curvature_lookahead.py

@@ -0,0 +1,21 @@
+import math
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+LOOKAHEAD_SECONDS = 0.20
+
+
+def get_lookahead_curvature(model_v2, v_ego: float, lat_delay: float) -> float | None:
+  try:
+    yaws = model_v2.orientation.z
+    yaw_rates = model_v2.orientationRate.z
+    if len(yaws) < CONTROL_N or len(yaw_rates) < CONTROL_N:
+      return None
+    if not all(math.isfinite(value) for value in yaws) or not all(math.isfinite(value) for value in yaw_rates):
+      return None
+    horizon = max(0.0, lat_delay) + LOOKAHEAD_SECONDS
+    return get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, v_ego, horizon)
+  except (AttributeError, TypeError, ValueError):
+    return None

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 iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.iqmodeld.config 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/desire_helper.py

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

iqpilot/selfdrive/controls/lib/drive_helpers.py

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

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

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

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

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

@@ -0,0 +1,182 @@
+"""
+Lateral Edge Guard uses the model's lateral road-edge geometry to withhold lane
+changes that lack room for a target lane. The model standard deviation remains
+in metres: measurements above the validity limit are rejected, while valid
+measurements use a two-sigma lower confidence bound for conservative clearance.
+Unavailable geometry briefly holds the last output, then fails open because a
+model dropout is not geometric evidence of a nearby edge.
+"""
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Any
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.constants import CV
+from iqpilot.common.swaglog import cloudlog
+
+
+MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS  # Matches the lane-change speed gate and excludes parking manoeuvres.
+MAX_VALID_ROAD_EDGE_STD_M = 1.0  # A 2-sigma bound beyond 2 m cannot distinguish an adjacent 3.5 m lane reliably.
+EDGE_CONFIDENCE_SIGMA = 2.0  # 97.7% one-sided confidence under the model's Gaussian uncertainty assumption.
+ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0  # Ignore near-field edge points dominated by vehicle-body perspective.
+ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0  # Covers about 2 s at the 20 m/s model-training reference speed.
+LANE_CENTER_OFFSET_M = 3.5  # Typical freeway lane width and the target-centre lateral displacement.
+# CarParams exposes neither width nor track; 0.95 m is half of an assumed conservative 1.90 m body width.
+VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
+EDGE_CLEARANCE_MARGIN_M = 0.25  # Additional lateral separation between the vehicle body and detected road edge.
+REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
+BLOCK_DEBOUNCE_S = 0.30  # Six model frames reject a transient close-edge prediction before blocking.
+CLEAR_DEBOUNCE_S = 0.50  # Ten model frames make release slower than assertion for conservative hysteresis.
+UNAVAILABLE_HOLD_S = 0.50  # Ten model frames bridge a short model-data dropout before failing open.
+TIMER_EPSILON_S = 1e-9  # Floating-point comparison tolerance, far below one model tick.
+
+LaneChangeDirection = log.LaneChangeDirection
+LateralEdgeBlock = custom.IQLateralEdgeBlock
+
+
+class RoadEdgeDataState(IntEnum):
+  VALID = 0
+  UNAVAILABLE = 1
+  INVALID = 2
+
+
+@dataclass(frozen=True, slots=True)
+class RoadEdgeMeasurement:
+  state: RoadEdgeDataState
+  lateral_distance_m: float | None = None
+  conservative_distance_m: float | None = None
+  should_block: bool | None = None
+
+
+@dataclass(frozen=True, slots=True)
+class _SideState:
+  blocked: bool = False
+  block_timer_s: float = 0.0
+  clear_timer_s: float = 0.0
+  unavailable_timer_s: float = 0.0
+  fallback_reported: bool = False
+
+
+def evaluate_road_edge(edge: Any, std_m: Any, direction: int) -> RoadEdgeMeasurement:
+  if edge is None or std_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    xs = edge.x
+    ys = edge.y
+    count = len(xs)
+    y_count = len(ys)
+  except (AttributeError, TypeError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  if count == 0 or y_count != count:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  try:
+    std = float(std_m)
+  except (TypeError, ValueError):
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+  if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M:
+    return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+
+  lateral_distance_m: float | None = None
+  for idx in range(count):
+    try:
+      x_m = float(xs[idx])
+      y_m = float(ys[idx])
+    except (IndexError, TypeError, ValueError):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not math.isfinite(x_m) or not math.isfinite(y_m):
+      return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M:
+      continue
+    if ((direction == LaneChangeDirection.left and y_m >= 0.0) or
+        (direction == LaneChangeDirection.right and y_m <= 0.0)):
+      return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
+    distance_m = abs(y_m)
+    lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m)
+
+  if lateral_distance_m is None:
+    return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
+  return RoadEdgeMeasurement(
+    RoadEdgeDataState.VALID,
+    lateral_distance_m,
+    conservative_distance_m,
+    conservative_distance_m < REQUIRED_ROAD_EDGE_DISTANCE_M,
+  )
+
+
+def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool,
+                    dt_s: float) -> tuple[_SideState, bool]:
+  if not speed_active:
+    return _SideState(), False
+
+  if measurement.state == RoadEdgeDataState.UNAVAILABLE:
+    unavailable_timer_s = state.unavailable_timer_s + dt_s
+    if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S:
+      return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s,
+                        fallback_reported=state.fallback_reported), False
+    fallback_started = not state.fallback_reported
+    return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started
+
+  should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False
+  if should_block == state.blocked:
+    return _SideState(blocked=state.blocked), False
+
+  if should_block:
+    block_timer_s = state.block_timer_s + dt_s
+    if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S:
+      return _SideState(blocked=True), False
+    return _SideState(block_timer_s=block_timer_s), False
+
+  clear_timer_s = state.clear_timer_s + dt_s
+  if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S:
+    return _SideState(), False
+  return _SideState(blocked=True, clear_timer_s=clear_timer_s), False
+
+
+class LateralEdgeGuard:
+  def __init__(self) -> None:
+    self._left = _SideState()
+    self._right = _SideState()
+    self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+    self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
+
+  @staticmethod
+  def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]:
+    if modeldata is None:
+      return None, None
+    try:
+      edges = modeldata.roadEdges
+      stds = modeldata.roadEdgeStds
+      if len(edges) <= side_index or len(stds) <= side_index:
+        return None, None
+      return edges[side_index], stds[side_index]
+    except (AttributeError, TypeError):
+      return None, None
+
+  def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
+    dt = max(float(dt_s), 0.0)
+    left_edge, left_std = self._model_side(modeldata, 0)
+    right_edge, right_std = self._model_side(modeldata, 1)
+    self.left_measurement = evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left)
+    self.right_measurement = evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right)
+    speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
+    self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
+    self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)
+    if left_fallback:
+      cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+    if right_fallback:
+      cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
+
+  def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock:
+    if direction == LaneChangeDirection.left and self._left.blocked:
+      return LateralEdgeBlock.left
+    if direction == LaneChangeDirection.right and self._right.blocked:
+      return LateralEdgeBlock.right
+    return LateralEdgeBlock.none

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 iqpilot.cereal.messaging as messaging
+from iqpilot.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 iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_MDL
+from 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 iqpilot.cereal import custom
+import iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from 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 iqpilot.cereal import messaging
+from numpy import interp
+from iqdbc.car import structs
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from 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 iqpilot.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/iq_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 iqpilot.cereal import messaging, custom
+from iqdbc.car import structs
+from iqpilot.common.constants import CV
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
+from iqpilot.selfdrive.controls.lib.iq_dynamic.engine import IQDynamicController
+from iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import IQConstants
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import EndToEndAlertEngine
+from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_ADAPT_ACC
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
+from 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, v_cruise: 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,
+      LongitudinalPlanSource.speedLimitAssist: slc_v_cruise,
+    }
+    if self.nav_valid:
+      targets[LongitudinalPlanSource.nav] = self.nav_speed_target
+
+    self.source = min(targets, key=lambda k: targets[k])
+    self.output_v_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
+
+  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/latcontrol.py

@@ -0,0 +1,31 @@
+import numpy as np
+from abc import abstractmethod, ABC
+from iqpilot.selfdrive.locationd.helpers import Pose
+
+
+class LatControl(ABC):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    self.dt = dt
+    self.sat_limit = CP.steerLimitTimer
+    self.sat_time = 0.
+    self.sat_check_min_speed = 10.
+
+    # we define the steer torque scale as [-1.0...1.0]
+    self.steer_max = 1.0
+
+  @abstractmethod
+  def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, calibrated_pose: Pose,
+             curvature_limited: bool, lat_delay: float):
+    pass
+
+  def reset(self):
+    self.sat_time = 0.
+
+  def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
+    # Saturated only if control output is not being limited by car torque/angle rate limits
+    if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
+      self.sat_time += self.dt
+    else:
+      self.sat_time -= self.dt
+    self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
+    return self.sat_time > (self.sat_limit - 1e-3)

iqpilot/selfdrive/controls/lib/latcontrol_angle.py

@@ -0,0 +1,52 @@
+import math
+
+from iqpilot.cereal import log
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+
+# TODO This is speed dependent
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+
+
+class LatControlAngle(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.sat_check_min_speed = 5.
+    self.use_steer_limited_by_safety = CP.brand == "tesla"
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+    self.target_curvature_last = 0.0
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    angle_log = log.ControlsState.LateralAngleState.new_message()
+
+    # the rack has ~70 ms of dead time before the wheel moves (measured on VW MQB), so track the
+    # curvature the path will need after lat_delay rather than the one it needs now. controlsd passes
+    # lookahead_curvature=None in maneuver mode, which keeps the maneuver report measuring raw response.
+    # controlsd only runs clip_curvature on desired_curvature, so the lookahead has to be bounded here
+    # or the ISO jerk/accel limits are bypassed on the way to the rack.
+    target_curvature = desired_curvature
+    if active and self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature, _ = clip_curvature(CS.vEgo, self.target_curvature_last, lookahead_curvature, params.roll)
+    self.target_curvature_last = target_curvature
+
+    if not active:
+      angle_log.active = False
+      angle_steers_des = float(CS.steeringAngleDeg)
+    else:
+      angle_log.active = True
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-target_curvature, CS.vEgo, params.roll))
+      angle_steers_des += params.angleOffsetDeg
+
+    if self.use_steer_limited_by_safety:
+      # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
+      angle_control_saturated = steer_limited_by_safety
+    else:
+      # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
+      # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved
+      angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
+    angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    angle_log.steeringAngleDesiredDeg = angle_steers_des
+    return 0, float(angle_steers_des), angle_log

iqpilot/selfdrive/controls/lib/latcontrol_pid.py

@@ -0,0 +1,50 @@
+import math
+
+from iqpilot.cereal import log
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.common.pid import PIDController
+
+
+class LatControlPID(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                             pos_limit=self.steer_max, neg_limit=-self.steer_max)
+    self.ff_factor = CP.lateralTuning.pid.kf
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralPIDState.new_message()
+    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
+
+    pid_log.steeringAngleDesiredDeg = angle_steers_des
+    pid_log.angleError = error
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+
+    else:
+      # offset does not contribute to resistive torque
+      ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+
+      output_torque = self.pid.update(error,
+                                feedforward=ff,
+                                speed=CS.vEgo,
+                                freeze_integrator=freeze_integrator)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(output_torque)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    return output_torque, angle_steers_des, pid_log

iqpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -0,0 +1,636 @@
+import json
+import math
+import os
+import tomllib
+from collections import deque
+from difflib import SequenceMatcher
+from importlib.resources import files
+
+import numpy as np
+
+from iqpilot.cereal import log, custom  # noqa: F401  (custom kept available for downstream imports)
+from iqdbc.car import structs
+from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from iqdbc.lvbs.car.interfaces import LatControlInputs
+from iqdbc.lvbs.car.iq_lateral import get_friction as get_friction_in_torque_space
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import LateralAccelerationSlewLimiter
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.iqmodeld.parser import safe_exp
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import NavTorquePulseBrain
+
+
+# ===== locator =====
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "iqpilot", "iqpilot_iq_nnff_models", "neural_network_lateral_control")
+TORQUE_NN_MODEL_SUBSTITUTE_PATH = files("iqdbc").joinpath("car", "torque_data", "substitute.toml")
+MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
+
+# A candidate must reach this score for the fingerprint(+fw) match to count as exact.
+_EXACT_THRESHOLD = 0.99
+# Below this, we fall back to the next candidate ladder rung.
+_ACCEPT_THRESHOLD = 0.9
+
+
+def _score(a: str, b: str) -> float:
+  return SequenceMatcher(None, a, b).ratio()
+
+
+def _best_model_for(candidate: str) -> tuple[str | None, float]:
+  """Highest-scoring model file for a candidate string; (path, score)."""
+  best_path, best_score = None, -1.0
+  if not os.path.isdir(TORQUE_NN_MODEL_PATH):
+    return best_path, best_score
+  for entry in os.listdir(TORQUE_NN_MODEL_PATH):
+    if not entry.endswith(".json"):
+      continue
+    score = _score(os.path.splitext(entry)[0], candidate)
+    if score > best_score:
+      best_path, best_score = os.path.join(TORQUE_NN_MODEL_PATH, entry), score
+  return best_path, best_score
+
+
+def _substitute_for(fingerprint: str) -> str:
+  with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
+    table = tomllib.load(f)
+  return table.get(fingerprint, fingerprint)
+
+
+def _eps_suffix(CP: structs.CarParams) -> str:
+  eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+  return eps_fw.replace("\\", "") if len(eps_fw) > 3 else ""
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
+  """Pick the closest NNFF model for this car.
+
+  Angle-steered cars always get MOCK. Otherwise walk a candidate ladder —
+  fingerprint+eps-fw, then fingerprint, then the substitute mapping — accepting
+  the first rung that clears the match threshold; the top rung landing at ~1.0
+  marks an exact (non-fuzzy) match.
+  """
+  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
+    return MOCK_MODEL_PATH, "MOCK", False
+
+  fingerprint = CP.carFingerprint
+  suffix = _eps_suffix(CP)
+
+  # rung 1: fingerprint + eps fw (when we have a usable fw string)
+  if suffix:
+    path, score = _best_model_for(f"{fingerprint} {suffix}")
+    if path is not None and fingerprint in path and score >= _ACCEPT_THRESHOLD:
+      name = os.path.splitext(os.path.basename(path))[0]
+      return path, name, score >= _EXACT_THRESHOLD
+
+  # rung 2: fingerprint alone
+  path, score = _best_model_for(fingerprint)
+  if path is not None and fingerprint in path and score >= _ACCEPT_THRESHOLD:
+    name = os.path.splitext(os.path.basename(path))[0]
+    return path, name, score >= _EXACT_THRESHOLD
+
+  # rung 3: substitute mapping — never exact
+  path, _ = _best_model_for(_substitute_for(fingerprint))
+  name = os.path.splitext(os.path.basename(path))[0] if path else "MOCK"
+  return (path or MOCK_MODEL_PATH), name, False
+
+# ===== network =====
+
+# The JSON model format (Twilsonco NNFF) is external: unicode 'σ' names the
+# sigmoid activation, weights/biases live under keys suffixed _W/_b, and the
+# input normalisation is (x - mean) / std. We translate names through a registry
+# and keep the mean/std transposed once at load.
+_MIN_INPUT_LEN = 2
+_FRICTION_PROBE = (10.0, 0.0, 0.2)
+_FRICTION_THRESHOLD = 0.1
+
+_ACTIVATION_ALIASES = {"σ": "sigmoid"}
+
+
+def _sigmoid(x):
+  return 1.0 / (1.0 + safe_exp(-x))
+
+
+def _identity(x):
+  return x
+
+
+_ACTIVATIONS = {"sigmoid": _sigmoid, "identity": _identity}
+
+
+def _resolve_activation(name: str):
+  for symbol, canonical in _ACTIVATION_ALIASES.items():
+    name = name.replace(symbol, canonical)
+  fn = _ACTIVATIONS.get(name)
+  if fn is None:
+    raise ValueError(f"Unknown activation: {name}")
+  return fn
+
+
+def _pick(layer: dict, suffix: str):
+  key = next(k for k in layer if k.endswith(suffix))
+  return np.array(layer[key], dtype=np.float32).T
+
+
+class NNTorqueModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file) as f:
+      params = json.load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+
+    self._weights = []
+    self._biases = []
+    self._activations = []
+    for layer in params["layers"]:
+      weight = _pick(layer, "_W")
+      bias = np.zeros_like(_pick(layer, "_b")) if zero_bias else _pick(layer, "_b")
+      self._weights.append(weight)
+      self._biases.append(bias)
+      self._activations.append(_resolve_activation(layer["activation"]))
+
+    self.friction_override = self.evaluate(list(_FRICTION_PROBE)) < _FRICTION_THRESHOLD
+
+  def forward(self, x):
+    for weight, bias, activation in zip(self._weights, self._biases, self._activations, strict=True):
+      x = activation(x.dot(weight) + bias)
+    return x
+
+  def evaluate(self, input_array):
+    if len(input_array) != self.input_size:
+      if len(input_array) < _MIN_INPUT_LEN:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+      input_array = input_array + [0] * (self.input_size - len(input_array))
+    x = (np.array(input_array, dtype=np.float32) - self.input_mean) / self.input_std
+    return float(self.forward(x)[0, 0])
+
+  # names kept for callers/tests that introspected the old implementation
+  @staticmethod
+  def sigmoid(x):
+    return _sigmoid(x)
+
+  @staticmethod
+  def identity(x):
+    return _identity(x)
+
+# ===== brain =====
+
+PLAN_SAMPLE_START = 5
+LAG_EXTRA_S = 0.0
+
+BASE_P = 0.8
+BASE_I = 0.15
+PID_SPEED_BP = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+PID_P_GAIN = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, BASE_P]
+
+_JERK_FALLBACK_IDX = 16   # T_IDXS index used when nothing exceeds the lookahead horizon
+
+
+def sign(value: float) -> float:
+  if value > 0.0:
+    return 1.0
+  if value < 0.0:
+    return -1.0
+  return 0.0
+
+
+polarity = sign
+
+
+def _pointwise_jerk(accel_trace, dt_trace) -> list:
+  """Finite-difference jerk from an acceleration trace over per-step dt."""
+  delta = np.diff(accel_trace)
+  span = min(len(delta), len(dt_trace))
+  if span <= 0:
+    return []
+  return (delta[:span] / np.array(dt_trace)[:span]).tolist()
+
+
+def sign_locked_min(future_vals, seed_val):
+  """Smallest-magnitude jerk over the horizon, but only if the whole horizon
+  agrees in sign with the seed; a sign disagreement collapses to 0."""
+  if not future_vals:
+    return seed_val
+  agreeing = [v for v in future_vals if sign(v) == sign(seed_val)]
+  if len(agreeing) < len(future_vals):
+    return 0.0
+  return min(agreeing + [seed_val], key=abs)
+
+
+class PilotLateralBrain:
+  """Shared lateral-control scaffolding: PID core, model snapshot, and the
+  forward-looking jerk/friction estimates the feed-forward controllers build on."""
+
+  def __init__(self, torque_ctrl, cp, cp_iq, car_if):
+    del cp_iq
+    self.lac_torque = torque_ctrl
+    self.torque_from_lateral_accel_in_torque_space = car_if.torque_from_lateral_accel_in_torque_space()
+
+    self.model_v2 = None
+    self.model_valid = False
+
+    self.jerk_now = 0.0
+    self.jerk_goal = 0.0
+    self.jerk_obs = 0.0
+    self.jerk_ahead = 0.0
+
+    # per-cycle control snapshot
+    self._ff = 0.0
+    self._pid = PIDController([PID_SPEED_BP, PID_P_GAIN], BASE_I)
+    self._pid_log = None
+    self._accel_goal = 0.0
+    self._accel_obs = 0.0
+    self._roll_g = 0.0
+    self._deadband = 0.0
+    self._want_la = 0.0
+    self._have_la = 0.0
+    self._want_cv = 0.0
+    self._have_cv = 0.0
+    self._grav_la = 0.0
+    self._capped = False
+    self._out_tq = 0.0
+
+    # friction-lookahead tuning
+    self.friction_look_ahead_v = [1.4, 2.0]
+    self.friction_look_ahead_bp = [9.0, 30.0]
+    self.lat_jerk_friction_factor = 0.4
+    self.lat_accel_friction_factor = 0.7
+
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = cp.steerActuatorDelay + LAG_EXTRA_S
+
+  def update_model_v2(self, model_packet):
+    self.model_v2 = model_packet
+    self.model_valid = model_packet is not None and len(model_packet.orientation.x) >= CONTROL_N
+
+  def update_lateral_lag(self, lag):
+    self.desired_lat_jerk_time = max(0.01, lag) + LAG_EXTRA_S
+
+  def update_friction_input(self, target_val, measured_val):
+    error = target_val - measured_val
+    return self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * self.jerk_ahead
+
+  def _measured_jerk(self, car_state, vehicle_model) -> float:
+    curvature_rate = -vehicle_model.calc_curvature(math.radians(car_state.steeringRateDeg), car_state.vEgo, 0.0)
+    return curvature_rate * car_state.vEgo ** 2
+
+  def _horizon_index(self, speed_mps: float) -> int:
+    lookahead = np.interp(speed_mps, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+    return next((i for i, t in enumerate(ModelConstants.T_IDXS) if t > lookahead), _JERK_FALLBACK_IDX)
+
+  def _reset_jerk_estimates(self, car_state, vehicle_model):
+    self.jerk_now = self._measured_jerk(car_state, vehicle_model)
+    self.jerk_goal = 0.0
+    self.jerk_obs = 0.0
+    self.jerk_ahead = 0.0
+
+  def update_calculations(self, car_state, vehicle_model, desired_lat_accel):
+    self._reset_jerk_estimates(car_state, vehicle_model)
+    if not self.model_valid:
+      return
+
+    accel_y = self.model_v2.acceleration.y
+    horizon_accel = np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, accel_y)
+    desired_jerk = (horizon_accel - desired_lat_accel) / self.desired_lat_jerk_time
+
+    forecast = _pointwise_jerk(accel_y, self.t_diffs)
+    window = forecast[PLAN_SAMPLE_START:self._horizon_index(car_state.vEgo)]
+    self.jerk_ahead = sign_locked_min(window, desired_jerk)
+
+    if self.jerk_ahead == 0.0:
+      self.jerk_now = 0.0
+      self.lat_accel_friction_factor = 1.0
+
+    self.jerk_goal = self.lat_jerk_friction_factor * self.jerk_ahead
+    self.jerk_obs = self.lat_jerk_friction_factor * self.jerk_now
+
+
+TorqueBrainCore = PilotLateralBrain
+
+# ===== nnff =====
+
+LOW_SPEED_X = [0, 10, 20, 30]
+LOW_SPEED_Y = [12, 3, 1, 0]
+
+# NNFF input layout expected by the trained models (dictated by the model data):
+# 4 scalars (v_ego, target, jerk, roll) + past/future target repeats + past/future rolls.
+_ERROR_BLEND_BP = [1.0, 2.0]
+_ERROR_BLEND_V = [0.0, 1.0]
+
+
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+
+class _HistoryWindow:
+  """Rolling past/future sample windows the NNFF vector is assembled from."""
+
+  def __init__(self, past_times, future_times, jerk_time):
+    self.past_times = past_times
+    self.future_times = future_times
+    self.jerk_time = jerk_time
+    self.nn_future_times = [t + jerk_time for t in future_times]
+
+    check_frames = [int(abs(t) * 100) for t in past_times]
+    self.frame_offsets = [check_frames[0] - f for f in check_frames]
+    maxlen = check_frames[0]
+    self.roll = deque(maxlen=maxlen)
+    self.lat_accel_desired = deque(maxlen=maxlen)
+    self.past_future_len = len(past_times) + len(self.nn_future_times)
+
+  def refresh_lag(self, jerk_time):
+    self.jerk_time = jerk_time
+    self.nn_future_times = [t + jerk_time for t in self.future_times]
+
+  def push(self, roll, lat_accel_desired):
+    self.roll.append(roll)
+    self.lat_accel_desired.append(lat_accel_desired)
+
+  def _sample(self, buf):
+    return [buf[min(len(buf) - 1, i)] for i in self.frame_offsets]
+
+  def past_rolls(self):
+    return self._sample(self.roll)
+
+  def past_lat_accels(self):
+    return self._sample(self.lat_accel_desired)
+
+
+class NeuralNetworkFeedForward(PilotLateralBrain):
+  def __init__(self, lac_torque, CP, CP_IQ, CI):
+    super().__init__(lac_torque, CP, CP_IQ, CI)
+    self.params = Params()
+    self.enabled = self.params.get_bool("NeuralNetworkFeedForward")
+    # NNFF applies only when a real trained model for this car is present on disk.
+    # No models shipped (or no match / MOCK) -> skip NNFF entirely and fall back to
+    # the stock torque feed-forward. Models are re-added as they are retrained.
+    self.has_nn_model = (CP_IQ.iqLateralNet.model.path != MOCK_MODEL_PATH
+                         and os.path.isfile(CP_IQ.iqLateralNet.model.path))
+    self.model = NNTorqueModel(CP_IQ.iqLateralNet.model.path) if self.has_nn_model else None
+    self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+    self.pitch_last = 0.0
+
+    self.future_times = [0.3, 0.6, 1.0, 1.5]
+    self._window = _HistoryWindow([-0.3, -0.2, -0.1], self.future_times, self.desired_lat_jerk_time)
+    self.nav_torque_pulse = NavTorquePulseBrain(lac_torque)
+
+  # -- back-compat views onto the history window -------------------------------
+  @property
+  def nn_future_times(self):
+    return self._window.nn_future_times
+
+  @property
+  def past_future_len(self):
+    return self._window.past_future_len
+
+  @property
+  def _nnff_enabled(self):
+    return self.enabled and self.model_valid and self.has_nn_model
+
+  def update_limits(self):
+    if not self._nnff_enabled:
+      return
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
+  def update_lateral_lag(self, lag):
+    super().update_lateral_lag(lag)
+    self._window.refresh_lag(self.desired_lat_jerk_time)
+
+  # -- torque-space feedforward (non-NN path used for error scaling) -----------
+  def _torque_space(self, lateral_accel, CS, gravity_adjusted):
+    return self.torque_from_lateral_accel_in_torque_space(
+      LatControlInputs(lateral_accel, self._roll_g, CS.vEgo, CS.aEgo),
+      self.lac_torque.torque_params, gravity_adjusted=gravity_adjusted)
+
+  def update_feedforward_torque_space(self, CS):
+    torque_from_setpoint = self._torque_space(self._accel_goal, CS, gravity_adjusted=False)
+    torque_from_measurement = self._torque_space(self._accel_obs, CS, gravity_adjusted=False)
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+    self._ff = self._torque_space(self._grav_la, CS, gravity_adjusted=True)
+    self._ff += get_friction_in_torque_space(self._want_la - self._have_la,
+                                             self._deadband, FRICTION_THRESHOLD,
+                                             self.lac_torque.torque_params)
+
+  def update_output_torque(self, CS):
+    freeze_integrator = self._capped or CS.steeringPressed or CS.vEgo < 5
+    self._out_tq = self._pid.update(self._pid_log.error, feedforward=self._ff,
+                                           speed=CS.vEgo, freeze_integrator=freeze_integrator)
+
+  # -- NN input assembly -------------------------------------------------------
+  def _effective_roll(self, params, calibrated_pose):
+    roll = params.roll
+    if calibrated_pose is not None:
+      pitch = self.pitch.update(calibrated_pose.orientation.pitch)
+      roll = roll_pitch_adjust(roll, pitch)
+      self.pitch_last = pitch
+    return roll
+
+  def _future_rolls(self, roll, adjusted_future_times):
+    return [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
+                              np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last)
+            for t in adjusted_future_times]
+
+  def _future_lat_accels(self, adjusted_future_times):
+    return [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in adjusted_future_times]
+
+  def _query(self, lead_scalar, jerk_scalar, tail):
+    """Build one model input from its 4 leading scalars + the shared tail, then
+    run the interpreter. `tail` is (repeat_value_or_None, extra_pairs...)."""
+    head = [self._v, lead_scalar, jerk_scalar, self._roll]
+    return self.model.evaluate(head + tail)
+
+  def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
+    if not self._nnff_enabled:
+      return
+
+    self.update_feedforward_torque_space(CS)
+    creep = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
+    self._accel_goal = self._want_la + creep * self._want_cv
+    self._accel_obs = self._have_la + creep * self._have_cv
+
+    # cache per-cycle scalars the query builder reads
+    self._v = CS.vEgo
+    self._roll = self._effective_roll(params, calibrated_pose)
+    self._window.push(self._roll, self._want_la)
+
+    horizon = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+    roll_ctx = self._window.past_rolls() + self._future_rolls(self._roll, horizon)
+    accel_ctx = self._window.past_lat_accels() + self._future_lat_accels(horizon)
+
+    goal_torque = self._query(self._accel_goal, self.jerk_goal, [self._accel_goal] * self.past_future_len + roll_ctx)
+    obs_torque = self._query(self._accel_obs, self.jerk_obs, [self._accel_obs] * self.past_future_len + roll_ctx)
+    self._pid_log.error = goal_torque - obs_torque
+    self._apply_error_blend()
+
+    friction_input = self.update_friction_input(self._accel_goal, self._accel_obs)
+    self._ff = self._query(self._want_la, friction_input, accel_ctx + roll_ctx)
+    if self.model.friction_override:
+      self._pid_log.error += get_friction(friction_input, self._deadband,
+                                          FRICTION_THRESHOLD, self.lac_torque.torque_params)
+
+    self.update_output_torque(CS)
+
+  def _apply_error_blend(self):
+    blend = float(np.interp(abs(self._want_la), _ERROR_BLEND_BP, _ERROR_BLEND_V))
+    if blend <= 0.0:
+      return
+    # error query carries a 0.0 roll slot (not the live roll), so build it directly
+    from_error = self.model.evaluate([self._v, self._accel_goal - self._accel_obs,
+                                      self.jerk_goal - self.jerk_obs, 0.0])
+    live = self._pid_log.error
+    if sign(live) == sign(from_error) and abs(live) < abs(from_error):
+      self._pid_log.error = live * (1.0 - blend) + from_error * blend
+
+  # -- per-cycle snapshot + entry point ----------------------------------------
+  def _snapshot_cycle(self, feedforward_seed, pid_core, pid_trace, torque_goal, torque_actual, roll_bias,
+                      deadzone, lat_accel_goal, lat_accel_actual, curvature_goal, curvature_actual,
+                      gravity_lat_accel, safety_limited, torque_output) -> None:
+    self._ff = feedforward_seed
+    self._pid = pid_core
+    self._pid_log = pid_trace
+    self._accel_goal = torque_goal
+    self._accel_obs = torque_actual
+    self._roll_g = roll_bias
+    self._deadband = deadzone
+    self._want_la = lat_accel_goal
+    self._have_la = lat_accel_actual
+    self._want_cv = curvature_goal
+    self._have_cv = curvature_actual
+    self._grav_la = gravity_lat_accel
+    self._capped = safety_limited
+    self._out_tq = torque_output
+
+  def update(self, car_state, vehicle_model, pid_core, calibrator, feedforward_seed, pid_trace,
+             torque_goal, torque_actual, calibrated_pose, roll_bias, lat_accel_goal, lat_accel_actual,
+             deadzone, gravity_lat_accel, curvature_goal, curvature_actual, safety_limited, torque_output):
+    self._snapshot_cycle(feedforward_seed, pid_core, pid_trace, torque_goal, torque_actual, roll_bias,
+                         deadzone, lat_accel_goal, lat_accel_actual, curvature_goal, curvature_actual,
+                         gravity_lat_accel, safety_limited, torque_output)
+    self.update_calculations(car_state, vehicle_model, lat_accel_goal)
+    self.update_neural_network_feedforward(car_state, calibrator, calibrated_pose)
+    self._out_tq = self.nav_torque_pulse.nudge_output_torque(True, car_state, self._out_tq)
+    return self._pid_log, self._out_tq
+
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects the
+# proportional gain is increased at low speeds by the PID controller.
+# Additionally, there is friction in the steering wheel that needs
+# to be overcome to move it at all, this is compensated for too.
+
+KP = 0.8
+KI = 0.15
+
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.2
+JERK_LOOKAHEAD_SECONDS = 0.19
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
+    self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
+    self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.lateral_acceleration_slew_limiter = LateralAccelerationSlewLimiter(Params().get_bool("IQLateralAccelSlew"))
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+
+    self.nnff_assist = NeuralNetworkFeedForward(self, CP, CP_IQ, CI)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    target_curvature = desired_curvature
+    if self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature = lookahead_curvature
+    if not active and self.lateral_acceleration_slew_limiter.enabled:
+      self.lateral_acceleration_slew_limiter.reset(target_curvature * CS.vEgo ** 2)
+    limited_curvature = self.lateral_acceleration_slew_limiter.update(target_curvature, CS.vEgo, self.dt)
+    future_desired_lateral_accel = limited_curvature * CS.vEgo ** 2
+    self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+
+    roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+    curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+    lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
+    expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+    setpoint = expected_lateral_accel
+    error = setpoint - measurement
+
+    lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
+    raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
+    desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+    ff = gravity_adjusted_future_lateral_accel
+    # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
+    ff -= self.torque_params.latAccelOffset
+    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
+      pid_log.error = float(error)
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.nnff_assist.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                       future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
+                                                       limited_curvature, measured_curvature, steer_limited_by_safety, output_torque)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque) # TODO: log lat accel?
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    # TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log

iqpilot/selfdrive/controls/lib/latcontrol_torque_pq.py

@@ -0,0 +1,131 @@
+import math
+import numpy as np
+from collections import deque
+
+from iqpilot.cereal import log
+from iqdbc.car.lateral import get_friction
+from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import LateralAccelerationSlewLimiter
+from iqpilot.common.pid import PIDController
+
+FRICTION_THRESHOLD_PQ = 1.0
+KP = 0.8
+KI = 0.15
+
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.5
+JERK_LOOKAHEAD_SECONDS = 0.34
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+DEFAULT_LAT_ACCEL_FACTOR = 2.2
+DEFAULT_LAT_ACCEL_OFFSET = -0.13
+DEFAULT_FRICTION = 0.1
+FREEZE_LIVE_TORQUE_PARAMS = True
+
+ASSIST_COMPENSATION = True
+ASSIST_SPEEDS_KPH = [0.0, 50.0, 120.0]
+ASSIST_GAIN = [0.688, 0.883, 1.211]
+ASSIST_REF_KPH = 100.0
+
+
+def _assist_comp(v_ego_ms):
+  import numpy as _np
+  ref = _np.interp(ASSIST_REF_KPH, ASSIST_SPEEDS_KPH, ASSIST_GAIN)
+  g = _np.interp(v_ego_ms * 3.6, ASSIST_SPEEDS_KPH, ASSIST_GAIN)
+  return float(_np.clip(ref / g, 0.7, 1.6))
+
+
+class LatControlTorquePQ(LatControl):
+  def __init__(self, CP, CP_IQ, CI, dt):
+    super().__init__(CP, CP_IQ, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_params.latAccelFactor = DEFAULT_LAT_ACCEL_FACTOR
+    self.torque_params.latAccelOffset = DEFAULT_LAT_ACCEL_OFFSET
+    self.torque_params.friction = DEFAULT_FRICTION
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len)
+    self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
+    self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+    self.lateral_acceleration_slew_limiter = LateralAccelerationSlewLimiter(Params().get_bool("IQLateralAccelSlew"))
+    self.curvature_lookahead_enabled = Params().get_bool("IQLateralCurvatureLookahead")
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    if FREEZE_LIVE_TORQUE_PARAMS:
+      return
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay,
+             lookahead_curvature=None):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    target_curvature = desired_curvature
+    if self.curvature_lookahead_enabled and lookahead_curvature is not None:
+      target_curvature = lookahead_curvature
+    if not active and self.lateral_acceleration_slew_limiter.enabled:
+      self.lateral_acceleration_slew_limiter.reset(target_curvature * CS.vEgo ** 2)
+    limited_curvature = self.lateral_acceleration_slew_limiter.update(target_curvature, CS.vEgo, self.dt)
+    future_desired_lateral_accel = limited_curvature * CS.vEgo ** 2
+    self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+
+    roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+    curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+    lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
+    expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+    setpoint = expected_lateral_accel
+    error = setpoint - measurement
+
+    lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len + 1, -2))
+    raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx + 1] - self.lat_accel_request_buffer[lookahead_idx - 1]) / (2 * self.dt)
+    desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+    ff = gravity_adjusted_future_lateral_accel
+    ff -= self.torque_params.latAccelOffset
+    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD_PQ, self.torque_params)
+
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      pid_log.error = float(error)
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+      if ASSIST_COMPENSATION:
+        output_torque = float(np.clip(output_torque * _assist_comp(CS.vEgo),
+                                      -self.steer_max, self.steer_max))
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque)
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    return -output_torque, 0.0, pid_log

iqpilot/selfdrive/controls/lib/lateral_acceleration_slew_limiter.py

@@ -0,0 +1,40 @@
+import numpy as np
+
+
+JERK_SPEED_BP = [0.0, 8.0, 20.0, 35.0]
+JERK_MAX_BP = [5.0, 4.0, 2.5, 2.0]
+A_LAT_MAX = 3.0
+MIN_LIMIT_SPEED = 5.0
+AVOIDANCE_BYPASS_ACCEL_DELTA = 2.0
+CURVATURE_SPEED_FLOOR = 0.1
+
+
+class LateralAccelerationSlewLimiter:
+  def __init__(self, enabled: bool):
+    self.enabled = enabled
+    self.a_lim = 0.0
+
+  def reset(self, a_lat: float) -> None:
+    self.a_lim = float(np.clip(a_lat, -A_LAT_MAX, A_LAT_MAX))
+
+  def jerk_max(self, v_ego: float) -> float:
+    return float(np.interp(v_ego, JERK_SPEED_BP, JERK_MAX_BP))
+
+  def update(self, desired_curvature: float, v_ego: float, dt: float) -> float:
+    if not self.enabled:
+      return desired_curvature
+
+    a_des = v_ego ** 2 * desired_curvature
+    if v_ego < MIN_LIMIT_SPEED:
+      self.reset(a_des)
+      return desired_curvature
+
+    if abs(a_des - self.a_lim) > AVOIDANCE_BYPASS_ACCEL_DELTA:
+      self.reset(a_des)
+    else:
+      da_max = self.jerk_max(v_ego) * dt
+      self.a_lim += float(np.clip(a_des - self.a_lim, -da_max, da_max))
+      self.a_lim = float(np.clip(self.a_lim, -A_LAT_MAX, A_LAT_MAX))
+
+    speed = max(abs(v_ego), CURVATURE_SPEED_FLOOR)
+    return self.a_lim / speed ** 2

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/.gitignore

@@ -0,0 +1,2 @@
+acados_ocp_lat.json
+c_generated_code/

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -0,0 +1,100 @@
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/lat_model/lat_expl_ode_fun.c',
+  f'{gen}/lat_model/lat_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/lat_cost/lat_cost_y_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/lat_cost/lat_cost_y_e_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/lat_cost/lat_cost_y_0_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_0_hess.c',
+]
+
+build_files = [f'{gen}/acados_solver_lat.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_lat.c',
+  f'{gen}/main_sim_lat.c',
+  f'{gen}/acados_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.h',
+  f'{gen}/acados_sim_solver_lat.c',
+  f'{gen}/acados_solver.pxd',
+
+  f'{gen}/lat_model/lat_expl_vde_adj.c',
+
+  f'{gen}/lat_model/lat_model.h',
+  f'{gen}/lat_constraints/lat_constraints.h',
+  f'{gen}/lat_cost/lat_cost.h',
+] + build_files
+
+acados_dir = '#iqpilot/third_party/acados'
+acados_templates_dir = '#iqpilot/third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['lat_mpc.py',
+  '#iqpilot/selfdrive/iqmodeld/config.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+acados_rel_path = Dir(gen).rel_path(Dir(f"#iqpilot/third_party/acados/{arch}/lib"))
+lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+lenv.Clean(generated_files, Dir(gen))
+
+_mpc_dir = Dir('.').abspath
+generated_lat = lenv.Command(generated_files,
+                             source_list,
+                             lenv.PrettyAction(f"cd {_mpc_dir} && python3 lat_mpc.py", 'GEN',
+                                               logfile=f"{_mpc_dir}/gen.log", capture_stderr=True))
+lenv.Depends(generated_lat, [msgq_python, common_python])
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+if arch != "Darwin":
+  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+else:
+  lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_lat.dylib")
+  lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}")
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
+                                build_files,
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#iqpilot/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#iqpilot/third_party/acados/acados_template/acados_solver_common.pxd")
+libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
+libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
+lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
+lenv2.Command(libacados_ocp_solver_c,
+  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
+  lenv2.PrettyAction(
+    f'cython' + \
+    f' -o {libacados_ocp_solver_c.get_labspath()}' + \
+    f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
+    f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
+    f' {acados_ocp_solver_pyx.get_labspath()}', 'CYTHON'))
+lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_lat'])
+lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(libacados_ocp_solver_c, np_version)

iqpilot/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+
+from casadi import SX, vertcat, sin, cos
+# WARNING: imports outside of constants will not trigger a rebuild
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+if __name__ == '__main__':  # generating code
+  from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
+X_DIM = 4
+P_DIM = 2
+COST_E_DIM = 3
+COST_DIM = COST_E_DIM + 2
+SPEED_OFFSET = 10.0
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+N = 32
+
+def gen_lat_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  y_ego = SX.sym('y_ego')
+  psi_ego = SX.sym('psi_ego')
+  psi_rate_ego = SX.sym('psi_rate_ego')
+  model.x = vertcat(x_ego, y_ego, psi_ego, psi_rate_ego)
+
+  # parameters
+  v_ego = SX.sym('v_ego')
+  rotation_radius = SX.sym('rotation_radius')
+  model.p = vertcat(v_ego, rotation_radius)
+
+  # controls
+  psi_accel_ego = SX.sym('psi_accel_ego')
+  model.u = vertcat(psi_accel_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  y_ego_dot = SX.sym('y_ego_dot')
+  psi_ego_dot = SX.sym('psi_ego_dot')
+  psi_rate_ego_dot = SX.sym('psi_rate_ego_dot')
+
+  model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, psi_rate_ego_dot)
+
+  # dynamics model
+  f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * psi_rate_ego,
+                   v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * psi_rate_ego,
+                   psi_rate_ego,
+                   psi_accel_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_lat_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_lat_model()
+
+  Tf = np.array(ModelConstants.T_IDXS)[N]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  Q = np.diag(np.zeros(COST_E_DIM))
+  QR = np.diag(np.zeros(COST_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  y_ego, psi_ego, psi_rate_ego = ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
+  psi_rate_ego_dot = ocp.model.u[0]
+  v_ego = ocp.model.p[0]
+
+  ocp.parameter_values = np.zeros((P_DIM, ))
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+  # Add offset to smooth out low speed control
+  # TODO unclear if this right solution long term
+  v_ego_offset = v_ego + SPEED_OFFSET
+  # TODO there are two costs on psi_rate_ego_dot, one
+  # is correlated to jerk the other to steering wheel movement
+  # the steering wheel movement cost is added to prevent excessive
+  # wheel movements
+  ocp.model.cost_y_expr = vertcat(y_ego,
+                                  v_ego_offset * psi_ego,
+                                  v_ego_offset * psi_rate_ego,
+                                  v_ego_offset * psi_rate_ego_dot,
+                                  psi_rate_ego_dot / (v_ego + 0.1))
+  ocp.model.cost_y_expr_e = vertcat(y_ego,
+                                   v_ego_offset * psi_ego,
+                                   v_ego_offset * psi_rate_ego)
+
+  # set constraints
+  ocp.constraints.constr_type = 'BGH'
+  ocp.constraints.idxbx = np.array([2,3])
+  ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
+  ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
+  x0 = np.zeros((X_DIM,))
+  ocp.constraints.x0 = x0
+
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_iter_max = 1
+  ocp.solver_options.qp_solver_cond_N = 1
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = np.array(ModelConstants.T_IDXS)[:N+1]
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LateralMpc:
+  def __init__(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset(x0)
+
+  def reset(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(X_DIM)
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.yref = np.zeros((N+1, COST_DIM))
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    # Somehow needed for stable init
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+      self.solver.set(i, 'p', np.zeros(P_DIM))
+    self.solver.constraints_set(0, "lbx", x0)
+    self.solver.constraints_set(0, "ubx", x0)
+    self.solver.solve()
+    self.solution_status = 0
+    self.solve_time = 0.0
+    self.cost = 0
+
+  def set_weights(self, path_weight, heading_weight,
+                  lat_accel_weight, lat_jerk_weight,
+                  steering_rate_weight):
+    W = np.asfortranarray(np.diag([path_weight, heading_weight,
+                                   lat_accel_weight, lat_jerk_weight,
+                                   steering_rate_weight]))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
+    self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM])
+
+  def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
+    x0_cp = np.copy(x0)
+    p_cp = np.copy(p)
+    self.solver.constraints_set(0, "lbx", x0_cp)
+    self.solver.constraints_set(0, "ubx", x0_cp)
+    self.yref[:,0] = y_pts
+    v_ego = p_cp[0, 0]
+    # rotation_radius = p_cp[1]
+    self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
+    self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+      self.solver.set(i, "p", p_cp[i])
+    self.solver.set(N, "p", p_cp[N])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    t = time.monotonic()
+    self.solution_status = self.solver.solve()
+    self.solve_time = time.monotonic() - t
+
+    for i in range(N+1):
+      self.x_sol[i] = self.solver.get(i, 'x')
+    for i in range(N):
+      self.u_sol[i] = self.solver.get(i, 'u')
+    self.cost = self.solver.get_cost()
+
+
+if __name__ == "__main__":
+  ocp = gen_lat_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

iqpilot/selfdrive/controls/lib/ldw.py

@@ -0,0 +1,41 @@
+from iqpilot.cereal import log
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.common.constants import CV
+
+
+CAMERA_OFFSET = 0.04
+LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
+LANE_DEPARTURE_THRESHOLD = 0.1
+
+class LaneDepartureWarning:
+  def __init__(self):
+    self.left = False
+    self.right = False
+    self.last_blinker_frame = 0
+
+  def update(self, frame, modelV2, CS, CC):
+    if CS.leftBlinker or CS.rightBlinker:
+      self.last_blinker_frame = frame
+
+    recent_blinker = (frame - self.last_blinker_frame) * DT_CTRL < 5.0  # 5s blinker cooldown
+    ldw_allowed = CS.vEgo > LDW_MIN_SPEED and not recent_blinker and not CC.latActive
+
+    desire_prediction = modelV2.meta.desirePrediction
+    if len(desire_prediction) and ldw_allowed:
+      right_lane_visible = modelV2.laneLineProbs[2] > 0.5
+      left_lane_visible = modelV2.laneLineProbs[1] > 0.5
+      l_lane_change_prob = desire_prediction[log.Desire.laneChangeLeft]
+      r_lane_change_prob = desire_prediction[log.Desire.laneChangeRight]
+
+      lane_lines = modelV2.laneLines
+      l_lane_close = left_lane_visible and (lane_lines[1].y[0] > -(1.08 + CAMERA_OFFSET))
+      r_lane_close = right_lane_visible and (lane_lines[2].y[0] < (1.08 - CAMERA_OFFSET))
+
+      self.left = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
+      self.right = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)
+    else:
+      self.left, self.right = False, False
+
+  @property
+  def warning(self) -> bool:
+    return bool(self.left or self.right)

iqpilot/selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,95 @@
+import numpy as np
+from iqpilot.cereal import car
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.smooth_stops import SmoothStopController
+
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
+
+def long_control_state_trans(CP_IQ, active, long_control_state, should_stop, brake_pressed, cruise_standstill):
+  # Gas Interceptor
+  cruise_standstill = cruise_standstill and not CP_IQ.enableGasInterceptor
+
+  starting_condition = (not should_stop and
+                        not cruise_standstill and
+                        not brake_pressed)
+
+  if not active:
+    long_control_state = LongCtrlState.off
+
+  else:
+    if long_control_state == LongCtrlState.off:
+      if not starting_condition:
+        long_control_state = LongCtrlState.stopping
+      else:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.stopping:
+      if starting_condition:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.pid:
+      if should_stop:
+        long_control_state = LongCtrlState.stopping
+  return long_control_state
+
+class LongControl:
+  def __init__(self, CP, CP_IQ):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+    self.long_control_state = LongCtrlState.off
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             rate=1 / DT_CTRL)
+    self.last_output_accel = 0.0
+    self.stopping_decel_rate = CP_IQ.stoppingDecelRateOverride or 1.0
+    self.smooth = SmoothStopController()
+
+  def reset(self):
+    self.pid.reset()
+
+  def update(self, active, CS, a_target, should_stop, accel_limits, lead_distance=0.0, has_lead=False, gas_override=False):
+    """Update longitudinal control. This updates the state machine and runs a PID loop"""
+    self.pid.neg_limit = accel_limits[0]
+    self.pid.pos_limit = accel_limits[1]
+    self.smooth.update()
+
+    if self.smooth.enabled and active and self.long_control_state != LongCtrlState.stopping:
+      stop_now = self.smooth.want_hold(should_stop, CS.vEgo, CS.standstill)
+    else:
+      stop_now = should_stop
+
+    self.long_control_state = long_control_state_trans(self.CP_IQ, active, self.long_control_state, stop_now, CS.brakePressed,
+                                                       CS.cruiseState.standstill)
+    if self.long_control_state == LongCtrlState.off:
+      self.reset()
+      self.smooth.reset()
+      output_accel = 0.
+
+    elif self.long_control_state == LongCtrlState.stopping:
+      output_accel = self.last_output_accel
+      if output_accel > self.CP.stopAccel:
+        output_accel = min(output_accel, 0.0)
+        # TODO: can we just go straight to stopAccel?
+        output_accel -= self.stopping_decel_rate * DT_CTRL  # m/s^2/s while trying to stop
+      self.reset()
+      self.smooth.reset()
+
+    else:  # LongCtrlState.pid
+      if self.smooth.enabled and active and should_stop:
+        output_accel = self.smooth.settle(a_target, CS.vEgo, lead_distance, has_lead, self.last_output_accel)
+        self.reset()
+      else:
+        error = a_target - CS.aEgo
+        output_accel = self.pid.update(error, speed=CS.vEgo,
+                                       feedforward=a_target,
+                                       freeze_integrator=gas_override)
+        self.smooth.reset()
+
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
+    return self.last_output_accel

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

@@ -0,0 +1,2 @@
+acados_ocp_long.json
+c_generated_code/

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -0,0 +1,105 @@
+Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'np_version')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/long_model/long_expl_ode_fun.c',
+  f'{gen}/long_model/long_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/long_cost/long_cost_y_fun.c',
+  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/long_cost/long_cost_y_e_fun.c',
+  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/long_cost/long_cost_y_0_fun.c',
+  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_0_hess.c',
+]
+
+casadi_constraints = [
+  f'{gen}/long_constraints/long_constr_h_fun.c',
+  f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
+]
+
+build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
+              casadi_cost_0 + casadi_constraints
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_long.c',
+  f'{gen}/main_sim_long.c',
+  f'{gen}/acados_solver_long.h',
+  f'{gen}/acados_sim_solver_long.h',
+  f'{gen}/acados_sim_solver_long.c',
+  f'{gen}/acados_solver.pxd',
+
+  f'{gen}/long_model/long_expl_vde_adj.c',
+
+  f'{gen}/long_model/long_model.h',
+  f'{gen}/long_constraints/long_constraints.h',
+  f'{gen}/long_cost/long_cost.h',
+] + build_files
+
+acados_dir = '#iqpilot/third_party/acados'
+acados_templates_dir = '#iqpilot/third_party/acados/acados_template/c_templates_tera'
+
+source_list = ['long_mpc.py',
+  '#iqpilot/selfdrive/iqmodeld/config.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_templates_dir}/acados_solver.in.c',
+]
+
+lenv = env.Clone()
+acados_rel_path = Dir(gen).rel_path(Dir(f"#iqpilot/third_party/acados/{arch}/lib"))
+lenv["RPATH"] += [lenv.Literal(f'\\$$ORIGIN/{acados_rel_path}')]
+lenv.Clean(generated_files, Dir(gen))
+_mpc_dir = Dir('.').abspath
+generated_long = lenv.Command(generated_files,
+                              source_list,
+                              lenv.PrettyAction(f"cd {_mpc_dir} && python3 long_mpc.py", 'GEN',
+                                                logfile=f"{_mpc_dir}/gen.log", capture_stderr=True))
+lenv.Depends(generated_long, [msgq_python, common_python])
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+if arch != "Darwin":
+  lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+else:
+  lenv["LINKFLAGS"].append("-Wl,-install_name,@loader_path/libacados_ocp_solver_long.dylib")
+  lenv["LINKFLAGS"].append(f"-Wl,-rpath,@loader_path/{acados_rel_path}")
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
+                                build_files,
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#iqpilot/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx")
+acados_ocp_solver_common = File("#iqpilot/third_party/acados/acados_template/acados_solver_common.pxd")
+libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
+libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LIBPATH"] += [lib_solver[0].dir.abspath]
+lenv2["RPATH"] += [lenv2.Literal('\\$$ORIGIN')]
+lenv2.Command(libacados_ocp_solver_c,
+  [acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
+  lenv2.PrettyAction(
+    f'cython' + \
+    f' -o {libacados_ocp_solver_c.get_labspath()}' + \
+    f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
+    f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
+    f' {acados_ocp_solver_pyx.get_labspath()}', 'CYTHON'))
+lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c], LIBS=['acados_ocp_solver_long'])
+lenv2.Depends(lib_cython, lib_solver)
+lenv2.Depends(libacados_ocp_solver_c, np_version)

iqpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,433 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from iqpilot.cereal import log
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
+from iqpilot.selfdrive.iqmodeld.config import index_function, ModelConstants
+from iqpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU  # legacy lead extrapolation (newLeadMpc=False)
+from iqpilot.common.params import Params, UnknownKeyName
+
+LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS)  # [0, 2, 4, 6, 8, 10]s
+
+if __name__ == '__main__':  # generating code
+  from iqpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+from casadi import SX, vertcat
+
+MODEL_NAME = 'long'
+LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
+
+LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
+
+X_DIM = 3
+U_DIM = 1
+PARAM_DIM = 5
+COST_E_DIM = 4
+COST_DIM = COST_E_DIM + 1
+CONSTR_DIM = 4
+
+X_EGO_OBSTACLE_COST = 3.
+X_EGO_COST = 0.
+V_EGO_COST = 0.
+A_EGO_COST = 0.
+J_EGO_COST = 20.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .25
+LEAD_DANGER_FACTOR = 0.75
+LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+
+# Fewer timestamps don't hurt performance and lead to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
+
+T_IDXS = np.array(T_IDXS_LST)
+FCW_IDXS = T_IDXS < 5.0
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+COMFORT_BRAKE = 2.5
+STOP_DISTANCE = 3.0
+MIN_X_LEAD_FACTOR = 0.5
+LEAD_PULLAWAY_VREL = 0.5
+LEAD_PULLAWAY_ABRAKE = -0.5
+
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 0.5
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+
+def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.75
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.45
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 1.25
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+def get_stopped_equivalence_factor(v_lead):
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
+
+def get_safe_obstacle_distance(v_ego, t_follow):
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # states
+  x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  a_min = SX.sym('a_min')
+  a_max = SX.sym('a_max')
+  x_obstacle = SX.sym('x_obstacle')
+  lead_t_follow = SX.sym('lead_t_follow')
+  lead_danger_factor = SX.sym('lead_danger_factor')
+  model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+def gen_long_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = T_IDXS[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.zeros((COST_DIM, COST_DIM))
+  Q = np.zeros((COST_E_DIM, COST_E_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  a_min, a_max = ocp.model.p[0], ocp.model.p[1]
+  x_obstacle = ocp.model.p[2]
+  lead_t_follow = ocp.model.p[3]
+  lead_danger_factor = ocp.model.p[4]
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
+
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
+  costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
+           x_ego,
+           v_ego,
+           a_ego,
+           j_ego]
+  ocp.model.cost_y_expr = vertcat(*costs)
+  ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
+
+  # Constraints on speed, acceleration and desired distance to
+  # the obstacle, which is treated as a slack constraint so it
+  # behaves like an asymmetrical cost.
+  constraints = vertcat(v_ego,
+                        (a_ego - a_min),
+                        (a_max - a_ego),
+                        ((x_obstacle - x_ego) - lead_danger_factor * (desired_dist_comfort)) / (v_ego + 10.))
+  ocp.model.con_h_expr = constraints
+
+  x0 = np.zeros(X_DIM)
+  ocp.constraints.x0 = x0
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
+
+
+  # We put all constraint cost weights to 0 and only set them at runtime
+  cost_weights = np.zeros(CONSTR_DIM)
+  ocp.cost.zl = cost_weights
+  ocp.cost.Zl = cost_weights
+  ocp.cost.Zu = cost_weights
+  ocp.cost.zu = cost_weights
+
+  ocp.constraints.lh = np.zeros(CONSTR_DIM)
+  ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.idxsh = np.arange(CONSTR_DIM)
+
+  # The HPIPM solver can give decent solutions even when it is stopped early
+  # Which is critical for our purpose where compute time is strictly bounded
+  # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
+  # does not cause issues since the problem is well bounded.
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_cond_N = 1
+
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
+  ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = T_IDXS
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc:
+  def __init__(self, dt=DT_MDL):
+    self.dt = dt
+    self._params = Params()
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset()
+    self.source = LongitudinalPlanSource.cruise
+
+  def _read_new_lead_mpc(self) -> bool:
+    try:
+      return self._params.get_bool("newLeadMpc")
+    except UnknownKeyName:
+      return True
+
+  def reset(self):
+    self.solver.reset()
+
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.v_solution = np.zeros(N+1)
+    self.a_solution = np.zeros(N+1)
+    self.j_solution = np.zeros(N)
+    self.yref = np.zeros((N+1, COST_DIM))
+
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params = np.zeros((N+1, PARAM_DIM))
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+
+    self.last_cloudlog_t = 0
+    self.status = False
+    self.crash_cnt = 0.0
+    self.solution_status = 0
+    # timers
+    self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
+    self.x0 = np.zeros(X_DIM)
+    self.lead_xv_0 = np.zeros((N+1, 2))
+    self.lead_xv_1 = np.zeros((N+1, 2))
+    self.set_weights()
+
+  def set_cost_weights(self, cost_weights, constraint_cost_weights):
+    W = np.asfortranarray(np.diag(cost_weights))
+    for i in range(N):
+      self.solver.cost_set(i, 'W', W)
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
+    self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
+
+    # Set L2 slack cost on lower bound constraints
+    Zl = np.array(constraint_cost_weights)
+    for i in range(N):
+      self.solver.cost_set(i, 'Zl', Zl)
+
+  def set_weights(self, personality=log.LongitudinalPersonality.standard):
+    jerk_factor = get_jerk_factor(personality)
+    cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
+    constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    self.set_cost_weights(cost_weights, constraint_cost_weights)
+
+  def set_cur_state(self, v, a):
+    v_prev = self.x0[1]
+    self.x0[1] = v
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.:  # probably only helps if v < v_prev
+      for i in range(N+1):
+        self.solver.set(i, 'x', self.x0)
+
+  @staticmethod
+  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
+    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
+    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
+    return lead_xv
+
+  def process_lead_legacy(self, lead):
+    # behavior before PR #37824 (newLeadMpc=False): one immediate radar lead prediction
+    # extrapolated forward with acceleration decaying to 0
+    v_ego = self.x0[1]
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
+    else:
+      # Fake a fast lead car, so mpc can keep running in the same mode
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
+    a_lead = np.clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+    return lead_xv
+
+  def process_lead(self, model_lead, radar_lead):
+    v_ego = self.x0[1]
+    try:
+      x_model = np.asarray(model_lead.x, dtype=np.float64)
+      v_model = np.asarray(model_lead.v, dtype=np.float64)
+      valid_model_lead = (float(model_lead.prob) > 0.5 and radar_lead.status and float(radar_lead.modelProb) > 0.5 and
+                          x_model.shape == LEAD_T_IDXS_MODEL.shape and v_model.shape == LEAD_T_IDXS_MODEL.shape and
+                          np.all(np.isfinite(x_model)) and np.all(np.isfinite(v_model)))
+    except (AttributeError, TypeError, ValueError):
+      valid_model_lead = False
+
+    if not valid_model_lead:
+      return self.process_lead_legacy(radar_lead)
+
+    x_lead_traj = float(radar_lead.dRel) + (x_model - x_model[0])
+    v_lead_traj = float(radar_lead.vLead) + (v_model - v_model[0])
+
+    # MPC won't converge on immediate crashes; lift h=0 to the minimum braking distance.
+    v_lead_0 = v_lead_traj[0]
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead_0) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2)
+    x_lead_traj[0] = max(x_lead_traj[0], min_x_lead)
+    v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8)
+
+    x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj))
+    v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj)
+    if radar_lead.status and radar_lead.vRel > LEAD_PULLAWAY_VREL and radar_lead.aLeadK > LEAD_PULLAWAY_ABRAKE:
+      # ty spysyweeb for lead pull away fix phantom launch braking so you don't ram the lead in edge cases.
+      radar_velocity_floor = np.full_like(T_IDXS, float(radar_lead.vLead))
+      radar_distance_floor = float(radar_lead.dRel) + float(radar_lead.vLead) * T_IDXS
+      v_lead_mpc = np.maximum(v_lead_mpc, radar_velocity_floor)
+      x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
+    return np.column_stack((x_lead_mpc, v_lead_mpc))
+
+  def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
+    self.new_lead_mpc = self._read_new_lead_mpc()
+    t_follow = get_T_FOLLOW(personality)
+    model_leads = modelV2.leadsV3
+
+    if self.new_lead_mpc:
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      model_lead_0 = model_leads[0] if len(model_leads) > 0 else None
+      model_lead_1 = model_leads[1] if len(model_leads) > 1 else None
+      lead_xv_0 = self.process_lead(model_lead_0, radarstate.leadOne)
+      lead_xv_1 = self.process_lead(model_lead_1, radarstate.leadTwo)
+    else:
+      # pre-PR behavior: radar lead extrapolated with accel decay
+      self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+      lead_xv_0 = self.process_lead_legacy(radarstate.leadOne)
+      lead_xv_1 = self.process_lead_legacy(radarstate.leadTwo)
+    self.lead_xv_0 = lead_xv_0
+    self.lead_xv_1 = lead_xv_1
+
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
+    # distance that lead needs as a minimum. We can add that to the current distance
+    # and then treat that as a stopped car/obstacle at this new distance.
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
+    self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
+
+    self.yref[:,:] = 0.0
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params[:,0] = ACCEL_MIN
+    self.params[:,1] = ACCEL_MAX
+    self.params[:,2] = np.min(x_obstacles, axis=1)
+    self.params[:,3] = t_follow
+    self.params[:,4] = LEAD_DANGER_FACTOR
+
+    self.run()
+    lead_crash_prob = radarstate.leadOne.modelProb
+    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
+            lead_crash_prob > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
+
+  def run(self):
+    for i in range(N+1):
+      self.solver.set(i, 'p', self.params[i])
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+
+    self.solution_status = self.solver.solve()
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    for i in range(N+1):
+      self.x_sol[i] = self.solver.get(i, 'x')
+    for i in range(N):
+      self.u_sol[i] = self.solver.get(i, 'u')
+
+    self.v_solution = self.x_sol[:,1]
+    self.a_solution = self.x_sol[:,2]
+    self.j_solution = self.u_sol[:,0]
+
+    t = time.monotonic()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning(f"Long mpc reset, solution_status: {self.solution_status}")
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)

iqpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,297 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+
+import iqpilot.cereal.messaging as messaging
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from iqpilot.common.constants import CV
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params, UnknownKeyName
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, DEFAULT_STOPPING_SPEED, get_accel_from_plan
+from iqpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.issue_debug import log_issue_limited
+
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
+
+A_CRUISE_MAX_VALS = [2.0, 1.6, 0.8, 0.6]
+A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
+A_CRUISE_MIN = -1.2
+J_CRUISE = 1.0
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+ALLOW_THROTTLE_THRESHOLD = 0.4
+MIN_ALLOW_THROTTLE_SPEED = 2.5
+
+LAUNCH_DISARM_SPEED = 2.0
+LAUNCH_COMMIT_T = 3.5
+LAUNCH_MOVING_SPEED = 1.2
+LAUNCH_MAX_ACCEL = 1.5
+
+E2E_CRUISE_CONVERGENCE_TAU = 15.0
+E2E_CRUISE_ACCEL_MAX = 0.5
+E2E_MODEL_SPEED_HORIZON = 5.0
+E2E_ACCEL_INTENT_BP = [-0.05, 0.05]
+E2E_MODEL_SPEED_INTENT_BP = [-0.5, 0.0]
+
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
+
+def get_max_accel(v_ego):
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
+
+def get_coast_accel(pitch):
+  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
+
+def get_lead_distance(radarState):
+  if radarState.leadOne.status and (not radarState.leadTwo.status or radarState.leadOne.dRel < radarState.leadTwo.dRel):
+    return radarState.leadOne.dRel
+  if radarState.leadTwo.status:
+    return radarState.leadTwo.dRel
+  return 0
+
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
+  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
+
+  if not e2e:
+    a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+    a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+    a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+    max_accel = min(max_accel, a_x_allowed)
+  if not allow_throttle:
+    clipped_accel_coast = max(accel_coast, ACCEL_MIN)
+    coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
+    max_accel = min(max_accel, coast_limit)
+
+  target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
+  target_accel = float(np.clip(target_accel, a_cruise_prev - J_CRUISE * dt, a_cruise_prev + J_CRUISE * dt))
+
+  cruise_should_stop = v_cruise == 0.0
+  return target_accel, cruise_should_stop
+
+
+def get_e2e_accel(v_ego, v_cruise, model_v, a_target, should_stop):
+  if should_stop or v_cruise <= v_ego or len(model_v) != len(T_IDXS_MPC):
+    return a_target
+
+  convergence_accel = min((v_cruise - v_ego) / E2E_CRUISE_CONVERGENCE_TAU, E2E_CRUISE_ACCEL_MAX)
+  if convergence_accel <= a_target:
+    return a_target
+
+  # Only help the model converge to cruise when both its immediate action and
+  # velocity trajectory show no active deceleration intent. The lead MPC and
+  # cruise candidates remain hard upper bounds on the final acceleration.
+  accel_intent = np.interp(a_target, E2E_ACCEL_INTENT_BP, [0.0, 1.0])
+  model_speed = np.interp(E2E_MODEL_SPEED_HORIZON, T_IDXS_MPC, model_v)
+  speed_intent = np.interp(model_speed - v_ego, E2E_MODEL_SPEED_INTENT_BP, [0.0, 1.0])
+  return float(np.interp(min(accel_intent, speed_intent), [0.0, 1.0], [a_target, convergence_accel]))
+
+
+def get_accel_candidates(e2e, has_lead, mpc_candidate, cruise_candidate, e2e_candidate):
+  candidates = []
+  # With no lead, the MPC follows a synthetic fast lead. It remains the ACC
+  # policy, but must not limit the model policy in full E2E.
+  if not e2e or has_lead:
+    candidates.append(mpc_candidate)
+  candidates.append(cruise_candidate)
+  if e2e:
+    candidates.append(e2e_candidate)
+  return candidates
+
+
+class LongitudinalPlanner(LongitudinalPlannerIQ):
+  def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
+    self.CP = CP
+    self.stopping_speed = CP_IQ.longitudinalStoppingSpeedOverride or DEFAULT_STOPPING_SPEED
+    self.mpc = LongitudinalMpc(dt=dt)
+    LongitudinalPlannerIQ.__init__(self, self.CP, CP_IQ, self.mpc)
+    self.fcw = False
+    self.dt = dt
+    self.allow_throttle = True
+
+    self.a_desired = init_a
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
+    self.a_cruise = init_a
+    self.output_a_target = 0.0
+    self.output_should_stop = False
+    self.launch_armed = False
+    try:
+      self.exp_speed_conv = Params().get_bool("expSpeedConv")
+    except UnknownKeyName:
+      self.exp_speed_conv = False
+
+    self.v_desired_trajectory = np.zeros(CONTROL_N)
+    self.a_desired_trajectory = np.zeros(CONTROL_N)
+    self.j_desired_trajectory = np.zeros(CONTROL_N)
+
+  @staticmethod
+  def parse_model(model_msg):
+    if (len(model_msg.position.x) == ModelConstants.IDX_N and
+      len(model_msg.velocity.x) == ModelConstants.IDX_N and
+      len(model_msg.acceleration.x) == ModelConstants.IDX_N):
+      x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)
+      v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)
+      a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
+      j = np.zeros(len(T_IDXS_MPC))
+    else:
+      x = np.zeros(len(T_IDXS_MPC))
+      v = np.zeros(len(T_IDXS_MPC))
+      a = np.zeros(len(T_IDXS_MPC))
+      j = np.zeros(len(T_IDXS_MPC))
+    if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1:
+      throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1]
+    else:
+      throttle_prob = 1.0
+    return x, v, a, j, throttle_prob
+
+  def update(self, sm):
+    LongitudinalPlannerIQ.update(self, sm)
+
+    if len(sm['carControl'].orientationNED) == 3:
+      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
+    else:
+      accel_coast = ACCEL_MAX
+
+    v_ego = sm['carState'].vEgo
+    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
+    if sm['controlsState'].forceDecel:
+      v_cruise = 0.0
+
+    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
+
+    # Reset current state when not engaged, or user is controlling the speed
+    reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
+    # PCM cruise speed may be updated a few cycles later, check if initialized
+    v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
+    reset_state = reset_state or not v_cruise_initialized
+    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['vehicleParameters'].angleOffsetDeg
+
+    if reset_state:
+      self.v_desired_filter.x = v_ego
+      self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
+      self.a_cruise = self.a_desired
+
+    # Prevent divergence, smooth in current v_ego
+    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
+    _, model_v, model_a, _, throttle_prob = self.parse_model(sm['modelV2'])
+    # Don't clip at low speeds since throttle_prob doesn't account for creep
+    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+
+    # Get new v_cruise from Smart Cruise Control and Speed Limit Assist
+    v_cruise = LongitudinalPlannerIQ.update_targets(self, sm, self.v_desired_filter.x, v_cruise)
+
+    if sm['controlsState'].forceDecel:
+      v_cruise = 0.0
+
+    personality = sm['selfdriveState'].personality
+    self.mpc.set_weights(personality=personality)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(sm['modelV2'], sm['radarState'], personality=personality)
+
+    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
+    self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
+    self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
+
+    # TODO counter is only needed because radar is glitchy, remove once radar is gone
+    self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
+    if self.fcw:
+      cloudlog.info("FCW triggered")
+
+    # Save starting point for next iteration
+    a_prev = self.a_desired
+
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                                        action_t=action_t, stopping_speed=self.stopping_speed)
+
+    output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+    output_should_stop_e2e = sm['modelV2'].action.shouldStop
+    output_a_target_e2e, output_should_stop_e2e = self.apply_e2e_stop_distance(sm, v_ego, output_a_target_e2e, output_should_stop_e2e)
+    if self.is_e2e(sm) and self.exp_speed_conv and not self.mpc.status:
+      output_a_target_e2e = get_e2e_accel(v_ego, v_cruise, model_v, output_a_target_e2e, output_should_stop_e2e)
+
+    if sm['carState'].standstill:
+      self.launch_armed = True
+    elif v_ego > LAUNCH_DISARM_SPEED:
+      self.launch_armed = False
+    if (self.launch_armed and self.is_e2e(sm) and not output_should_stop_e2e and
+        np.interp(LAUNCH_COMMIT_T, T_IDXS_MPC, model_v) > LAUNCH_DISARM_SPEED):
+      t_cut = min(float(T_IDXS_MPC[np.argmax(model_v > LAUNCH_MOVING_SPEED)]), LAUNCH_COMMIT_T)
+      t_shifted = T_IDXS_MPC + t_cut
+      v_shifted = np.interp(t_shifted, T_IDXS_MPC, model_v)
+      a_shifted = np.interp(t_shifted, T_IDXS_MPC, model_a)
+      a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t)[0]
+      a_launch_max = np.interp(v_ego, [LAUNCH_MOVING_SPEED, LAUNCH_DISARM_SPEED], [LAUNCH_MAX_ACCEL, 0.])
+      output_a_target_e2e = max(output_a_target_e2e, min(a_launch, a_launch_max))
+
+    e2e = self.is_e2e(sm)
+    self.a_cruise, cruise_should_stop = get_cruise_accel(e2e, v_cruise, v_ego, self.a_cruise,
+                                                          steer_angle_without_offset, self.CP, self.dt,
+                                                          accel_coast, self.allow_throttle)
+
+    candidates = get_accel_candidates(
+      e2e,
+      self.mpc.status,
+      (output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
+      (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),
+      (output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e),
+    )
+
+    output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
+    self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
+
+    self.output_should_stop = self.output_should_stop or self.forcing_stop
+    self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
+
+    self.a_desired = float(self.output_a_target)
+    self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
+
+  def publish(self, sm, pm):
+    plan_send = messaging.new_message('longitudinalPlan')
+
+    gate_services = ['carState', 'controlsState', 'selfdriveState', 'radarState']
+    plan_send.valid = sm.all_checks(service_list=gate_services)
+    if not plan_send.valid:
+      log_issue_limited(
+        "longitudinal_plan_invalid",
+        "planner",
+        f"longitudinalPlan invalid alive={ {s: sm.alive[s] for s in gate_services} } "
+        f"freq_ok={ {s: sm.freq_ok[s] for s in gate_services} } valid={ {s: sm.valid[s] for s in gate_services} } "
+        f"subchecks=({sm.all_alive(gate_services)},{sm.all_freq_ok(gate_services)},{sm.all_valid(gate_services)}) "
+        f"recheck={sm.all_checks(service_list=gate_services)}",
+        interval_sec=5.0,
+      )
+
+    longitudinalPlan = plan_send.longitudinalPlan
+    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
+
+    longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
+    longitudinalPlan.accels = self.a_desired_trajectory.tolist()
+    longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
+
+    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.leadDistance = get_lead_distance(sm['radarState'])
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    longitudinalPlan.fcw = self.fcw
+
+    longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist()
+    longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist()
+    longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist()
+
+    longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.shouldStop = bool(self.output_should_stop)
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
+
+    pm.send('longitudinalPlan', plan_send)
+
+    self.publish_longitudinal_plan_iq(sm, pm)

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,101 @@
+"""
+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 iqpilot.selfdrive.controls.lib.latcontrol_torque as locator
+
+
+def test_packaged_substitute_table():
+  assert locator.TORQUE_NN_MODEL_SUBSTITUTE_PATH.is_file()
+  assert locator._substitute_for("MAZDA_3") == "MAZDA_CX9_2021"
+  assert locator._substitute_for("UNKNOWN") == "UNKNOWN"
+
+
+@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"
+
+
+def test_missing_model_directory_falls_back(model_dir, tmp_path, monkeypatch):
+  missing = tmp_path / "missing"
+  monkeypatch.setattr(locator, "TORQUE_NN_MODEL_PATH", str(missing))
+  monkeypatch.setattr(locator, "MOCK_MODEL_PATH", str(missing / "MOCK.json"))
+  path, name, exact = locator.get_nn_model_path(make_cp("HONDA_CIVIC"))
+  assert path == locator.MOCK_MODEL_PATH
+  assert name == "MOCK"
+  assert exact is False

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

@@ -0,0 +1,98 @@
+"""
+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 iqpilot.selfdrive.controls.lib.latcontrol_torque import NNTorqueModel
+from iqpilot.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": 18,
+  "output_size": 1,
+  "input_mean": [[0.0]] * 18,
+  "input_std": [[1.0]] * 18,
+  "layers": [
+    {"dense_1_W": [[0.5] * 18, [0.5] * 18], "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 os.path.isdir(TORQUE_NN_MODEL_PATH) else []
+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):
+  with open(_path(SAMPLE[0])) as model_file:
+    base = json.load(model_file)
+  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,103 @@
+"""
+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 iqpilot.cereal import log
+from iqpilot.common.params import Params
+from iqpilot.common.pid import PIDController
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import NeuralNetworkFeedForward
+from iqpilot.selfdrive.controls.lib.neural_network_feed_forward.tests.test_network import _MODEL_DIR, _NAMES
+
+_REAL_MODEL = next((f for f in _NAMES if f != "MOCK.json"), _NAMES[0])
+
+
+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(_MODEL_DIR, 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
+    if "MOCK.json" in _NAMES:
+      assert nnff.model.input_size >= 2
+    else:
+      assert nnff.model is None
+
+  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 iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.k3_slc_log import k3_slc_log
+from 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 iqpilot.common.params import Params
+from iqpilot.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,826 @@
+#!/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 iqpilot.cereal import car, custom
+from iqpilot.common.constants import CV
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.k3_slc_log import k3_slc_log
+from iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
+from 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 iqpilot.system.proprietary_runtime._verified_import import import_verified_module
+      runtime_common = import_verified_module("iqpilot_navd_private", "iqpilot_private.navd.runtime_common")
+      return runtime_common.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["vehicleParameters"].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["vehicleParameters"].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_curvature_lookahead.py

@@ -0,0 +1,27 @@
+from types import SimpleNamespace
+
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.curvature_lookahead import LOOKAHEAD_SECONDS, get_lookahead_curvature
+from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+def test_lookahead_samples_total_delay_horizon():
+  yaws = np.square(np.asarray(ModelConstants.T_IDXS)) * 0.02
+  yaw_rates = np.asarray(ModelConstants.T_IDXS) * 0.04
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=yaws.tolist()),
+    orientationRate=SimpleNamespace(z=yaw_rates.tolist()),
+  )
+  lat_delay = 0.3
+  expected = get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, 20.0, lat_delay + LOOKAHEAD_SECONDS)
+  assert get_lookahead_curvature(model_v2, 20.0, lat_delay) == expected
+
+
+def test_invalid_trajectory_falls_back_to_none():
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=[0.0]),
+    orientationRate=SimpleNamespace(z=[0.0]),
+  )
+  assert get_lookahead_curvature(model_v2, 20.0, 0.3) is None

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 iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from 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 iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_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_lateral_acceleration_slew_limiter.py

@@ -0,0 +1,87 @@
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import (
+  A_LAT_MAX,
+  AVOIDANCE_BYPASS_ACCEL_DELTA,
+  LateralAccelerationSlewLimiter,
+)
+
+
+def test_disabled_is_exact_passthrough_without_state_change():
+  limiter = LateralAccelerationSlewLimiter(False)
+  limiter.reset(1.25)
+  rng = np.random.default_rng(0)
+  for curvature in rng.standard_normal(100):
+    assert limiter.update(curvature, 25.0, 0.01) is curvature
+    assert limiter.a_lim == 1.25
+
+
+def test_step_is_limited_by_speed_scheduled_jerk():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  v_ego = 20.0
+  dt = 0.01
+  target = 1.5 / v_ego ** 2
+  previous = limiter.a_lim
+  for _ in range(100):
+    limiter.update(target, v_ego, dt)
+    assert abs(limiter.a_lim - previous) <= limiter.jerk_max(v_ego) * dt + 1e-12
+    previous = limiter.a_lim
+
+
+def test_converges_to_held_target():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(0.0)
+  for _ in range(100):
+    limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+
+
+def test_reset_prevents_reengagement_jump():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(target_accel)
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert curvature == target_accel / v_ego ** 2
+  assert limiter.a_lim == target_accel
+
+
+def test_low_speed_passes_through_and_resets():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(-1.0)
+  curvature = 0.2
+  assert limiter.update(curvature, 4.0, 0.01) == curvature
+  assert limiter.a_lim == A_LAT_MAX
+
+
+def test_speed_schedule_changes_slew_rate():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  limiter.update(1.0 / 8.0 ** 2, 8.0, 0.01)
+  low_speed_step = limiter.a_lim
+  limiter.reset(0.0)
+  limiter.update(1.0 / 35.0 ** 2, 35.0, 0.01)
+  high_speed_step = limiter.a_lim
+  assert low_speed_step > high_speed_step
+
+
+def test_sharp_avoidance_bypasses_limiter():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  limiter.reset(0.0)
+  target_accel = AVOIDANCE_BYPASS_ACCEL_DELTA + 0.1
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+  assert curvature == target_accel / v_ego ** 2
+
+
+def test_acceleration_space_couples_speed_and_curvature_changes():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.5)
+  limiter.update(0.005, 10.0, 0.01)
+  previous = limiter.a_lim
+  limiter.update(0.003, 20.0, 0.01)
+  assert limiter.a_lim - previous <= limiter.jerk_max(20.0) * 0.01 + 1e-12

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

@@ -0,0 +1,195 @@
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
+  BLOCK_DEBOUNCE_S,
+  CLEAR_DEBOUNCE_S,
+  MAX_VALID_ROAD_EDGE_STD_M,
+  MIN_ACTIVE_SPEED_MPS,
+  REQUIRED_ROAD_EDGE_DISTANCE_M,
+  UNAVAILABLE_HOLD_S,
+  LateralEdgeGuard,
+  RoadEdgeDataState,
+  evaluate_road_edge,
+)
+from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ, ET
+from iqpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+
+
+@dataclass
+class Edge:
+  x: list[float]
+  y: list[float]
+
+
+@dataclass
+class ModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+
+
+class CarState:
+  def __init__(self, left_blindspot: bool = False) -> None:
+    self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
+    self.leftBlinker = True
+    self.rightBlinker = False
+    self.leftBlindspot = left_blindspot
+    self.rightBlindspot = False
+    self.steeringPressed = True
+    self.steeringTorque = 1.0
+    self.brakePressed = False
+    self.standstill = False
+
+
+def edge_model(left_distance_m: float = 6.0, right_distance_m: float = 6.0,
+               left_std_m: float = 0.0, right_std_m: float = 0.0) -> ModelData:
+  xs = [5.0, 20.0, 40.0]
+  return ModelData(
+    [Edge(xs, [-left_distance_m] * len(xs)), Edge(xs, [right_distance_m] * len(xs))],
+    [left_std_m, right_std_m],
+  )
+
+
+def cycles(duration_s: float) -> int:
+  return math.ceil(duration_s / DT_MDL)
+
+
+def update_for(guard: LateralEdgeGuard, modeldata: ModelData | None, duration_s: float,
+               speed_mps: float = MIN_ACTIVE_SPEED_MPS) -> None:
+  for _ in range(cycles(duration_s)):
+    guard.update(modeldata, speed_mps, DT_MDL)
+
+
+def test_valid_geometry_blocks_and_clear_geometry_does_not_block() -> None:
+  blocked = evaluate_road_edge(edge_model(4.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  clear = evaluate_road_edge(edge_model(6.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocked.state == RoadEdgeDataState.VALID
+  assert blocked.should_block is True
+  assert clear.state == RoadEdgeDataState.VALID
+  assert clear.should_block is False
+
+
+def test_unavailable_and_invalid_are_distinct() -> None:
+  unavailable = evaluate_road_edge(Edge([5.0], []), 0.2, log.LaneChangeDirection.left)
+  invalid = evaluate_road_edge(edge_model().roadEdges[0], MAX_VALID_ROAD_EDGE_STD_M + 0.01,
+                               log.LaneChangeDirection.left)
+  assert unavailable.state == RoadEdgeDataState.UNAVAILABLE
+  assert unavailable.lateral_distance_m is None
+  assert invalid.state == RoadEdgeDataState.INVALID
+  assert invalid.should_block is None
+
+
+def test_two_sigma_bound_uses_std_in_metres() -> None:
+  measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert measurement.lateral_distance_m == 5.0
+  assert measurement.conservative_distance_m == 4.6
+  assert measurement.should_block is True
+
+
+def test_distance_threshold_on_either_side() -> None:
+  epsilon_m = 0.001
+  for direction, edge_index in ((log.LaneChangeDirection.left, 0), (log.LaneChangeDirection.right, 1)):
+    below = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m)
+    above = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m)
+    assert evaluate_road_edge(below.roadEdges[edge_index], 0.0, direction).should_block is True
+    assert evaluate_road_edge(above.roadEdges[edge_index], 0.0, direction).should_block is False
+
+
+def test_block_debounce_rejects_a_single_clear_frame() -> None:
+  guard = LateralEdgeGuard()
+  blocking = edge_model(4.0)
+  clear = edge_model(6.0)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+  guard.update(clear, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_unavailable_holds_then_falls_back_to_not_blocking() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, None, UNAVAILABLE_HOLD_S - DT_MDL)
+  assert guard.left_measurement.state == RoadEdgeDataState.UNAVAILABLE
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(None, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_invalid_measurement_clears_through_release_debounce() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  invalid = edge_model(4.0, left_std_m=MAX_VALID_ROAD_EDGE_STD_M + 0.01)
+  update_for(guard, invalid, CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(invalid, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_speed_gate_is_inactive_below_threshold() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS - 0.01)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_desire_helper_keeps_edge_block_out_of_blindspot_path() -> None:
+  helper = DesireHelper()
+  helper.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  helper.lane_change_state = log.LaneChangeState.preLaneChange
+  helper.lane_change_direction = log.LaneChangeDirection.left
+  update_for(helper.lateral_edge_guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  blindspot_arguments: list[bool] = []
+
+  def record_blindspot(blindspot_detected: bool, brake_pressed: bool) -> None:
+    blindspot_arguments.append(blindspot_detected)
+
+  helper.alc.update_lane_change = record_blindspot
+  helper.update(CarState(left_blindspot=False), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments == [False]
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
+  assert helper.lane_change_state == log.LaneChangeState.preLaneChange
+
+  helper.update(CarState(left_blindspot=True), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments[-1] is True
+
+
+def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
+  message = messaging.new_message("iqDriveModelData")
+  message.iqDriveModelData.lateralEdgeBlock = custom.IQLateralEdgeBlock.right
+
+  class SubMaster:
+    updated = {"iqDriveModelData": True}
+
+    def __getitem__(self, service: str):
+      assert service == "iqDriveModelData"
+      return message.iqDriveModelData
+
+  selfdrived = SelfdriveD.__new__(SelfdriveD)
+  selfdrived.sm = SubMaster()
+  selfdrived._cached_model_event_names = ()
+  selfdrived._refresh_cached_model_events()
+
+  event_name = custom.IQOnroadEvent.EventName.lateralEdgeBlocked
+  assert selfdrived._cached_model_event_names == (event_name,)
+  alert = EVENTS_IQ[event_name][ET.WARNING]
+  assert alert.alert_text_1 == "Lane Change Blocked"
+  assert alert.alert_text_2 == "Road edge detected"

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

@@ -0,0 +1,190 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import numpy as np
+import pytest
+
+from iqdbc.car.honda.interface import CarInterface
+from iqdbc.car.honda.values import CAR
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+CRUISE = "cruise"
+SPEED_LIMIT_ASSIST = "speedLimitAssist"
+NAV = "nav"
+SOURCES = [CRUISE, SPEED_LIMIT_ASSIST, NAV]
+PLAN_SOURCE = custom.IQPlan.LongitudinalPlanSource
+
+V_CRUISE_MS = 25.0
+NAV_SPEED_TARGET = 11.0
+SLC_SPEED_TARGET = 12.0
+
+APPROACH_V_EGO = 11.2
+APPROACH_D_REL = 100.0
+APPROACH_STEPS = 250
+MIN_SAFE_GAP = 2.0
+COAST_THROTTLE_PROB = 0.1
+
+
+def build_planner(init_v=V_CRUISE_MS, init_a=0.0):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC)
+  return LongitudinalPlanner(CP, CP_IQ, init_v=init_v, init_a=init_a)
+
+
+def build_sm(v_ego, d_rel, v_lead, source, enabled=True, throttle_prob=1.0, a_ego=0.0, v_cruise=V_CRUISE_MS):
+  radar = messaging.new_message('radarState')
+  control = messaging.new_message('controlsState')
+  ss = messaging.new_message('selfdriveState')
+  car_state = messaging.new_message('carState')
+  car_control = messaging.new_message('carControl')
+  vehicle_params = messaging.new_message('vehicleParameters')
+  model = messaging.new_message('modelV2')
+  iq_car_state = messaging.new_message('iqCarState')
+  iq_nav_state = messaging.new_message('iqNavState')
+  iq_live_data = messaging.new_message('iqLiveData')
+  gps = messaging.new_message('gpsLocation')
+
+  lead = log.RadarState.LeadData.new_message()
+  lead.dRel = float(d_rel)
+  lead.vRel = float(v_lead - v_ego)
+  lead.vLead = float(v_lead)
+  lead.vLeadK = float(v_lead)
+  lead.status = True
+  lead.modelProb = 1.0
+  radar.radarState.leadOne = lead
+
+  t_idxs = np.array(ModelConstants.T_IDXS)
+  position = log.XYZTData.new_message()
+  position.x = [float(x) for x in v_ego * t_idxs]
+  model.modelV2.position = position
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(v_ego) for _ in t_idxs]
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [0.0 for _ in t_idxs]
+  model.modelV2.acceleration = acceleration
+  model.modelV2.action.desiredAcceleration = 0.0
+  model.modelV2.meta.disengagePredictions.gasPressProbs = [float(throttle_prob) for _ in range(6)]
+
+  lead_times = np.array(ModelConstants.LEAD_T_IDXS)
+  for lead_prediction in model.modelV2.leadsV3:
+    lead_prediction.prob = 1.0
+    lead_prediction.x = [float(d_rel + v_lead * t) for t in lead_times]
+    lead_prediction.v = [float(v_lead) for _ in lead_times]
+
+  control.controlsState.longControlState = LongCtrlState.pid if enabled else LongCtrlState.off
+  ss.selfdriveState.enabled = enabled
+  car_state.carState.vEgo = float(v_ego)
+  car_state.carState.aEgo = float(a_ego)
+  car_state.carState.standstill = bool(v_ego < 0.01)
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+  car_control.carControl.orientationNED = [0.0, 0.0, 0.0]
+
+  if source == NAV:
+    iq_nav_state.iqNavState.longitudinalEngaged = True
+    iq_nav_state.iqNavState.valid = True
+    iq_nav_state.iqNavState.speedTarget = NAV_SPEED_TARGET
+    iq_nav_state.iqNavState.accelTarget = 0.0
+
+  return {
+    'radarState': radar.radarState,
+    'carState': car_state.carState,
+    'carControl': car_control.carControl,
+    'controlsState': control.controlsState,
+    'selfdriveState': ss.selfdriveState,
+    'vehicleParameters': vehicle_params.vehicleParameters,
+    'modelV2': model.modelV2,
+    'iqCarState': iq_car_state.iqCarState,
+    'iqNavState': iq_nav_state.iqNavState,
+    'iqLiveData': iq_live_data.iqLiveData,
+    'gpsLocation': gps.gpsLocation,
+  }
+
+
+def stub_speed_limit_assist(planner):
+  planner.slimit.update = lambda *args, **kwargs: SLC_SPEED_TARGET
+
+
+def run_approach(planner, source, v_ego_0=APPROACH_V_EGO, d_rel_0=APPROACH_D_REL,
+                 steps=APPROACH_STEPS, throttle_prob=COAST_THROTTLE_PROB):
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+
+  v_ego = v_ego_0
+  d_rel = d_rel_0
+  prev_output_a_target = None
+  trace = []
+  for _ in range(steps):
+    planner.update(build_sm(v_ego, d_rel, 0.0, source, throttle_prob=throttle_prob))
+    trace.append({
+      'v_ego': v_ego,
+      'd_rel': d_rel,
+      'accels_0': float(planner.a_desired_trajectory[0]),
+      'prev_output_a_target': prev_output_a_target,
+      'output_a_target': float(planner.output_a_target),
+    })
+    prev_output_a_target = float(planner.output_a_target)
+    v_ego = max(0.0, v_ego + prev_output_a_target * planner.dt)
+    d_rel = max(0.0, d_rel - v_ego * planner.dt)
+  return trace
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_mpc_initial_accel_state_carries_previous_command(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  for i, step in enumerate(trace):
+    if step['prev_output_a_target'] is None:
+      continue
+    assert step['accels_0'] == pytest.approx(step['prev_output_a_target'], abs=1e-6), (
+      f"step {i} source={source}: MPC initial accel state was {step['accels_0']:.4f} "
+      f"but the previous commanded accel was {step['prev_output_a_target']:.4f}"
+    )
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_brakes_for_stopped_lead(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  min_gap = min(step['d_rel'] for step in trace)
+  assert min_gap > MIN_SAFE_GAP, (
+    f"source={source}: closed to {min_gap:.2f} m of a stopped lead first seen at "
+    f"{APPROACH_D_REL:.0f} m while coasting from {APPROACH_V_EGO:.1f} m/s"
+  )
+
+
+@pytest.mark.parametrize("source,expected", [
+  (CRUISE, V_CRUISE_MS),
+  (SPEED_LIMIT_ASSIST, SLC_SPEED_TARGET),
+  (NAV, NAV_SPEED_TARGET),
+])
+def test_speed_source_arbitration_unchanged(source, expected):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+  planner.update(build_sm(APPROACH_V_EGO, APPROACH_D_REL, 0.0, source))
+
+  assert planner.output_v_target == pytest.approx(expected, abs=1e-6)
+  assert planner.source == getattr(PLAN_SOURCE, source)
+
+
+def test_cruise_accel_initializes_from_planner_accel():
+  planner = build_planner(init_a=-0.35)
+
+  assert planner.a_cruise == pytest.approx(-0.35)
+
+
+def test_cruise_accel_resets_from_measured_accel():
+  a_ego = -0.45
+  v_ego = 20.0
+  planner = build_planner(init_v=v_ego)
+  planner.a_cruise = 0.5
+  planner.update(build_sm(v_ego, APPROACH_D_REL, v_ego, CRUISE, enabled=False, a_ego=a_ego, v_cruise=v_ego + a_ego))
+
+  assert planner.a_cruise == pytest.approx(a_ego, abs=1e-6)

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 iqpilot.cereal import custom, log
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
+from 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 iqpilot.common.constants import CV
+from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
+from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
+from 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),
+    "vehicleParameters": 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 iqpilot.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 iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.smooth_stops import (
+  SmoothStopController,
+  read_smooth_stops_enabled,
+  STANDSTILL_SPEED,
+  STANDSTILL_HOLD_SPEED,
+  SETTLE_DECEL,
+  TAPER_SPEED,
+  STOP_KISS_DECEL,
+  SETTLE_JERK,
+  EMERGENCY_DECEL,
+)
+
+JERK_STEP = SETTLE_JERK * DT_CTRL
+
+
+def _build(enabled=True):
+  c = SmoothStopController.__new__(SmoothStopController)
+  c.enabled = enabled
+  c._v_min = float("inf")
+  c._stall_s = 0.0
+  return c
+
+
+def test_unified_toggle_reads_force_stops():
+  seen = {}
+
+  class FakeParams:
+    def get_bool(self, key):
+      seen["key"] = key
+      return True
+
+  assert read_smooth_stops_enabled(FakeParams()) is True
+  assert seen["key"] == "IQForceStops"
+
+
+def test_hold_only_arms_at_standstill():
+  c = _build()
+  assert not c.want_hold(True, 0.5, False)
+  assert not c.want_hold(True, STANDSTILL_SPEED + 0.05, False)
+  assert not c.want_hold(True, 1.0, True)
+  assert not c.want_hold(True, STANDSTILL_HOLD_SPEED + 0.05, True)
+  assert c.want_hold(True, STANDSTILL_SPEED - 0.01, False)
+  assert c.want_hold(True, STANDSTILL_HOLD_SPEED - 0.01, True)
+  assert not c.want_hold(False, 0.0, True)
+
+
+def test_settle_feathers_toward_baseline():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -JERK_STEP
+
+
+def test_settle_never_softer_than_mpc():
+  c = _build()
+  out = c.settle(a_target=-2.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=-1.0)
+  assert out == -1.0 - JERK_STEP
+  assert out < -1.0
+
+
+def test_settle_emergency_bypasses_jerk_limit():
+  c = _build()
+  out = c.settle(a_target=-3.4, v_ego=2.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -3.4
+  assert out <= -EMERGENCY_DECEL
+
+
+def test_settle_lead_firms_up_when_close():
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=50.0, has_lead=True, last_output=-SETTLE_DECEL) == -SETTLE_DECEL
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=3.0, has_lead=True, last_output=-1.0) == -1.0
+
+
+def test_settle_anti_creep_firms_up_when_not_slowing():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=-SETTLE_DECEL)
+  for _ in range(60):
+    out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=out)
+  assert out < -SETTLE_DECEL
+
+
+def test_settle_eases_off_near_stop():
+  c = _build()
+  near = c.settle(a_target=0.0, v_ego=0.1, lead_distance=0.0, has_lead=False, last_output=-0.305)
+  c = _build()
+  high = c.settle(a_target=0.0, v_ego=0.9, lead_distance=0.0, has_lead=False, last_output=-0.745)
+  assert near > high
+  assert near == -(STOP_KISS_DECEL + (SETTLE_DECEL - STOP_KISS_DECEL) * (0.1 / TAPER_SPEED))
+
+
+def test_settle_kiss_decel_at_stop():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.0, lead_distance=0.0, has_lead=False, last_output=-STOP_KISS_DECEL)
+  assert out == -STOP_KISS_DECEL

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

@@ -0,0 +1,78 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import (
+  DesireHelper,
+  TURN_DESIRE_STOP_CYCLE_TIME,
+  TURN_DESIRE_STOP_HOLD_TIME,
+)
+
+
+TurnDirection = custom.IQTurnSignalDirection
+
+
+def helper(v_ego=0.0, yaw_rate=0.0):
+  result = DesireHelper.__new__(DesireHelper)
+  result._last_carstate = SimpleNamespace(vEgo=v_ego, yawRate=yaw_rate)
+  result.turn_desire_stop_timer = 0.0
+  result.turn_desire_stop_active = False
+  result.turn_desire_cycle_input = log.Desire.none
+  result.turn_desire_committed = False
+  result.nav_turn_direction = TurnDirection.none
+  result.lane_turn_direction = TurnDirection.none
+  result.lane_change_direction = log.LaneChangeDirection.none
+  result.lane_change_state = log.LaneChangeState.off
+  result.desire = log.Desire.none
+  return result
+
+
+@pytest.mark.parametrize("source", ["manual", "nav"])
+def test_manual_and_nav_turn_desires_receive_rising_edges(source):
+  h = helper()
+  if source == "manual":
+    h.lane_turn_direction = TurnDirection.turnLeft
+  else:
+    h.nav_turn_direction = TurnDirection.turnLeft
+
+  outputs = []
+  for _ in range(round((TURN_DESIRE_STOP_CYCLE_TIME + 2 * DT_MDL) / DT_MDL)):
+    h._pick_desire_output()
+    outputs.append(h.desire)
+
+  gap_index = next(i for i, output in enumerate(outputs) if output == log.Desire.none)
+  assert gap_index * DT_MDL == pytest.approx(TURN_DESIRE_STOP_HOLD_TIME, abs=DT_MDL * 1.1)
+  assert log.Desire.turnLeft in outputs[gap_index + 1:]
+
+
+def test_creeping_restarts_stopped_turn_cycle():
+  h = helper()
+  for _ in range(round(TURN_DESIRE_STOP_HOLD_TIME / DT_MDL)):
+    h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+
+  h._last_carstate.vEgo = 3.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  h._last_carstate.vEgo = 0.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  assert h.turn_desire_stop_timer == pytest.approx(DT_MDL)
+
+
+def test_measured_turn_commitment_stops_cycling():
+  h = helper()
+  h._last_carstate.yawRate = -0.1
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) == log.Desire.turnLeft
+  h._last_carstate.yawRate = 0.0
+
+  outputs = [h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) for _ in range(300)]
+  assert set(outputs) == {log.Desire.turnLeft}
+
+
+def test_new_turn_direction_rearms_cycle_after_commitment():
+  h = helper(yaw_rate=0.1)
+  h._cycle_turn_desire_when_stopped(log.Desire.turnLeft)
+  h._last_carstate.yawRate = 0.0
+  h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+  assert h.turn_desire_committed is False
+  assert h.turn_desire_cycle_input == log.Desire.turnRight

iqpilot/selfdrive/controls/plannerd.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+from iqpilot.cereal import car, custom
+from iqpilot.common.gps import get_gps_location_service
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import Priority, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.selfdrive.controls.lib.ldw import LaneDepartureWarning
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+import iqpilot.cereal.messaging as messaging
+
+
+def main():
+  config_realtime_process(5, Priority.CTRL_LOW)
+
+  cloudlog.info("plannerd is waiting for CarParams")
+  params = Params()
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+  cloudlog.info("plannerd got CarParams: %s", CP.brand)
+
+  cloudlog.info("plannerd is waiting for IQCarParams")
+  CP_IQ = messaging.log_from_bytes(params.get("IQCarParams", block=True), custom.IQCarParams)
+  cloudlog.info("plannerd got IQCarParams")
+
+  gps_location_service = get_gps_location_service(params)
+
+  ldw = LaneDepartureWarning()
+  longitudinal_planner = LongitudinalPlanner(CP, CP_IQ)
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'iqPlan'])
+  # poll modelV2 (stock): the whole loop body is gated on sm.updated['modelV2'], so a
+  # carState poll only makes carState the polled service with strict 100Hz receive bounds —
+  # each 20Hz planner iteration (>10ms) then swallows conflated carState messages, freq_ok
+  # drops below 80Hz, and every published plan goes event-invalid (commIssue, long degraded).
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'vehicleParameters', 'radarState', 'modelV2', 'selfdriveState',
+                            'iqLiveLocation', 'iqLiveData', 'iqNavState', 'iqCarState', 'iqConstructionZone', gps_location_service],
+                           poll='modelV2')
+
+  while True:
+    sm.update()
+    if sm.updated['modelV2']:
+      longitudinal_planner.update(sm)
+      longitudinal_planner.publish(sm, pm)
+
+      ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl'])
+      msg = messaging.new_message('driverAssistance')
+      msg.valid = sm.all_checks(['carState', 'carControl', 'modelV2', 'vehicleParameters'])
+      msg.driverAssistance.leftLaneDeparture = ldw.left
+      msg.driverAssistance.rightLaneDeparture = ldw.right
+      pm.send('driverAssistance', msg)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/radard.py

@@ -0,0 +1,303 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+from collections import deque
+from typing import Any
+
+import capnp
+from iqpilot.cereal import messaging, log, car, custom
+from iqpilot.common.filter_simple import FirstOrderFilter
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL, Priority, config_realtime_process
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.simple_kalman import KF1D
+
+from iqdbc.car import structs
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
+
+
+# Default lead acceleration decay set to 50% at 1s
+_LEAD_ACCEL_TAU = 1.5
+
+# radar tracks
+SPEED, ACCEL = 0, 1     # Kalman filter states enum
+
+# stationary qualification parameters
+V_EGO_STATIONARY = 4.   # no stationary object flag below this speed
+
+RADAR_TO_CENTER = 2.7   # (deprecated) RADAR is ~ 2.7m ahead from center of car
+RADAR_TO_CAMERA = 1.52  # RADAR is ~ 1.5m ahead from center of mesh frame
+
+
+class KalmanParams:
+  def __init__(self, dt: float):
+    # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
+    # hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
+    assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
+    self.A = [[1.0, dt], [0.0, 1.0]]
+    self.C = [1.0, 0.0]
+    #Q = np.matrix([[10., 0.0], [0.0, 100.]])
+    #R = 1e3
+    #K = np.matrix([[ 0.05705578], [ 0.03073241]])
+    dts = [i * 0.01 for i in range(1, 21)]
+    K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689,  0.21372394,
+          0.22761098, 0.24069424, 0.253096,   0.26491023, 0.27621103, 0.28705801,
+          0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
+          0.35353899, 0.36200124]
+    K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
+          0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
+          0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
+          0.26393339, 0.26278425]
+    self.K = [[np.interp(dt, dts, K0)], [np.interp(dt, dts, K1)]]
+
+
+class Track:
+  def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams):
+    self.identifier = identifier
+    self.cnt = 0
+    self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
+    self.K_A = kalman_params.A
+    self.K_C = kalman_params.C
+    self.K_K = kalman_params.K
+    self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K)
+
+  def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float):
+    # relative values, copy
+    self.dRel = d_rel   # LONG_DIST
+    self.yRel = y_rel   # -LAT_DIST
+    self.vRel = v_rel   # REL_SPEED
+    self.vLead = v_lead
+    self.measured = measured   # measured or estimate
+
+    # computed velocity and accelerations
+    if self.cnt > 0:
+      self.kf.update(self.vLead)
+
+    self.vLeadK = float(self.kf.x[SPEED][0])
+    self.aLeadK = float(self.kf.x[ACCEL][0])
+
+    # Learn if constant acceleration
+    if abs(self.aLeadK) < 0.5:
+      self.aLeadTau.x = _LEAD_ACCEL_TAU
+    else:
+      self.aLeadTau.update(0.0)
+
+    self.cnt += 1
+
+  def get_RadarState(self, model_prob: float = 0.0):
+    return {
+      "dRel": float(self.dRel),
+      "yRel": float(self.yRel),
+      "vRel": float(self.vRel),
+      "vLead": float(self.vLead),
+      "vLeadK": float(self.vLeadK),
+      "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau.x),
+      "status": True,
+      "fcw": self.is_potential_fcw(model_prob),
+      "modelProb": model_prob,
+      "radar": True,
+      "radarTrackId": self.identifier,
+    }
+
+  def potential_low_speed_lead(self, v_ego: float):
+    # stop for stuff in front of you and low speed, even without model confirmation
+    # Radar points closer than 0.75, are almost always glitches on toyota radars
+    return abs(self.yRel) < 1.0 and (v_ego < V_EGO_STATIONARY) and (0.75 < self.dRel < 25)
+
+  def is_potential_fcw(self, model_prob: float):
+    return model_prob > .9
+
+  def __str__(self):
+    ret = f"x: {self.dRel:4.1f}  y: {self.yRel:4.1f}  v: {self.vRel:4.1f}  a: {self.aLeadK:4.1f}"
+    return ret
+
+
+def laplacian_pdf(x: float, mu: float, b: float):
+  b = max(b, 1e-4)
+  return math.exp(-abs(x-mu)/b)
+
+
+def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
+  offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
+
+  def prob(c):
+    prob_d = laplacian_pdf(c.dRel, offset_vision_dist, lead.xStd[0])
+    prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0])
+    prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
+
+    # This isn't exactly right, but it's a good heuristic
+    return prob_d * prob_y * prob_v
+
+  track = max(tracks.values(), key=prob)
+
+  # if no 'sane' match is found return -1
+  # stationary radar points can be false positives
+  dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
+  vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3)
+  if dist_sane and vel_sane:
+    return track
+  else:
+    return None
+
+
+def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
+  lead_v_rel_pred = lead_msg.v[0] - model_v_ego
+  return {
+    "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
+    "yRel": float(-lead_msg.y[0]),
+    "vRel": float(lead_v_rel_pred),
+    "vLead": float(v_ego + lead_v_rel_pred),
+    "vLeadK": float(v_ego + lead_v_rel_pred),
+    "aLeadK": float(lead_msg.a[0]),
+    "aLeadTau": 0.3,
+    "fcw": False,
+    "modelProb": float(lead_msg.prob),
+    "status": True,
+    "radar": False,
+    "radarTrackId": -1,
+  }
+
+
+def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
+             model_v_ego: float, CP: structs.CarParams, CP_IQ: structs.IQCarParams, low_speed_override: bool = True) -> dict[str, Any]:
+  # Determine leads, this is where the essential logic happens
+  if len(tracks) > 0 and ready and lead_msg.prob > .5:
+    track = match_vision_to_track(v_ego, lead_msg, tracks)
+  else:
+    track = None
+
+  lead_dict = {'status': False}
+  if track is not None:
+    lead_dict = track.get_RadarState(lead_msg.prob)
+    lead_dict = get_custom_yrel(CP, CP_IQ, lead_dict, lead_msg)
+  elif (track is None) and ready and (lead_msg.prob > .5):
+    lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego)
+
+  if low_speed_override:
+    low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)]
+    if len(low_speed_tracks) > 0:
+      closest_track = min(low_speed_tracks, key=lambda c: c.dRel)
+
+      # Only choose new track if it is actually closer than the previous one
+      if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']):
+        lead_dict = closest_track.get_RadarState()
+
+  return lead_dict
+
+
+def get_custom_yrel(CP: structs.CarParams, CP_IQ: structs.IQCarParams, lead_dict: dict[str, Any],
+                    lead_msg: capnp._DynamicStructReader) -> dict[str, Any]:
+  if CP.brand == "hyundai" and (CP_IQ.flags & HyundaiFlagsIQ.ENHANCED_SCC or
+                                CP.flags & (HyundaiFlags.CANFD_CAMERA_SCC | HyundaiFlags.CAMERA_SCC)):
+    lead_dict['yRel'] = float(-lead_msg.y[0])
+
+  return lead_dict
+
+
+class RadarD:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.CarParams, delay: float = 0.0):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.current_time = 0.0
+
+    self.tracks: dict[int, Track] = {}
+    self.kalman_params = KalmanParams(DT_MDL)
+
+    self.v_ego = 0.0
+    self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1)
+    self.last_v_ego_frame = -1
+
+    self.radar_state: capnp._DynamicStructBuilder | None = None
+    self.radar_state_valid = False
+
+    self.ready = False
+
+    self.custom_stop_distance = CustomStopDistance()
+
+  def update(self, sm: messaging.SubMaster, rr: car.RadarData):
+    self.ready = sm.seen['modelV2']
+    self.current_time = 1e-9*max(sm.logMonoTime.values())
+    self.custom_stop_distance.update()
+
+    if sm.recv_frame['carState'] != self.last_v_ego_frame:
+      self.v_ego = sm['carState'].vEgo
+      self.v_ego_hist.append(self.v_ego)
+      self.last_v_ego_frame = sm.recv_frame['carState']
+
+    ar_pts = {pt.trackId: [pt.dRel, pt.yRel, pt.vRel, pt.measured] for pt in rr.points}
+
+    # *** remove missing points from meta data ***
+    for ids in list(self.tracks.keys()):
+      if ids not in ar_pts:
+        self.tracks.pop(ids, None)
+
+    # *** compute the tracks ***
+    for ids in ar_pts:
+      rpt = ar_pts[ids]
+
+      # align v_ego by a fixed time to align it with the radar measurement
+      v_lead = rpt[2] + self.v_ego_hist[0]
+
+      # create the track if it doesn't exist or it's a new track
+      if ids not in self.tracks:
+        self.tracks[ids] = Track(ids, v_lead, self.kalman_params)
+      self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3])
+
+    # *** publish radarState ***
+    self.radar_state_valid = sm.all_checks()
+    self.radar_state = log.RadarState.new_message()
+    self.radar_state.mdMonoTime = sm.logMonoTime['modelV2']
+    self.radar_state.radarErrors = rr.errors
+    self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
+
+    if len(sm['modelV2'].velocity.x):
+      model_v_ego = sm['modelV2'].velocity.x[0]
+    else:
+      model_v_ego = self.v_ego
+    leads_v3 = sm['modelV2'].leadsV3
+    if len(leads_v3) > 1:
+      lead_one = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.CP, self.CP_IQ, low_speed_override=True)
+      lead_two = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.CP, self.CP_IQ, low_speed_override=False)
+      self.radar_state.leadOne = self.custom_stop_distance.apply_lead(lead_one)
+      self.radar_state.leadTwo = self.custom_stop_distance.apply_lead(lead_two)
+
+  def publish(self, pm: messaging.PubMaster):
+    assert self.radar_state is not None
+
+    radar_msg = messaging.new_message("radarState")
+    radar_msg.valid = self.radar_state_valid
+    radar_msg.radarState = self.radar_state
+    pm.send("radarState", radar_msg)
+
+
+# fuses camera and radar data for best lead detection
+def main() -> None:
+  config_realtime_process(5, Priority.CTRL_LOW)
+
+  # wait for stats about the car to come in from controls
+  cloudlog.info("radard is waiting for CarParams")
+  CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
+  cloudlog.info("radard got CarParams")
+
+  cloudlog.info("radard is waiting for IQCarParams")
+  CP_IQ = messaging.log_from_bytes(Params().get("IQCarParams", block=True), custom.IQCarParams)
+  cloudlog.info("radard got IQCarParams")
+
+  # *** setup messaging
+  sm = messaging.SubMaster(['modelV2', 'carState', 'radarTracks'], poll='modelV2')
+  pm = messaging.PubMaster(['radarState'])
+
+  RD = RadarD(CP, CP_IQ, CP.radarDelay)
+
+  while 1:
+    sm.update()
+
+    RD.update(sm, sm['radarTracks'])
+    RD.publish(pm)
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/controls/tests/test_drive_helpers.py

@@ -0,0 +1,20 @@
+import pytest
+
+from iqpilot.selfdrive.controls.lib.drive_helpers import DEFAULT_STOPPING_SPEED, should_stop
+
+
+class TestShouldStop:
+  @pytest.mark.parametrize("v_ego, expected", [
+    (DEFAULT_STOPPING_SPEED - 0.01, True),
+    (DEFAULT_STOPPING_SPEED, False),
+  ])
+  def test_upstream_default(self, v_ego, expected):
+    assert should_stop(v_ego, -0.1) == expected
+
+  @pytest.mark.parametrize("stopping_speed", [0.55 / 3.6, 1.5 / 3.6])
+  def test_car_override(self, stopping_speed):
+    assert should_stop(stopping_speed - 0.01, -0.1, stopping_speed)
+    assert not should_stop(stopping_speed, -0.1, stopping_speed)
+
+  def test_requires_deceleration(self):
+    assert not should_stop(0.0, 0.1, 1.0)

iqpilot/selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,46 @@
+import pytest
+import itertools
+from parameterized import parameterized_class
+
+from iqpilot.cereal import log
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from iqpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+def desired_follow_distance(v_ego, v_lead, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
+
+def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0):
+  man = Maneuver(
+    '',
+    duration=t_end,
+    initial_speed=float(v_lead),
+    lead_relevancy=True,
+    initial_distance_lead=100,
+    speed_lead_values=[v_lead],
+    breakpoints=[0.],
+    e2e=e2e,
+    personality=personality,
+  )
+  valid, output = man.evaluate()
+  assert valid
+  return output[-1,2] - output[-1,1]
+
+
+@parameterized_class(("e2e", "personality", "speed"), itertools.product(
+                      [True, False], # e2e
+                      [log.LongitudinalPersonality.relaxed, # personality
+                       log.LongitudinalPersonality.standard,
+                       log.LongitudinalPersonality.aggressive],
+                      [0,10,35])) # speed
+class TestFollowingDistance:
+  def test_following_distance(self):
+    v_lead = float(self.speed)
+    simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality)
+    correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality))
+    err_ratio = 0.2 if self.e2e else 0.1
+    abs_err_margin = 0.5 if v_lead > 0.0 else 1.15
+    assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + abs_err_margin)

iqpilot/selfdrive/controls/tests/test_latcontrol.py

@@ -0,0 +1,106 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.honda.values import CAR as HONDA
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.nissan.values import CAR as NISSAN
+from iqdbc.car.gm.values import CAR as GM
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ
+import iqpilot.selfdrive.controls.lib.latcontrol_torque_pq as latcontrol_torque_pq
+from iqpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+
+class TestLatControl:
+
+  @staticmethod
+  def build_pq_controller():
+    car_name = TOYOTA.TOYOTA_RAV4
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    return CP, LatControlTorquePQ(CP.as_reader(), convert_to_capnp(CP_IQ).as_reader(), CI, DT_CTRL)
+
+  @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
+  def test_saturation(self, car_name, controller):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    CP_IQ = convert_to_capnp(CP_IQ)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    # Saturate for curvature limited and controller limited
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated
+
+  def test_pq_controller_update(self):
+    CP, controller = self.build_pq_controller()
+    VM = VehicleModel(CP)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    _, _, lac_log = controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert lac_log.active
+
+  def test_pq_controller_inactive_lookahead_and_slew_reset(self):
+    CP, controller = self.build_pq_controller()
+    controller.curvature_lookahead_enabled = True
+    controller.lateral_acceleration_slew_limiter.enabled = True
+    VM = VehicleModel(CP)
+    CS = car.CarState.new_message(vEgo=30)
+    params = log.VehicleParameters.new_message()
+    pose = Pose.from_live_pose(generate_deviceMotion().deviceMotion)
+
+    torque, angle, lac_log = controller.update(False, CS, VM, params, False, 0.001, pose, False, 0.2, lookahead_curvature=0.002)
+    assert torque == 0.0
+    assert angle == 0.0
+    assert not lac_log.active
+    assert controller.lateral_acceleration_slew_limiter.a_lim == 1.8
+
+  def test_pq_live_torque_update_freeze_and_unfreeze(self, monkeypatch):
+    _, controller = self.build_pq_controller()
+    initial = (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert (controller.torque_params.latAccelFactor, controller.torque_params.latAccelOffset, controller.torque_params.friction) == initial
+
+    monkeypatch.setattr(latcontrol_torque_pq, "FREEZE_LIVE_TORQUE_PARAMS", False)
+    controller.update_live_torque_params(3.0, 0.2, 0.4)
+    assert controller.torque_params.latAccelFactor == 3.0
+    assert abs(controller.torque_params.latAccelOffset - 0.2) < 1e-6
+    assert abs(controller.torque_params.friction - 0.4) < 1e-6

iqpilot/selfdrive/controls/tests/test_latcontrol_torque_buffer.py

@@ -0,0 +1,47 @@
+from parameterized import parameterized
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS
+
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqpilot.common.mock.generators import generate_deviceMotion
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+def get_controller(car_name):
+  CarInterface = interfaces[car_name]
+  CP = CarInterface.get_non_essential_params(car_name)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+  CI = CarInterface(CP, CP_IQ)
+  iqpilot_interfaces.apply_iq_car_config(CI)
+  CP_IQ = convert_to_capnp(CP_IQ)
+  VM = VehicleModel(CP)
+  controller = LatControlTorque(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+  return controller, VM
+
+class TestLatControlTorqueBuffer:
+
+  @parameterized.expand([(TOYOTA.TOYOTA_COROLLA_TSS2,)])
+  def test_request_buffer_consistency(self, car_name):
+    buffer_steps = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / DT_CTRL)
+    controller, VM = get_controller(car_name)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+    params = log.VehicleParameters.new_message()
+
+    lp = generate_deviceMotion()
+    pose = Pose.from_live_pose(lp.deviceMotion)
+
+    for _ in range(buffer_steps):
+      controller.update(True, CS, VM, params, False, 0.001, pose, False, 0.2)
+    assert all(val != 0 for val in controller.lat_accel_request_buffer)
+
+    for _ in range(buffer_steps):
+      controller.update(False, CS, VM, params, False, 0.0, pose, False, 0.2)
+    assert all(val == 0 for val in controller.lat_accel_request_buffer)

iqpilot/selfdrive/controls/tests/test_lateral_mpc.py

@@ -0,0 +1,85 @@
+import pytest
+import numpy as np
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from iqpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS
+from iqpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+
+
+def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
+            lane_width=3.6, poly_shift=0.):
+
+  if lat_mpc is None:
+    lat_mpc = LateralMpc()
+  lat_mpc.set_weights(1., .1, 0.0, .05, 800)
+
+  y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
+  heading_pts = np.zeros(LAT_MPC_N + 1)
+  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
+
+  x0 = np.array([x_init, y_init, psi_init, curvature_init])
+  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
+                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
+
+  # converge in no more than 10 iterations
+  for _ in range(10):
+    lat_mpc.run(x0, p,
+                y_pts, heading_pts, curv_rate_pts)
+  return lat_mpc.x_sol
+
+
+class TestLateralMpc:
+
+  def _assert_null(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,2] == pytest.approx(0, abs=curvature)
+      assert sol[0,i,3] == pytest.approx(0, abs=curvature)
+
+  def _assert_simmetry(self, sol, curvature=1e-6):
+    for i in range(len(sol)):
+      assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature)
+      assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature)
+      assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature)
+      assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature)
+
+  def test_straight(self):
+    sol = run_mpc()
+    self._assert_null(np.array([sol]))
+
+  def test_y_symmetry(self):
+    sol = []
+    for y_init in [-0.5, 0.5]:
+      sol.append(run_mpc(y_init=y_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_poly_symmetry(self):
+    sol = []
+    for poly_shift in [-1., 1.]:
+      sol.append(run_mpc(poly_shift=poly_shift))
+    self._assert_simmetry(np.array(sol))
+
+  def test_curvature_symmetry(self):
+    sol = []
+    for curvature_init in [-0.1, 0.1]:
+      sol.append(run_mpc(curvature_init=curvature_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_psi_symmetry(self):
+    sol = []
+    for psi_init in [-0.1, 0.1]:
+      sol.append(run_mpc(psi_init=psi_init))
+    self._assert_simmetry(np.array(sol))
+
+  def test_no_overshoot(self):
+    y_init = 1.
+    sol = run_mpc(y_init=y_init)
+    for y in list(sol[:,1]):
+      assert y_init >= abs(y)
+
+  def test_switch_convergence(self):
+    lat_mpc = LateralMpc()
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0)
+    right_psi_deg = np.degrees(sol[:,2])
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0)
+    left_psi_deg = np.degrees(sol[:,2])
+    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)

iqpilot/selfdrive/controls/tests/test_leads.py

@@ -0,0 +1,33 @@
+import iqpilot.cereal.messaging as messaging
+import pytest
+
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqpilot.selfdrive.test.process_replay import replay_process_with_name
+
+
+class TestLeads:
+  @pytest.mark.linux
+  def test_radar_fault(self):
+    # if there's no radar-related can traffic, radard should either not respond or respond with an error
+    # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check
+    def single_iter_pkg():
+      # single iter package, with meaningless cans and empty carState/modelV2
+      msgs = []
+      for _ in range(500):
+        can = messaging.new_message("can", 1)
+        cs = messaging.new_message("carState")
+        cp = messaging.new_message("carParams")
+        msgs.append(can.as_reader())
+        msgs.append(cs.as_reader())
+        msgs.append(cp.as_reader())
+      model = messaging.new_message("modelV2")
+      msgs.append(model.as_reader())
+
+      return msgs
+
+    msgs = [m for _ in range(3) for m in single_iter_pkg()]
+    out = replay_process_with_name("card", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2)
+    states = [m for m in out if m.which() == "radarTracks"]
+    failures = [not state.valid for state in states]
+
+    assert len(states) == 0 or all(failures)

iqpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -0,0 +1,72 @@
+from types import SimpleNamespace
+
+from iqpilot.cereal import custom
+from iqpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
+
+
+class TestLongControlStateTransition:
+
+  def test_stay_stopped(self):
+    CP_IQ = custom.IQCarParams.new_message()
+    active = True
+    current_state = LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.pid
+    active = False
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.off
+
+def test_engage():
+  CP_IQ = custom.IQCarParams.new_message()
+  active = True
+  current_state = LongCtrlState.off
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid
+
+
+def test_gas_override_preserves_negative_accel_command():
+  pid_calls = []
+  control = object.__new__(LongControl)
+  control.CP = SimpleNamespace(stopAccel=-0.55)
+  control.CP_IQ = SimpleNamespace(enableGasInterceptor=False)
+  control.long_control_state = LongCtrlState.pid
+  control.pid = SimpleNamespace(
+    update=lambda error, **kwargs: pid_calls.append((error, kwargs)) or -0.5,
+    reset=lambda: None,
+  )
+  control.last_output_accel = -0.4
+  control.stopping_decel_rate = 1.0
+  control.smooth = SimpleNamespace(enabled=False, update=lambda: None, reset=lambda: None)
+  car_state = SimpleNamespace(
+    vEgo=15.0,
+    aEgo=0.0,
+    brakePressed=False,
+    standstill=False,
+    cruiseState=SimpleNamespace(standstill=False),
+  )
+
+  output = control.update(True, car_state, -0.5, False, (-3.5, 2.0), gas_override=True)
+
+  assert output == -0.5
+  assert pid_calls == [(-0.5, {"speed": 15.0, "feedforward": -0.5, "freeze_integrator": True})]

iqpilot/selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,136 @@
+import numpy as np
+import pytest
+from types import SimpleNamespace
+
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LEAD_T_IDXS_MODEL, T_IDXS
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from iqpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import J_CRUISE, get_accel_candidates, get_cruise_accel, get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestCruiseAccel:
+  @pytest.mark.parametrize("v_cruise,v_ego,a_cruise_prev,direction", [
+    (10.0, 30.0, 0.4, -1.0),
+    (40.0, 30.0, -0.4, 1.0),
+  ])
+  def test_e2e_rate_limit(self, v_cruise, v_ego, a_cruise_prev, direction):
+    dt = 0.05
+    accel, _ = get_cruise_accel(True, v_cruise, v_ego, a_cruise_prev, 0.0, SimpleNamespace(), dt, 0.0, True)
+
+    assert accel == pytest.approx(a_cruise_prev + direction * J_CRUISE * dt)
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)
+
+
+class TestAccelCandidates:
+  MPC = (-0.2, LongitudinalPlanSource.lead0, True)
+  CRUISE = (0.5, LongitudinalPlanSource.cruise, False)
+  E2E = (0.1, LongitudinalPlanSource.e2e, False)
+
+  def test_e2e_without_lead_frees_model_from_mpc(self):
+    candidates = get_accel_candidates(True, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.e2e
+    assert not any(should_stop for _, _, should_stop in candidates)
+
+  def test_e2e_with_lead_keeps_mpc_safety_constraint(self):
+    candidates = get_accel_candidates(True, True, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.lead0
+    assert any(should_stop for _, _, should_stop in candidates)
+
+  def test_acc_without_lead_keeps_mpc_policy(self):
+    candidates = get_accel_candidates(False, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE]
+
+
+class TestExperimentalLeadMpc:
+  @staticmethod
+  def mpc(v_ego=20.0):
+    mpc = object.__new__(LongitudinalMpc)
+    mpc.x0 = np.array([0.0, v_ego, 0.0])
+    return mpc
+
+  @staticmethod
+  def model_lead(prob=0.9, x=None, v=None):
+    return SimpleNamespace(
+      prob=prob,
+      x=np.asarray(x if x is not None else [30.0, 66.0, 98.0, 126.0, 150.0, 170.0]),
+      v=np.asarray(v if v is not None else [20.0, 18.0, 16.0, 14.0, 12.0, 10.0]),
+    )
+
+  @staticmethod
+  def radar_lead(status=True, model_prob=0.9, radar=True):
+    return SimpleNamespace(
+      status=status,
+      dRel=28.0,
+      vLead=19.0,
+      aLeadK=-0.5,
+      aLeadTau=1.5,
+      vRel=-1.0,
+      modelProb=model_prob,
+      radar=radar,
+    )
+
+  def test_uses_valid_trajectory_with_radar_anchor(self):
+    lead_xv = self.mpc().process_lead(self.model_lead(), self.radar_lead())
+
+    assert lead_xv[0, 0] == pytest.approx(28.0)
+    assert lead_xv[0, 1] == pytest.approx(19.0)
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_uses_valid_vision_only_trajectory(self):
+    radar_lead = self.radar_lead(radar=False)
+    lead_xv = self.mpc().process_lead(self.model_lead(), radar_lead)
+
+    assert lead_xv[-1, 0] == pytest.approx(168.0)
+    assert lead_xv[-1, 1] == pytest.approx(9.0)
+
+  @pytest.mark.parametrize("model_lead, radar_lead", [
+    (model_lead.__func__(prob=0.5), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0]), radar_lead.__func__()),
+    (model_lead.__func__(x=[30.0, 66.0, 98.0, np.nan, 150.0, 170.0]), radar_lead.__func__()),
+    (model_lead.__func__(v=[20.0, 18.0, 16.0, np.inf, 12.0, 10.0]), radar_lead.__func__()),
+    (model_lead.__func__(), radar_lead.__func__(model_prob=0.0)),
+    (None, radar_lead.__func__()),
+  ])
+  def test_falls_back_to_radar_extrapolation(self, model_lead, radar_lead):
+    mpc = self.mpc()
+
+    assert np.array_equal(mpc.process_lead(model_lead, radar_lead), mpc.process_lead_legacy(radar_lead))
+
+  def test_prevents_backward_position_trajectory(self):
+    model_lead = self.model_lead(x=[30.0, 40.0, 38.0, 60.0, 80.0, 100.0])
+    lead_xv = self.mpc().process_lead(model_lead, self.radar_lead())
+
+    assert np.all(np.diff(lead_xv[:, 0]) >= 0.0)
+
+  def test_model_time_shape_matches_expected_horizon(self):
+    assert LEAD_T_IDXS_MODEL.shape == (6,)

iqpilot/selfdrive/controls/tests/test_torqued_lat_accel_offset.py

@@ -0,0 +1,70 @@
+import numpy as np
+from iqpilot.cereal import car, messaging
+from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
+from iqdbc.car import structs
+from iqdbc.car.lateral import get_friction, FRICTION_THRESHOLD
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS
+
+np.random.seed(0)
+
+LA_ERR_STD = 1.0
+INPUT_NOISE_STD = 0.08
+V_EGO = 30.0
+
+WARMUP_BUCKET_POINTS = (1.5*MIN_BUCKET_POINTS).astype(int)
+STRAIGHT_ROAD_LA_BOUNDS = (0.02, 0.03)
+
+ROLL_BIAS_DEG = 2.0
+ROLL_COMPENSATION_BIAS = ACCELERATION_DUE_TO_GRAVITY*float(np.sin(np.deg2rad(ROLL_BIAS_DEG)))
+TORQUE_TUNE = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=0.0, friction=0.2)
+TORQUE_TUNE_BIASED = structs.CarParams.LateralTorqueTuning(latAccelFactor=2.0, latAccelOffset=-ROLL_COMPENSATION_BIAS, friction=0.2)
+
+def generate_inputs(torque_tune, la_err_std, input_noise_std=None):
+  rng = np.random.default_rng(0)
+  steer_torques = np.concat([rng.uniform(bnd[0], bnd[1], pts) for bnd, pts in zip(STEER_BUCKET_BOUNDS, WARMUP_BUCKET_POINTS, strict=True)])
+  la_errs = rng.normal(scale=la_err_std, size=steer_torques.size)
+  frictions = np.array([get_friction(la_err, 0.0, FRICTION_THRESHOLD, torque_tune) for la_err in la_errs])
+  lat_accels = torque_tune.latAccelFactor*steer_torques + torque_tune.latAccelOffset + frictions
+  if input_noise_std is not None:
+    steer_torques += rng.normal(scale=input_noise_std, size=steer_torques.size)
+    lat_accels += rng.normal(scale=input_noise_std, size=steer_torques.size)
+  return steer_torques, lat_accels
+
+def get_warmed_up_estimator(steer_torques, lat_accels):
+  est = TorqueEstimator(car.CarParams())
+  for steer_torque, lat_accel in zip(steer_torques, lat_accels, strict=True):
+    est.filtered_points.add_point(steer_torque, lat_accel)
+  return est
+
+def simulate_straight_road_msgs(est):
+  carControl = messaging.new_message('carControl').carControl
+  carOutput = messaging.new_message('carOutput').carOutput
+  carState = messaging.new_message('carState').carState
+  deviceMotion = messaging.new_message('deviceMotion').deviceMotion
+  carControl.latActive = True
+  carState.vEgo = V_EGO
+  carState.steeringPressed = False
+  ts = DT_MDL*np.arange(2*POINTS_PER_BUCKET)
+  steer_torques = np.concat((np.linspace(-0.03, -0.02, POINTS_PER_BUCKET), np.linspace(0.02, 0.03, POINTS_PER_BUCKET)))
+  lat_accels = TORQUE_TUNE.latAccelFactor * steer_torques
+  for t, steer_torque, lat_accel in zip(ts, steer_torques, lat_accels, strict=True):
+    carOutput.actuatorsOutput.torque = float(-steer_torque)
+    deviceMotion.orientationNED.x = float(np.deg2rad(ROLL_BIAS_DEG))
+    deviceMotion.angularVelocityDevice.z = float(lat_accel / V_EGO)
+    for which, msg in (('carControl', carControl), ('carOutput', carOutput), ('carState', carState), ('deviceMotion', deviceMotion)):
+      est.handle_log(t, which, msg)
+
+def test_estimated_offset():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE_BIASED, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  msg = est.get_msg()
+  # TODO add lataccelfactor and friction check when we have more accurate estimates
+  assert abs(msg.lateralTorqueParameters.latAccelOffsetRaw - TORQUE_TUNE_BIASED.latAccelOffset) < 0.1
+
+def test_straight_road_roll_bias():
+  steer_torques, lat_accels = generate_inputs(TORQUE_TUNE, la_err_std=LA_ERR_STD, input_noise_std=INPUT_NOISE_STD)
+  est = get_warmed_up_estimator(steer_torques, lat_accels)
+  simulate_straight_road_msgs(est)
+  msg = est.get_msg()
+  assert (msg.lateralTorqueParameters.latAccelOffsetRaw < -0.05) and np.isfinite(msg.lateralTorqueParameters.latAccelOffsetRaw)