IQ.Pilot Prebuilt Release @ ab07000

selfdrive/controls/lib/desire_helper.py

@@ -0,0 +1,269 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from cereal import car, custom, log
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_change import (
+  IQLaneSwapController,
+  AutoLaneChangeMode,
+  NavExitLaneChangeController,
+)
+from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
+
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = custom.IQTurnSignalDirection
+NavManeuverPhase = custom.IQNavState.ManeuverPhase
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.0
+TURN_DESIRE_STOP_HOLD_TIME = 4.8
+TURN_DESIRE_STOP_GAP_TIME = 1.0
+TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
+TURN_DESIRE_STOP_SPEED_EPS = 0.1
+
+_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.lane_turn_direction = TurnDirection.none
+    self.nav_turn_direction = TurnDirection.none
+    self.turn_desire_stop_timer = 0.0
+    self.turn_desire_stop_active = 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)
+    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:
+      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
+
+  def _is_standstill(self) -> bool:
+    return bool(getattr(self._last_carstate, "standstill", False) or self._last_carstate.vEgo <= TURN_DESIRE_STOP_SPEED_EPS)
+
+  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 not self._is_standstill():
+      self._clear_turn_desire_stop_cycle()
+      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
+    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()

selfdrive/controls/lib/drive_helpers.py

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

selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,31 @@
+import numpy as np
+from abc import abstractmethod, ABC
+from openpilot.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)

selfdrive/controls/lib/latcontrol_angle.py

@@ -0,0 +1,37 @@
+import math
+
+from cereal import log
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+
+# 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"
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
+    angle_log = log.ControlsState.LateralAngleState.new_message()
+
+    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(-desired_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

selfdrive/controls/lib/latcontrol_pid.py

@@ -0,0 +1,49 @@
+import math
+
+from cereal import log
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.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):
+    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

selfdrive/controls/lib/latcontrol_torque.py

@@ -0,0 +1,623 @@
+import json
+import math
+import os
+import tomllib
+from collections import deque
+from difflib import SequenceMatcher
+
+import numpy as np
+
+from 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 openpilot.common.basedir import BASEDIR
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.modeld.parse_model_outputs import safe_exp
+from openpilot.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 = os.path.join(BASEDIR, "iqdbc", "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
+  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.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):
+    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
+    future_desired_lateral_accel = desired_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,
+                                                       desired_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

selfdrive/controls/lib/latcontrol_torque_pq.py

@@ -0,0 +1,198 @@
+import math
+import numpy as np
+from collections import deque
+
+from cereal import log
+from iqdbc.car.lateral import get_friction
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+#   - Actuation delay dominates, not rack slew rate. liveDelay converged to
+#     0.32-0.40s (median 0.34) across routes; the 150/300-per-frame slew is
+#     irrelevant to loop stability because the delay caps usable loop gain.
+#     STIFFENING THE LOOP FAILED VALIDATION: KP=1.0/KI=0.2 (my first draft)
+#     produced ~25x the jerk cost. KP/KI are therefore left at generic values.
+#   - Feedforward must stay DETUNED relative to the open-loop plant gain.
+#     Measured open-loop latAccelFactor is ~1.6 (outer-region fit; torqued live
+#     median 1.69). But driving the feedforward at the matched 1.6 overshoots
+#     against the delay (replay cost 63.7 vs 54.9). A gentler effective factor
+#     of ~2.2 -- close to the old placeholder -- validated best. So we keep a
+#     detuned FF factor and FREEZE torqued's live override, which would
+#     otherwise pull it back toward matched and destabilize.
+#   - latAccelOffset = -0.13 (road-crown / device-roll bias, consistent across
+#     all routes) is the single biggest honest win: ~4.5 replay-cost points.
+#   - Friction compensation should be WIDE and gentle, not a tall narrow spike.
+#     torqued pins friction at its 0.2 cap on every route and the binned
+#     torque->lataccel curve shows a ~0.45-wide flat zone -- but that saturation
+#     is an artifact of torqued's NARROW interp needing a tall spike to cover
+#     the deadband. Spreading the SAME 0.1 amplitude over a wide error band
+#     (threshold 1.0) covers the deadband more gently and, unlike either a tall
+#     narrow ramp or a wide 0.2 ramp, does not dump enough torque at small
+#     errors to ring at saturation. A per-stretch regression guard (the article
+#     methodology) caught this: friction 0.2 gave a 504-pt worst-case
+#     single-stretch regression; friction 0.1 cut that to ~240 AND improved the
+#     mean, so 0.1 it is.
+#     PQ mean cost 58.2 vs 67.2 generic (+13%), better on ~60%
+#     of stretches, plant-check residual 0.22 m/s^2. The remaining worst-case
+#     regressions are LOW-SPEED (~15 m/s) saturated maneuvers where both
+#     controllers already score ~1000+ and where torque control is least valid
+#     (see class docstring in latcontrol_torque.py: lataccel<->torque only
+#     correlates cleanly above ~25mph) and where this high-speed-cruise-heavy
+#     plant fit is least trustworthy. A naive speed-gated blend of these params
+#     made things WORSE (time-varying jerk filter chatters across the band), so
+#     it was rejected rather than shipped. Treat absolutes as soft; confirm
+#     gains on-road. The deferred EPS-firmware pass will pin the true rack
+#     deadband/gain and let us revisit the FF detuning from first principles.
+
+FRICTION_THRESHOLD_PQ = 1.0    # wide, gentle friction-comp ramp (validated vs narrow 0.35)
+KP = 0.8                       # generic value; stiffening failed replay validation
+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      # rack settles fast once friction breaks
+JERK_LOOKAHEAD_SECONDS = 0.34  # matched to measured/converged lateral delay
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+# Feedforward plant params. FACTOR is deliberately detuned above the measured
+# open-loop gain (~1.6) for delay robustness; see header. torqued live updates
+# are frozen for this controller so it cannot drift back to matched.
+DEFAULT_LAT_ACCEL_FACTOR = 2.2
+DEFAULT_LAT_ACCEL_OFFSET = -0.13
+DEFAULT_FRICTION = 0.1          # spread wide (threshold 1.0); 0.2 rang at saturation
+FREEZE_LIVE_TORQUE_PARAMS = True
+
+# --- EPS assist-curve compensation (firmware-derived, PQ35_ZF_EPS_3501) ---
+# The ZF EPS does NOT apply LM_Offset (our torque command) to the rack 1:1. In
+# hca_lm_offset_torque_handler it computes
+#     rack_force = LM_Offset * hca_table[speed] >> 7
+# where hca_table is a speed-breakpoint curve (decoded from the binary at
+# 0x5e664). The multiplier / 128 is:
+#     0 km/h -> 0.688,  50 km/h -> 0.883,  120 km/h -> 1.211  (linear interp)
+# So the EPS delivers only ~0.69-0.84x of commanded torque at low speed and
+# ~1.21x at highway speed -- a 1.76x swing the stock torque controller is blind
+# to (it assumes a single latAccelFactor). This is exactly the region where the
+# controller felt under-assisted at low speed. We invert the KNOWN curve so the
+# LM_Offset->rack_force gain is flat across speed and the single-point FF tuning
+# holds everywhere. Normalized to ASSIST_REF so the validated latAccelFactor
+# (calibrated around highway speed) is unchanged at the reference point.
+#
+# Ghidra confirmation (full torque chain traced, not just this function): the
+# HCA_torque_map speed lookup is the ONLY speed-dependent scaling applied to our
+# command. output_torque_math's second multiplier (force_multiplier[row]) is a
+# per-variant scalar that also scales driver force, so it folds into the overall
+# latAccelFactor rather than adding speed dependence; the speed-interpolation
+# routine (FUN_00039b2a) is called only for this curve. Final motor torque is
+# clamped to 0x220=544. So the inversion below models the complete speed term.
+#
+# HONESTY: direction and magnitude here come from firmware, not a fit, so they
+# are trustworthy on their own terms. But the closed-loop logs are too noisy
+# (0.2 m/s^2 plant residual, composite-gain regression dominated by closed-loop
+# bias) to VALIDATE a cost improvement in replay -- so this is shipped as a
+# first-principles physical inversion to confirm on-road, not a replay-validated
+# gain. Toggle with ASSIST_COMPENSATION if on-road testing disagrees.
+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          # normalize so comp == 1 near highway calibration speed
+
+
+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)
+  # clamp the boost so a near-zero low-speed gain can't explode the command
+  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()
+    # Always seed the validated feedforward params. Unlike the generic
+    # controller we do not defer to whatever the platform carried, because the
+    # detuned FF factor is a deliberate tuning choice, not a plant estimate.
+    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)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    # Frozen: the detuned feedforward factor is intentional (see header). Letting
+    # torqued pull latAccelFactor toward the matched open-loop gain destabilizes
+    # against the 0.34s actuation delay.
+    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):
+    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
+    future_desired_lateral_accel = desired_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)
+      # Invert the EPS speed-dependent assist so the rack sees a flat gain (see
+      # header). The whole LM_Offset is scaled, matching where the EPS applies it.
+      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

selfdrive/controls/lib/lateral_mpc_lib/.gitignore

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

selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json

@@ -0,0 +1,538 @@
+{
+    "acados_include_path": "/data/openpilot/third_party/acados/include",
+    "acados_lib_path": "/data/openpilot/third_party/acados/lib",
+    "code_export_directory": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code",
+    "constraints": {
+        "C": [],
+        "C_e": [],
+        "D": [],
+        "constr_type": "BGH",
+        "constr_type_e": "BGH",
+        "idxbu": [],
+        "idxbx": [
+            2,
+            3
+        ],
+        "idxbx_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxbx_e": [],
+        "idxbxe_0": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxsbu": [],
+        "idxsbx": [],
+        "idxsbx_e": [],
+        "idxsg": [],
+        "idxsg_e": [],
+        "idxsh": [],
+        "idxsh_e": [],
+        "idxsphi": [],
+        "idxsphi_e": [],
+        "lbu": [],
+        "lbx": [
+            -1.5707963267948966,
+            -0.8726646259971648
+        ],
+        "lbx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lbx_e": [],
+        "lg": [],
+        "lg_e": [],
+        "lh": [],
+        "lh_e": [],
+        "lphi": [],
+        "lphi_e": [],
+        "lsbu": [],
+        "lsbx": [],
+        "lsbx_e": [],
+        "lsg": [],
+        "lsg_e": [],
+        "lsh": [],
+        "lsh_e": [],
+        "lsphi": [],
+        "lsphi_e": [],
+        "ubu": [],
+        "ubx": [
+            1.5707963267948966,
+            0.8726646259971648
+        ],
+        "ubx_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ubx_e": [],
+        "ug": [],
+        "ug_e": [],
+        "uh": [],
+        "uh_e": [],
+        "uphi": [],
+        "uphi_e": [],
+        "usbu": [],
+        "usbx": [],
+        "usbx_e": [],
+        "usg": [],
+        "usg_e": [],
+        "ush": [],
+        "ush_e": [],
+        "usphi": [],
+        "usphi_e": []
+    },
+    "cost": {
+        "Vu": [],
+        "Vu_0": [],
+        "Vx": [],
+        "Vx_0": [],
+        "Vx_e": [],
+        "Vz": [],
+        "Vz_0": [],
+        "W": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_e": [
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Zl": [],
+        "Zl_e": [],
+        "Zu": [],
+        "Zu_e": [],
+        "cost_ext_fun_type": "casadi",
+        "cost_ext_fun_type_0": "casadi",
+        "cost_ext_fun_type_e": "casadi",
+        "cost_type": "NONLINEAR_LS",
+        "cost_type_0": "NONLINEAR_LS",
+        "cost_type_e": "NONLINEAR_LS",
+        "yref": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "zl": [],
+        "zl_e": [],
+        "zu": [],
+        "zu_e": []
+    },
+    "cython_include_dirs": [
+        "/data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include",
+        "/usr/include/python3.12"
+    ],
+    "dims": {
+        "N": 32,
+        "nbu": 0,
+        "nbx": 2,
+        "nbx_0": 4,
+        "nbx_e": 0,
+        "nbxe_0": 4,
+        "ng": 0,
+        "ng_e": 0,
+        "nh": 0,
+        "nh_e": 0,
+        "np": 2,
+        "nphi": 0,
+        "nphi_e": 0,
+        "nr": 0,
+        "nr_e": 0,
+        "ns": 0,
+        "ns_e": 0,
+        "nsbu": 0,
+        "nsbx": 0,
+        "nsbx_e": 0,
+        "nsg": 0,
+        "nsg_e": 0,
+        "nsh": 0,
+        "nsh_e": 0,
+        "nsphi": 0,
+        "nsphi_e": 0,
+        "nu": 1,
+        "nx": 4,
+        "ny": 5,
+        "ny_0": 5,
+        "ny_e": 3,
+        "nz": 0
+    },
+    "json_file": "/data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/acados_ocp_lat.json",
+    "model": {
+        "con_h_expr": null,
+        "con_h_expr_e": null,
+        "con_phi_expr": null,
+        "con_phi_expr_e": null,
+        "con_r_expr": null,
+        "con_r_expr_e": null,
+        "con_r_in_phi": null,
+        "con_r_in_phi_e": null,
+        "cost_conl_custom_outer_hess": null,
+        "cost_conl_custom_outer_hess_0": null,
+        "cost_conl_custom_outer_hess_e": null,
+        "cost_expr_ext_cost": null,
+        "cost_expr_ext_cost_0": null,
+        "cost_expr_ext_cost_custom_hess": null,
+        "cost_expr_ext_cost_custom_hess_0": null,
+        "cost_expr_ext_cost_custom_hess_e": null,
+        "cost_expr_ext_cost_e": null,
+        "cost_psi_expr": null,
+        "cost_psi_expr_0": null,
+        "cost_psi_expr_e": null,
+        "cost_r_in_psi_expr": null,
+        "cost_r_in_psi_expr_0": null,
+        "cost_r_in_psi_expr_e": null,
+        "cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
+        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpgegeaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaachkjjjjjjjjjjjjlpd",
+        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegdaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgkaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
+        "disc_dyn_expr": null,
+        "dyn_disc_fun": null,
+        "dyn_disc_fun_jac": null,
+        "dyn_disc_fun_jac_hess": null,
+        "dyn_ext_fun_type": "casadi",
+        "dyn_generic_source": null,
+        "f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachfaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaachiaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
+        "f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegcaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegoaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdhegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachcaaaaaaaaaaaaaaaegnaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaachgaaaaaaaaaaaaaaaegoaaaaaaaaaaaaaaachdaaaaaaaaaaaaaaachjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehchjaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgehegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
+        "gnsf": {
+            "nontrivial_f_LO": 1,
+            "purely_linear": 0
+        },
+        "name": "lat",
+        "p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeggaaaaaaaaaaaaaaacaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaapaaaaaaachpgehbgehjgpgogpfchbgegjgfhdh",
+        "u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaaahdhjgpfbgdgdgfgmgpffghgpg",
+        "x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaajhpffghgpgegpcaaaaaaaaaaaaaahaaaaaaaahdhjgpffghgpgegpcaaaaaaaaaaaaaamaaaaaaaahdhjgpfchbgehfgpffghgpg",
+        "xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaajhpffghgpgpfegpgehegpcaaaaaaaaaaaaaalaaaaaaaahdhjgpffghgpgpfegpgehegpcaaaaaaaaaaaaaaabaaaaaaahdhjgpfchbgehfgpffghgpgpfegpgeh",
+        "z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
+    },
+    "parameter_values": [
+        0.0,
+        0.0
+    ],
+    "problem_class": "OCP",
+    "shared_lib_ext": ".so",
+    "solver_options": {
+        "Tsim": 0.009765625,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "custom_templates": [],
+        "custom_update_copy": true,
+        "custom_update_filename": "",
+        "custom_update_header_filename": "",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "ext_fun_compile_flags": "-O2",
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_ext_qp_res": 0,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_cond_ric_alg": 1,
+        "qp_solver_iter_max": 1,
+        "qp_solver_ric_alg": 1,
+        "qp_solver_tol_comp": null,
+        "qp_solver_tol_eq": null,
+        "qp_solver_tol_ineq": null,
+        "qp_solver_tol_stat": null,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "shooting_nodes": [
+            0.0,
+            0.009765625,
+            0.0390625,
+            0.087890625,
+            0.15625,
+            0.244140625,
+            0.3515625,
+            0.478515625,
+            0.625,
+            0.791015625,
+            0.9765625,
+            1.181640625,
+            1.40625,
+            1.650390625,
+            1.9140625,
+            2.197265625,
+            2.5,
+            2.822265625,
+            3.1640625,
+            3.525390625,
+            3.90625,
+            4.306640625,
+            4.7265625,
+            5.166015625,
+            5.625,
+            6.103515625,
+            6.6015625,
+            7.119140625,
+            7.65625,
+            8.212890625,
+            8.7890625,
+            9.384765625,
+            10.0
+        ],
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_newton_tol": 0.0,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 10.0,
+        "time_steps": [
+            0.009765625,
+            0.029296875,
+            0.048828125,
+            0.068359375,
+            0.087890625,
+            0.107421875,
+            0.126953125,
+            0.146484375,
+            0.166015625,
+            0.185546875,
+            0.205078125,
+            0.224609375,
+            0.244140625,
+            0.263671875,
+            0.283203125,
+            0.302734375,
+            0.322265625,
+            0.341796875,
+            0.361328125,
+            0.380859375,
+            0.400390625,
+            0.419921875,
+            0.439453125,
+            0.458984375,
+            0.478515625,
+            0.498046875,
+            0.517578125,
+            0.537109375,
+            0.556640625,
+            0.576171875,
+            0.595703125,
+            0.615234375
+        ]
+    }
+}

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/Makefile

@@ -0,0 +1,211 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+
+
+
+# define sources and use make's implicit rules to generate object files (*.o)
+
+# model
+MODEL_SRC=
+MODEL_SRC+= lat_model/lat_expl_ode_fun.c
+MODEL_SRC+= lat_model/lat_expl_vde_forw.c
+MODEL_SRC+= lat_model/lat_expl_vde_adj.c
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
+
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
+OCP_SRC+= lat_cost/lat_cost_y_0_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_0_hess.c
+OCP_SRC+= lat_cost/lat_cost_y_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_hess.c
+OCP_SRC+= lat_cost/lat_cost_y_e_fun.c
+OCP_SRC+= lat_cost/lat_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= lat_cost/lat_cost_y_e_hess.c
+
+OCP_SRC+= acados_solver_lat.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
+
+# for sim solver
+SIM_SRC= acados_sim_solver_lat.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
+
+# for target example
+EX_SRC= main_lat.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
+
+# for target example_sim
+EX_SIM_SRC= main_sim_lat.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
+
+# combine model, sim and ocp object files
+OBJ=
+OBJ+= $(MODEL_OBJ)
+OBJ+= $(SIM_OBJ)
+OBJ+= $(OCP_OBJ)
+
+EXTERNAL_DIR=
+EXTERNAL_LIB=
+
+INCLUDE_PATH = /data/openpilot/third_party/acados/include
+LIB_PATH = /data/openpilot/third_party/acados/lib
+
+# preprocessor flags for make's implicit rules
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+
+
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99   -O2#-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
+
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
+
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= 
+
+# libraries
+LIBACADOS_SOLVER=libacados_solver_lat.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_lat.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+
+# virtual targets
+.PHONY : all clean
+
+#all: clean example_sim example shared_lib
+
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
+
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
+
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
+
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+
+# Cython targets
+ocp_cython_c: ocp_shared_lib
+	cython \
+	-o acados_ocp_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code \
+
+ocp_cython_o: ocp_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_ocp_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
+	acados_ocp_solver_pyx.c \
+
+ocp_cython: ocp_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_ocp_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_ocp_solver_pyx.o \
+	$(abspath .)/libacados_ocp_solver_lat.so \
+	$(LDFLAGS) $(LDLIBS)
+
+# Sim Cython targets
+sim_cython_c: sim_shared_lib
+	cython \
+	-o acados_sim_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/lateral_mpc_lib/c_generated_code \
+
+sim_cython_o: sim_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_sim_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
+	acados_sim_solver_pyx.c \
+
+sim_cython: sim_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_sim_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_sim_solver_pyx.o \
+	$(abspath .)/libacados_sim_solver_lat.so \
+	$(LDFLAGS) $(LDLIBS)
+
+clean:
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
+
+clean_ocp_shared_lib:
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_lat.so
+	$(RM) acados_solver_lat.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
+
+clean_sim_cython:
+	$(RM) libacados_sim_solver_lat.so
+	$(RM) acados_sim_solver_lat.o
+	$(RM) acados_sim_solver_pyx.so
+	$(RM) acados_sim_solver_pyx.o

selfdrive/controls/lib/lateral_mpc_lib/gen.log

@@ -0,0 +1,49 @@
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:54: SyntaxWarning: invalid escape sequence '\d'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:60: SyntaxWarning: invalid escape sequence '\d'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:111: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:116: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:121: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:126: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:131: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_model.py:136: SyntaxWarning: invalid escape sequence '\p'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:119: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{\pi}` - dimension of the image of the inner nonlinear function in positive definite constraints.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:125: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{\pi}^e` - dimension of the image of the inner nonlinear function in positive definite constraints.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:143: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{\phi}` - number of convex-over-nonlinear constraints.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:149: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{\phi}^e` - number of convex-over-nonlinear constraints at terminal shooting node N.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:227: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{{s\phi}}` - number of soft convex-over-nonlinear constraints.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:233: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`n_{{s\phi}^e}` - number of soft convex-over-nonlinear constraints at terminal shooting node N.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:496: SyntaxWarning: invalid escape sequence '\D'
+  """
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1193: SyntaxWarning: invalid escape sequence '\,'
+  """:math:`C` - C matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1201: SyntaxWarning: invalid escape sequence '\,'
+  """:math:`D` - D matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1285: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`\\underline{\phi}` - lower bound for convex-over-nonlinear inequalities
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1293: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`\\bar{\phi}` - upper bound for convex-over-nonlinear inequalities
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1302: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`\\underline{\phi}^e` - lower bound on convex-over-nonlinear inequalities
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1310: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`\\bar{\phi}^e` - upper bound on convex-over-nonlinear inequalities
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1485: SyntaxWarning: invalid escape sequence '\p'
+  """:math:`J_{s, \phi}` - matrix coefficient for soft bounds on convex-over-nonlinear constraints.
+/data/openpilot/openpilot/third_party/acados/acados_template/acados_ocp.py:1572: SyntaxWarning: invalid escape sequence '\m'
+  """:math:`x_0 \\in \mathbb{R}^{n_x}` - initial state --
+Warning: Please note that the following versions of CasADi  are officially supported: 3.5.6 or 3.5.5 or 3.5.4 or 3.5.3 or 3.5.2 or 3.5.1 or 3.4.5 or 3.4.0.
+ If there is an incompatibility with the CasADi generated code, please consider changing your CasADi version.
+Version 3.7.2 currently in use.

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 openpilot.selfdrive.modeld.constants import ModelConstants
+
+if __name__ == '__main__':  # generating code
+  from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from openpilot.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)

selfdrive/controls/lib/ldw.py

@@ -0,0 +1,41 @@
+from cereal import log
+from openpilot.common.realtime import DT_CTRL
+from openpilot.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)

selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,99 @@
+import numpy as np
+from cereal import car
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.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()
+
+    if gas_override:
+      # safety blocks braking while the gas is pressed, and a blocked tx drops the whole frame
+      output_accel = max(output_accel, 0.0)
+
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
+    return self.last_output_accel

selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

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

selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -0,0 +1,483 @@
+{
+    "acados_include_path": "/data/openpilot/third_party/acados/include",
+    "acados_lib_path": "/data/openpilot/third_party/acados/lib",
+    "code_export_directory": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
+    "constraints": {
+        "C": [],
+        "C_e": [],
+        "D": [],
+        "constr_type": "BGH",
+        "constr_type_e": "BGH",
+        "idxbu": [],
+        "idxbx": [],
+        "idxbx_0": [
+            0,
+            1,
+            2
+        ],
+        "idxbx_e": [],
+        "idxbxe_0": [
+            0,
+            1,
+            2
+        ],
+        "idxsbu": [],
+        "idxsbx": [],
+        "idxsbx_e": [],
+        "idxsg": [],
+        "idxsg_e": [],
+        "idxsh": [
+            0,
+            1,
+            2,
+            3
+        ],
+        "idxsh_e": [],
+        "idxsphi": [],
+        "idxsphi_e": [],
+        "lbu": [],
+        "lbx": [],
+        "lbx_0": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lbx_e": [],
+        "lg": [],
+        "lg_e": [],
+        "lh": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lh_e": [],
+        "lphi": [],
+        "lphi_e": [],
+        "lsbu": [],
+        "lsbx": [],
+        "lsbx_e": [],
+        "lsg": [],
+        "lsg_e": [],
+        "lsh": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "lsh_e": [],
+        "lsphi": [],
+        "lsphi_e": [],
+        "ubu": [],
+        "ubx": [],
+        "ubx_0": [
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ubx_e": [],
+        "ug": [],
+        "ug_e": [],
+        "uh": [
+            10000.0,
+            10000.0,
+            10000.0,
+            10000.0
+        ],
+        "uh_e": [],
+        "uphi": [],
+        "uphi_e": [],
+        "usbu": [],
+        "usbx": [],
+        "usbx_e": [],
+        "usg": [],
+        "usg_e": [],
+        "ush": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "ush_e": [],
+        "usphi": [],
+        "usphi_e": []
+    },
+    "cost": {
+        "Vu": [],
+        "Vu_0": [],
+        "Vx": [],
+        "Vx_0": [],
+        "Vx_e": [],
+        "Vz": [],
+        "Vz_0": [],
+        "W": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_0": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "W_e": [
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ],
+            [
+                0.0,
+                0.0,
+                0.0,
+                0.0
+            ]
+        ],
+        "Zl": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "Zl_e": [],
+        "Zu": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "Zu_e": [],
+        "cost_ext_fun_type": "casadi",
+        "cost_ext_fun_type_0": "casadi",
+        "cost_ext_fun_type_e": "casadi",
+        "cost_type": "NONLINEAR_LS",
+        "cost_type_0": "NONLINEAR_LS",
+        "cost_type_e": "NONLINEAR_LS",
+        "yref": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "zl": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "zl_e": [],
+        "zu": [
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "zu_e": []
+    },
+    "cython_include_dirs": [
+        "/data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include",
+        "/usr/include/python3.12"
+    ],
+    "dims": {
+        "N": 12,
+        "nbu": 0,
+        "nbx": 0,
+        "nbx_0": 3,
+        "nbx_e": 0,
+        "nbxe_0": 3,
+        "ng": 0,
+        "ng_e": 0,
+        "nh": 4,
+        "nh_e": 0,
+        "np": 5,
+        "nphi": 0,
+        "nphi_e": 0,
+        "nr": 0,
+        "nr_e": 0,
+        "ns": 4,
+        "ns_e": 0,
+        "nsbu": 0,
+        "nsbx": 0,
+        "nsbx_e": 0,
+        "nsg": 0,
+        "nsg_e": 0,
+        "nsh": 4,
+        "nsh_e": 0,
+        "nsphi": 0,
+        "nsphi_e": 0,
+        "nu": 1,
+        "nx": 3,
+        "ny": 5,
+        "ny_0": 5,
+        "ny_e": 4,
+        "nz": 0
+    },
+    "json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
+    "model": {
+        "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
+        "con_h_expr_e": null,
+        "con_phi_expr": null,
+        "con_phi_expr_e": null,
+        "con_r_expr": null,
+        "con_r_expr_e": null,
+        "con_r_in_phi": null,
+        "con_r_in_phi_e": null,
+        "cost_conl_custom_outer_hess": null,
+        "cost_conl_custom_outer_hess_0": null,
+        "cost_conl_custom_outer_hess_e": null,
+        "cost_expr_ext_cost": null,
+        "cost_expr_ext_cost_0": null,
+        "cost_expr_ext_cost_custom_hess": null,
+        "cost_expr_ext_cost_custom_hess_0": null,
+        "cost_expr_ext_cost_custom_hess_e": null,
+        "cost_expr_ext_cost_e": null,
+        "cost_psi_expr": null,
+        "cost_psi_expr_0": null,
+        "cost_psi_expr_e": null,
+        "cost_r_in_psi_expr": null,
+        "cost_r_in_psi_expr_0": null,
+        "cost_r_in_psi_expr_e": null,
+        "cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgdaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
+        "disc_dyn_expr": null,
+        "dyn_disc_fun": null,
+        "dyn_disc_fun_jac": null,
+        "dyn_disc_fun_jac_hess": null,
+        "dyn_ext_fun_type": "casadi",
+        "dyn_generic_source": null,
+        "f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "gnsf": {
+            "nontrivial_f_LO": 1,
+            "purely_linear": 0
+        },
+        "name": "long",
+        "p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
+        "u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
+        "xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
+        "z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
+    },
+    "parameter_values": [
+        -1.2,
+        1.2,
+        0.0,
+        1.45,
+        0.75
+    ],
+    "problem_class": "OCP",
+    "shared_lib_ext": ".so",
+    "solver_options": {
+        "Tsim": 0.06944444444444445,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "custom_templates": [],
+        "custom_update_copy": true,
+        "custom_update_filename": "",
+        "custom_update_header_filename": "",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "ext_fun_compile_flags": "-O2",
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_ext_qp_res": 0,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_cond_ric_alg": 1,
+        "qp_solver_iter_max": 10,
+        "qp_solver_ric_alg": 1,
+        "qp_solver_tol_comp": 0.001,
+        "qp_solver_tol_eq": 0.001,
+        "qp_solver_tol_ineq": 0.001,
+        "qp_solver_tol_stat": 0.001,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "shooting_nodes": [
+            0.0,
+            0.06944444444444445,
+            0.2777777777777778,
+            0.625,
+            1.1111111111111112,
+            1.7361111111111114,
+            2.5,
+            3.4027777777777786,
+            4.444444444444445,
+            5.625,
+            6.9444444444444455,
+            8.402777777777777,
+            10.0
+        ],
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_newton_tol": 0.0,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 10.0,
+        "time_steps": [
+            0.06944444444444445,
+            0.20833333333333334,
+            0.3472222222222222,
+            0.48611111111111116,
+            0.6250000000000002,
+            0.7638888888888886,
+            0.9027777777777786,
+            1.041666666666666,
+            1.1805555555555554,
+            1.3194444444444455,
+            1.4583333333333313,
+            1.5972222222222232
+        ]
+    }
+}

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/Makefile

@@ -0,0 +1,213 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+
+
+
+# define sources and use make's implicit rules to generate object files (*.o)
+
+# model
+MODEL_SRC=
+MODEL_SRC+= long_model/long_expl_ode_fun.c
+MODEL_SRC+= long_model/long_expl_vde_forw.c
+MODEL_SRC+= long_model/long_expl_vde_adj.c
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
+
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
+OCP_SRC+= long_constraints/long_constr_h_fun_jac_uxt_zt.c
+OCP_SRC+= long_constraints/long_constr_h_fun.c
+OCP_SRC+= long_cost/long_cost_y_0_fun.c
+OCP_SRC+= long_cost/long_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_0_hess.c
+OCP_SRC+= long_cost/long_cost_y_fun.c
+OCP_SRC+= long_cost/long_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_hess.c
+OCP_SRC+= long_cost/long_cost_y_e_fun.c
+OCP_SRC+= long_cost/long_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= long_cost/long_cost_y_e_hess.c
+
+OCP_SRC+= acados_solver_long.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
+
+# for sim solver
+SIM_SRC= acados_sim_solver_long.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
+
+# for target example
+EX_SRC= main_long.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
+
+# for target example_sim
+EX_SIM_SRC= main_sim_long.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
+
+# combine model, sim and ocp object files
+OBJ=
+OBJ+= $(MODEL_OBJ)
+OBJ+= $(SIM_OBJ)
+OBJ+= $(OCP_OBJ)
+
+EXTERNAL_DIR=
+EXTERNAL_LIB=
+
+INCLUDE_PATH = /data/openpilot/third_party/acados/include
+LIB_PATH = /data/openpilot/third_party/acados/lib
+
+# preprocessor flags for make's implicit rules
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+
+
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99   -O2#-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
+
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
+
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= 
+
+# libraries
+LIBACADOS_SOLVER=libacados_solver_long.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_long.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+
+# virtual targets
+.PHONY : all clean
+
+#all: clean example_sim example shared_lib
+
+all: clean example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
+
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
+
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
+
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
+
+
+# Cython targets
+ocp_cython_c: ocp_shared_lib
+	cython \
+	-o acados_ocp_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+
+ocp_cython_o: ocp_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_ocp_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
+	acados_ocp_solver_pyx.c \
+
+ocp_cython: ocp_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_ocp_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_ocp_solver_pyx.o \
+	$(abspath .)/libacados_ocp_solver_long.so \
+	$(LDFLAGS) $(LDLIBS)
+
+# Sim Cython targets
+sim_cython_c: sim_shared_lib
+	cython \
+	-o acados_sim_solver_pyx.c \
+	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
+	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
+	-I /data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code \
+
+sim_cython_o: sim_cython_c
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
+	-o acados_sim_solver_pyx.o \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I /data/openpilot/.venv/lib/python3.12/site-packages/numpy/_core/include \
+	-I /usr/include/python3.12 \
+	acados_sim_solver_pyx.c \
+
+sim_cython: sim_cython_o
+	$(CC) $(ACADOS_FLAGS) -shared \
+	-o acados_sim_solver_pyx.so \
+	-Wl,-rpath=$(LIB_PATH) \
+	acados_sim_solver_pyx.o \
+	$(abspath .)/libacados_sim_solver_long.so \
+	$(LDFLAGS) $(LDLIBS)
+
+clean:
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
+
+clean_ocp_shared_lib:
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_long.so
+	$(RM) acados_solver_long.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
+
+clean_sim_cython:
+	$(RM) libacados_sim_solver_long.so
+	$(RM) acados_sim_solver_long.o
+	$(RM) acados_sim_solver_pyx.so
+	$(RM) acados_sim_solver_pyx.o

selfdrive/controls/lib/longitudinal_mpc_lib/gen.log

@@ -0,0 +1,3 @@
+Warning: Please note that the following versions of CasADi  are officially supported: 3.5.6 or 3.5.5 or 3.5.4 or 3.5.3 or 3.5.2 or 3.5.1 or 3.4.5 or 3.4.0.
+ If there is an incompatibility with the CasADi generated code, please consider changing your CasADi version.
+Version 3.7.2 currently in use.

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,429 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from cereal import log
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
+from openpilot.selfdrive.modeld.constants import index_function, ModelConstants
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU  # legacy lead extrapolation (newLeadMpc=False)
+from openpilot.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 openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from openpilot.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]
+    if model_lead.prob > 0.5 and radar_lead.status:
+      # Anchor at radar's trusted h=0, use model's delta for h>0. On radarless, radarState
+      # is synthesized from the model (radard.get_RadarState_from_vision), so this collapses
+      # to `x - RADAR_TO_CAMERA` and `v_ego + (model.v - model_v_ego)` — identical to the
+      # prior formula. On radar cars, real radar measurements anchor the trajectory.
+      x_lead_traj = float(radar_lead.dRel) + (np.asarray(model_lead.x, dtype=np.float64) - model_lead.x[0])
+      v_lead_traj = float(radar_lead.vLead) + (np.asarray(model_lead.v, dtype=np.float64) - model_lead.v[0])
+    else:
+      # Fake a fast lead so MPC stays in the same mode.
+      x_lead_traj = 50.0 + (v_ego + 10.0) * LEAD_T_IDXS_MODEL
+      v_lead_traj = np.full_like(LEAD_T_IDXS_MODEL, v_ego + 10.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:
+      # PR #37824: use the model's full predicted lead horizon
+      self.status = model_leads[0].prob > 0.5 or model_leads[1].prob > 0.5
+      lead_xv_0 = self.process_lead(model_leads[0], radarstate.leadOne)
+      lead_xv_1 = self.process_lead(model_leads[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 = model_leads[0].prob if self.new_lead_mpc else 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)

selfdrive/controls/lib/longitudinal_planner.py

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