IQ.Pilot Prebuilt Release @ ab07000

selfdrive/controls/.gitignore

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

selfdrive/controls/controlsd.py

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

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)

selfdrive/controls/plannerd.py

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

selfdrive/controls/radard.py

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

selfdrive/controls/tests/test_drive_helpers.py

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

selfdrive/controls/tests/test_following_distance.py

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

selfdrive/controls/tests/test_latcontrol.py

@@ -0,0 +1,55 @@
+from parameterized import parameterized
+
+from cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.honda.values import CAR as HONDA
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.nissan.values import CAR as NISSAN
+from iqdbc.car.gm.values import CAR as GM
+from iqdbc.car.vehicle_model import VehicleModel
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from openpilot.selfdrive.locationd.helpers import Pose
+from openpilot.common.mock.generators import generate_livePose
+from openpilot.iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+
+
+class TestLatControl:
+
+  @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),
+                         (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_EUV, LatControlTorque)])
+  def test_saturation(self, car_name, controller):
+    CarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CP_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
+    CI = CarInterface(CP, CP_IQ)
+    iqpilot_interfaces.apply_iq_car_config(CI)
+    CP_IQ = convert_to_capnp(CP_IQ)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP.as_reader(), CP_IQ.as_reader(), CI, DT_CTRL)
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.LiveParametersData.new_message()
+
+    lp = generate_livePose()
+    pose = Pose.from_live_pose(lp.livePose)
+
+    # Saturate for curvature limited and controller limited
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated

selfdrive/controls/tests/test_latcontrol_torque_buffer.py

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

selfdrive/controls/tests/test_lateral_mpc.py

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

selfdrive/controls/tests/test_leads.py

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

selfdrive/controls/tests/test_longcontrol.py

@@ -0,0 +1,43 @@
+from cereal import custom
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+
+
+class TestLongControlStateTransition:
+
+  def test_stay_stopped(self):
+    CP_IQ = custom.IQCarParams.new_message()
+    active = True
+    current_state = LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.pid
+    active = False
+    next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.off
+
+def test_engage():
+  CP_IQ = custom.IQCarParams.new_message()
+  active = True
+  current_state = LongCtrlState.off
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP_IQ, active, current_state,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -0,0 +1,54 @@
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
+from openpilot.selfdrive.controls.lib.longitudinal_planner import get_accel_candidates, get_e2e_accel
+
+
+def model_velocity(v_ego, v_future):
+  return np.interp(T_IDXS, [T_IDXS[0], T_IDXS[-1]], [v_ego, v_future])
+
+
+class TestE2eCruiseConvergence:
+  def test_converges_when_model_wants_to_accelerate(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), 0.1, False) == pytest.approx(0.5)
+
+  def test_scales_down_near_cruise_speed(self):
+    assert get_e2e_accel(28.5, 30.0, model_velocity(28.5, 30.0), 0.0, False) == pytest.approx(0.05)
+
+  def test_preserves_active_model_deceleration(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 25.0), -0.05, False) == pytest.approx(-0.05)
+
+  def test_preserves_future_model_slowdown(self):
+    assert get_e2e_accel(20.0, 30.0, model_velocity(20.0, 18.0), 0.1, False) == pytest.approx(0.1)
+
+  @pytest.mark.parametrize("v_ego, v_cruise, should_stop", [
+    (30.0, 30.0, False),
+    (31.0, 30.0, False),
+    (20.0, 30.0, True),
+  ])
+  def test_never_overrides_cruise_or_stop(self, v_ego, v_cruise, should_stop):
+    assert get_e2e_accel(v_ego, v_cruise, model_velocity(v_ego, v_ego + 5.0), -0.2, should_stop) == pytest.approx(-0.2)
+
+
+class TestAccelCandidates:
+  MPC = (-0.2, LongitudinalPlanSource.lead0, True)
+  CRUISE = (0.5, LongitudinalPlanSource.cruise, False)
+  E2E = (0.1, LongitudinalPlanSource.e2e, False)
+
+  def test_e2e_without_lead_frees_model_from_mpc(self):
+    candidates = get_accel_candidates(True, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.e2e
+    assert not any(should_stop for _, _, should_stop in candidates)
+
+  def test_e2e_with_lead_keeps_mpc_safety_constraint(self):
+    candidates = get_accel_candidates(True, True, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE, self.E2E]
+    assert min(candidates, key=lambda c: c[0])[1] == LongitudinalPlanSource.lead0
+    assert any(should_stop for _, _, should_stop in candidates)
+
+  def test_acc_without_lead_keeps_mpc_policy(self):
+    candidates = get_accel_candidates(False, False, self.MPC, self.CRUISE, self.E2E)
+    assert candidates == [self.MPC, self.CRUISE]

selfdrive/controls/tests/test_torqued_lat_accel_offset.py

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