IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/controls/lib/__init__.py

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

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

@@ -25,9 +25,9 @@ Two mechanisms share the param:
 import numpy as np
 
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.modeld.constants import ModelConstants
+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

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/blinker_pause.py

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import car
+from iqpilot.cereal import car
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 
 class SignalPauseEngine:

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

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

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

@@ -2,11 +2,11 @@
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
 
-from cereal import messaging, custom
+from iqpilot.cereal import messaging, custom
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+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"

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

@@ -1,10 +1,10 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
+from iqpilot.cereal import custom, log
 
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
+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

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

@@ -6,10 +6,10 @@ from __future__ import annotations
 
 from dataclasses import dataclass
 
-from cereal import custom
+from iqpilot.cereal import custom
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
 
 TurnDirection = custom.IQTurnSignalDirection
 

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

@@ -6,8 +6,8 @@ turns and highway exits. This is a lateral-control add-on driven by iqNavState;
 it is independent of the feed-forward model and is off by default.
 """
 import numpy as np
-import cereal.messaging as messaging
-from cereal import custom
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
 
 TURN_NUDGE_TORQUE = 0.8
 EXIT_NUDGE_TORQUE = 0.6

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

@@ -5,10 +5,10 @@ from types import SimpleNamespace
 
 import pytest
 
-import cereal.messaging as messaging
-from cereal import custom
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+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

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

@@ -6,9 +6,9 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from cereal import custom
-import openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+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)
 
 

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

@@ -1,12 +1,12 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import messaging
+from iqpilot.cereal import messaging
 from numpy import interp
 from iqdbc.car import structs
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
+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,

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

@@ -17,7 +17,7 @@ Intent produced:
   radarSetSpeedKph  OP set speed to sync the radar's ACA_V_Wunsch toward
   radarGapBars      OP follow-distance bars to mirror to the radar
 """
-from openpilot.common.constants import CV
+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)
 

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

@@ -1,256 +1,297 @@
-"""
-Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
-"""
-from datetime import datetime
-
+#!/usr/bin/env python3
+import math
 import numpy as np
 
-from cereal import messaging, custom
-from iqdbc.car import structs
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.engine import IQDynamicController
-from openpilot.iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import IQConstants
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import EndToEndAlertEngine
-from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_ADAPT_ACC
-from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
-from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
+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
 
-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
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
 
-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
+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
 
-  def is_e2e(self, sm: messaging.SubMaster) -> bool:
-    experimental_mode = sm['selfdriveState'].experimentalMode
-    if not self.iq_dynamic.active():
-      return experimental_mode
+LAUNCH_DISARM_SPEED = 2.0
+LAUNCH_COMMIT_T = 3.5
+LAUNCH_MOVING_SPEED = 1.2
+LAUNCH_MAX_ACCEL = 1.5
 
-    return experimental_mode and self.iq_dynamic.mode() == "blended"
+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]
 
-  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
-    CS = sm['carState']
-    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
-    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
-    # SLC should apply whenever IQ.Pilot is engaged, even on stock-longitudinal cars
-    # where carControl.longActive stays false.
-    slc_apply_enabled = bool(getattr(sm['selfdriveState'], "enabled", False))
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
 
-    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)
+def get_max_accel(v_ego):
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
-    # 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)
+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:
-      clocks = sm.get('clocks', None) if isinstance(sm, dict) else None
-      time_validated = bool(getattr(clocks, 'timeValid', False))
-    slc_v_cruise = self.slimit.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
-    self.iq_dynamic.set_slc_experimental_mode(self.slimit.slc_experimental_mode)
-    self.iq_dynamic.update(sm)
-    # Prefer confirmed controller output for UI/planner rendering.
-    # Fall back to active (policy-resolved) target/source when confirmed is unavailable.
-    display_speed_limit = self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target
-    display_source = self.slimit.slc_source if self.slimit.slc_source != "None" else self.slimit.slc_active_source
-
-    if display_speed_limit > 0:
-      self.speed_limit_last = display_speed_limit
-      self.speed_limit_final_last = display_speed_limit + self.slimit.slc_offset
-    elif display_source == "None":
-      self.speed_limit_last = 0.0
-      self.speed_limit_final_last = 0.0
-    # Respect user-defined max cruise speed when applying SLC.
-    if v_cruise_cluster > 0 and self.speed_limit_final_last > 0:
-      self.speed_limit_final_last = min(self.speed_limit_final_last, v_cruise_cluster)
-    source_map = {
-      "Dashboard": SpeedLimitSource.car,
-      "Map Data": SpeedLimitSource.map,
-      "Mapbox": SpeedLimitSource.map,
-      "None": SpeedLimitSource.none,
-    }
-    self.speed_limit_source = source_map.get(display_source, SpeedLimitSource.none)
-
-    targets = {
-      LongitudinalPlanSource.cruise: (v_cruise, a_ego),
-      LongitudinalPlanSource.speedLimitAssist: (slc_v_cruise, a_ego),
-    }
-    if self.nav_valid:
-      targets[LongitudinalPlanSource.nav] = (self.nav_speed_target, self.nav_accel_target)
-
-    self.source = min(targets, key=lambda k: targets[k][0])
-    self.output_v_target, self.output_a_target = targets[self.source]
-    self.output_v_target = self._apply_force_stop(self.output_v_target, v_ego, sm, slc_apply_enabled)
-    # envelope shaping only in Assist mode: info/warn must never change the plan
-    self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slimit, "controller_enabled", False))
-                              and bool(getattr(self.slimit, "mode_assist", False)))
-    return self.output_v_target, self.output_a_target
-
-  def cruise_envelope(self, v_target: float, v_ego: float, t_idxs) -> np.ndarray:
-    """Per-timestep cruise speed over the MPC horizon: the scalar target, shaped down
-    ahead of an upcoming lower speed limit so the solver decelerates before the sign
-    instead of at it."""
-    env = np.full(len(t_idxs), max(float(v_target), 0.0))
-    if not getattr(self, "_envelope_enabled", False):
-      return env
-    slc = getattr(self.slimit, "slc", None)
-    next_limit = float(getattr(slc, "next_speed_limit", 0.0) or 0.0)
-    next_dist = float(getattr(slc, "next_speed_distance", 0.0) or 0.0)
-    if next_limit <= 0.0 or next_dist <= 0.0:
-      return env
-    next_target = max(next_limit + float(getattr(self.slimit, "slc_offset", 0.0) or 0.0), 0.0)
-    if next_target >= env[0]:
-      return env
-    travel = np.maximum(v_ego, 1.0) * np.asarray(t_idxs)
-    v_allowed = np.sqrt(np.maximum(next_target ** 2 + 2.0 * abs(LIMIT_ADAPT_ACC) * (next_dist - travel), next_target ** 2))
-    return np.minimum(env, v_allowed)
-
-  def update(self, sm: messaging.SubMaster) -> None:
-    self.events_iq.clear()
-    for event_name in getattr(self.slimit, 'pending_events', []):
-      self.events_iq.add(event_name)
-    self.custom_stop_distance.update()
-    self.e2e_alerts.update(sm, self.events_iq)
-    if bool(getattr(sm["iqCarState"], "alcOverrideAlert", False)):
-      self.events_iq.add(custom.IQOnroadEvent.EventName.steeringOverrideReengageAlc)
-
-  def apply_e2e_stop_distance(self, sm: messaging.SubMaster, v_ego: float, a_target: float, should_stop: bool) -> tuple[float, bool]:
-    if not self.is_e2e(sm):
-      return a_target, should_stop
-    return self.custom_stop_distance.adjust_e2e_stop(a_target, should_stop, v_ego, sm['modelV2'])
-
-  def _apply_force_stop(self, v_target: float, v_ego: float, sm: messaging.SubMaster, apply_enabled: bool) -> float:
-    force_stop = self.iq_dynamic.force_stop_requested() and apply_enabled and self.override_force_stop_timer <= 0.0
-    self.force_stop_timer = self.force_stop_timer + DT_MDL if force_stop else 0.0
-    force_stop_enabled = self.force_stop_timer >= 1.0
-    force_stop_ramp_time = max(float(getattr(self.iq_dynamic, "model_stop_time", IQConstants.FORCE_STOP_PLANNER_TIME)), DT_MDL)
-
-    accel_pressed = bool(getattr(sm["iqCarState"], "accelPressed", False))
-    self.override_force_stop |= sm["carState"].gasPressed or accel_pressed
-    self.override_force_stop &= force_stop_enabled
-
-    if self.override_force_stop:
-      self.override_force_stop_timer = 10.0
-    elif self.override_force_stop_timer > 0.0:
-      self.override_force_stop_timer = max(0.0, self.override_force_stop_timer - DT_MDL)
+      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:
-      self.override_force_stop = False
+      throttle_prob = 1.0
+    return x, v, a, j, throttle_prob
 
-    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)
+  def update(self, sm):
+    LongitudinalPlannerIQ.update(self, sm)
 
-    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,
+    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),
     )
-    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()
+    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)
 
-      # 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()
+    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)
 
-      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
+    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
 
-      # 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.
+  def publish(self, sm, pm):
+    plan_send = messaging.new_message('longitudinalPlan')
 
-      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
+    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,
+      )
 
-      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.)
+    longitudinalPlan = plan_send.longitudinalPlan
+    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
 
-      e2eAlerts = plan_msg.e2eAlerts
-      e2eAlerts.pathOpen = self.e2e_alerts.path_alert
-      e2eAlerts.leadPullaway = self.e2e_alerts.lead_alert
+    longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
+    longitudinalPlan.accels = self.a_desired_trajectory.tolist()
+    longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
 
-    valid = sm.all_checks(service_list=['carState', 'controlsState'])
+    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.leadDistance = get_lead_distance(sm['radarState'])
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    longitudinalPlan.fcw = self.fcw
 
-    plan_iq_send = messaging.new_message('iqPlan')
-    plan_iq_send.valid = valid
-    fill_plan(plan_iq_send.iqPlan)
-    pm.send('iqPlan', plan_iq_send)
+    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/tests/test_locator.py

@@ -10,7 +10,13 @@ import os
 import pytest
 
 from iqdbc.car import structs
-import openpilot.selfdrive.controls.lib.latcontrol_torque as locator
+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
@@ -83,3 +89,13 @@ 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

@@ -9,24 +9,24 @@ import os
 import numpy as np
 import pytest
 
-from openpilot.selfdrive.controls.lib.latcontrol_torque import NNTorqueModel
-from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+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": 4,
+  "input_size": 18,
   "output_size": 1,
-  "input_mean": [[0.0], [0.0], [0.0], [0.0]],
-  "input_std": [[1.0], [1.0], [1.0], [1.0]],
+  "input_mean": [[0.0]] * 18,
+  "input_std": [[1.0]] * 18,
   "layers": [
-    {"dense_1_W": [[0.5, 0.5, 0.5, 0.5], [0.5, 0.5, 0.5, 0.5]], "dense_1_b": [[0.0], [0.0]], "activation": "sigmoid"},
+    {"dense_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"))
+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
@@ -83,7 +83,8 @@ class TestModelBehavior:
 
 
 def test_activation_registry_rejects_unknown(tmp_path):
-  base = json.load(open(_path(SAMPLE[0])))
+  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))

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

@@ -10,20 +10,14 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
-from cereal import log
-from openpilot.common.params import Params
-from openpilot.common.pid import PIDController
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.selfdrive.controls.lib.latcontrol_torque import NeuralNetworkFeedForward
-from openpilot.selfdrive.controls.lib.latcontrol_torque import TORQUE_NN_MODEL_PATH
+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 sorted(os.listdir(TORQUE_NN_MODEL_PATH))
-                    if f.endswith(".json") and f != "MOCK.json"), None)
-
-# Models are shipped separately and re-added as retrained; with none present,
-# NNFF is a no-op (falls back to stock torque FF), so the assembly tests skip.
-pytestmark = pytest.mark.skipif(_REAL_MODEL is None,
-                                reason="no NNFF models present (nuked pending retraining)")
+_REAL_MODEL = next((f for f in _NAMES if f != "MOCK.json"), _NAMES[0])
 
 
 def _torque_fn():
@@ -53,7 +47,7 @@ def _model_v2():
 
 def _make_controller(model_file):
   Params().put_bool("NeuralNetworkFeedForward", True)
-  path = os.path.join(TORQUE_NN_MODEL_PATH, model_file)
+  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])))
@@ -84,7 +78,10 @@ class TestControllerWiring:
   def test_mock_model_reports_absent(self):
     nnff = _make_controller("MOCK.json")
     assert nnff.has_nn_model is False
-    assert nnff.model.input_size >= 2  # MOCK still loads as a valid net
+    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)

iqpilot/selfdrive/controls/lib/slc_vcruise.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python3
 import time
 
-from openpilot.common.constants import CV
-from openpilot.common.params import Params
-from openpilot.common.swaglog import cloudlog
-from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController
+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
 

iqpilot/selfdrive/controls/lib/smooth_stops.py

@@ -4,8 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
 """
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.common.params import Params
-from openpilot.common.realtime import DT_CTRL
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_CTRL
 
 STANDSTILL_SPEED = 0.05
 STANDSTILL_HOLD_SPEED = 0.15

iqpilot/selfdrive/controls/lib/speed_limit_controller.py

@@ -11,13 +11,13 @@ from concurrent.futures import ThreadPoolExecutor
 
 import numpy as np
 
-from cereal import car, custom
-from openpilot.common.constants import CV
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.swaglog import cloudlog
-from openpilot.iqpilot.common.k3_slc_log import k3_slc_log
-from openpilot.iqpilot.common.slc_utilities import calculate_bearing_offset, is_url_pingable
-from openpilot.iqpilot.common.slc_variables import FREE_MAPBOX_REQUESTS, OFFSET_MAP_IMPERIAL, OFFSET_MAP_METRIC, OFFSET_PERCENT_MAX
+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
@@ -375,8 +375,9 @@ class SpeedLimitController:
 
   def _resolve_tomtom_token(self) -> str:
     try:
-      from openpilot.iqpilot.navd.runtime_common import resolve_tomtom_token
-      return resolve_tomtom_token(self.params) or ""
+      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()
@@ -505,7 +506,7 @@ class SpeedLimitController:
       self.segment_distance = 0.0
       return
 
-    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    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
@@ -642,7 +643,7 @@ class SpeedLimitController:
       self.tomtom_limit = 0.0
       return
 
-    steer_angle = sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg
+    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

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

@@ -6,8 +6,8 @@ Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
 from types import SimpleNamespace
 
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import (
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import (
   CustomStopDistance,
   MIN_ADJUSTED_D_REL,
 )

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

@@ -4,8 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, license
 
 from types import SimpleNamespace
 
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerIQ
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
 
 
 class _FakeIQDynamic:

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

@@ -1,9 +1,9 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+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

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

@@ -5,10 +5,10 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 from datetime import datetime
 from types import SimpleNamespace
 
-from openpilot.common.constants import CV
-from openpilot.iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
-from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
+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:
@@ -36,7 +36,7 @@ def _build_sm(v_cruise_cluster=100.0, v_ego_cluster=27.8, gas=False, enabled=Tru
                                 steeringAngleDeg=0.0, buttonEvents=[]),
     "iqCarState": SimpleNamespace(speedLimit=iq_limit, accelPressed=False, decelPressed=False),
     "selfdriveState": SimpleNamespace(enabled=enabled),
-    "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
+    "vehicleParameters": SimpleNamespace(angleOffsetDeg=0.0),
   }
 
 
@@ -443,7 +443,7 @@ def test_get_offset_percent_clamped():
 
 
 def test_construction_zone_fires_event_once_per_zone_entry():
-  from cereal import custom
+  from iqpilot.cereal import custom
   event = custom.IQOnroadEvent.EventName.constructionZoneDetected
 
   controller = _construction_controller()

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

@@ -3,8 +3,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 
 Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
 """
-from openpilot.common.realtime import DT_CTRL
-from openpilot.iqpilot.selfdrive.controls.lib.smooth_stops import (
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.smooth_stops import (
   SmoothStopController,
   read_smooth_stops_enabled,
   STANDSTILL_SPEED,

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