Release 260111

selfdrive/controls/beep.py

@@ -0,0 +1,139 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+from cereal import car, messaging
+from openpilot.common.realtime import Ratekeeper
+import threading
+
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+class Beepd:
+  def __init__(self):
+    self.current_alert = AudibleAlert.none
+    self.enable_gpio()
+    self.startup_beep()
+
+  def enable_gpio(self):
+    # 尝试 export，忽略已 export 的错误
+    try:
+      subprocess.run("echo 42 | sudo tee /sys/class/gpio/export",
+                     shell=True,
+                     stderr=subprocess.DEVNULL,
+                     stdout=subprocess.DEVNULL,
+                     encoding='utf8')
+    except Exception:
+      pass
+    subprocess.run("echo \"out\" | sudo tee /sys/class/gpio/gpio42/direction",
+                   shell=True,
+                   stderr=subprocess.DEVNULL,
+                   stdout=subprocess.DEVNULL,
+                   encoding='utf8')
+
+  def _beep(self, on):
+    val = "1" if on else "0"
+    subprocess.run(f"echo \"{val}\" | sudo tee /sys/class/gpio/gpio42/value",
+                   shell=True,
+                   stderr=subprocess.DEVNULL,
+                   stdout=subprocess.DEVNULL,
+                   encoding='utf8')
+
+  def engage(self):
+    self._beep(True)
+    time.sleep(0.05)
+    self._beep(False)
+
+  def disengage(self):
+    for _ in range(2):
+      self._beep(True)
+      time.sleep(0.01)
+      self._beep(False)
+      time.sleep(0.01)
+
+  def warning(self):
+    for _ in range(3):
+      self._beep(True)
+      time.sleep(0.01)
+      self._beep(False)
+      time.sleep(0.01)
+
+  def startup_beep(self):
+    self._beep(True)
+    time.sleep(0.1)
+    self._beep(False)
+
+  def ding(self):
+    self._beep(True)
+    time.sleep(0.02)
+    self._beep(False)
+
+  def dong(self):
+    self._beep(True)
+    time.sleep(0.03)
+    self._beep(False)
+
+  def beep(self):
+    self._beep(True)
+    time.sleep(0.04)
+    self._beep(False)
+
+  def dispatch_beep(self, func):
+    threading.Thread(target=func, daemon=True).start()
+
+  def update_alert(self, new_alert):
+    if new_alert != self.current_alert:
+      self.current_alert = new_alert
+      print(f"[BEEP] New alert: {new_alert}")
+      if new_alert == AudibleAlert.engage:
+        self.dispatch_beep(self.engage)
+      elif new_alert == AudibleAlert.disengage:
+        self.dispatch_beep(self.disengage)
+      elif new_alert in [AudibleAlert.refuse, AudibleAlert.prompt, AudibleAlert.warningImmediate,AudibleAlert.warningSoft]:
+        self.dispatch_beep(self.warning)
+      elif new_alert in [AudibleAlert.longEngaged, AudibleAlert.longDisengaged, AudibleAlert.trafficSignGreen, AudibleAlert.trafficSignChanged, AudibleAlert.trafficError, AudibleAlert.bsdWarning, AudibleAlert.laneChange]:
+        self.dispatch_beep(self.ding)
+      elif new_alert in [AudibleAlert.stopStop, AudibleAlert.stopping, AudibleAlert.autoHold, AudibleAlert.engage2, AudibleAlert.disengage2, AudibleAlert.speedDown, AudibleAlert.audioTurn, AudibleAlert.reverseGear]:
+        self.dispatch_beep(self.dong)
+      elif new_alert in [AudibleAlert.audio1, AudibleAlert.audio2, AudibleAlert.audio3, AudibleAlert.audio4, AudibleAlert.audio5,
+                         AudibleAlert.audio6, AudibleAlert.audio7, AudibleAlert.audio8, AudibleAlert.audio9, AudibleAlert.audio10]:
+        self.dispatch_beep(self.beep)
+
+  def get_audible_alert(self, sm):
+    if sm.updated['selfdriveState']:
+      new_alert = sm['selfdriveState'].alertSound.raw
+      self.update_alert(new_alert)
+
+  def test_beepd_thread(self):
+    frame = 0
+    rk = Ratekeeper(20)
+    pm = messaging.PubMaster(['selfdriveState'])
+    while True:
+      cs = messaging.new_message('selfdriveState')
+      if frame == 40:
+        cs.selfdriveState.alertSound = AudibleAlert.engage
+      if frame == 60:
+        cs.selfdriveState.alertSound = AudibleAlert.disengage
+      if frame == 80:
+        cs.selfdriveState.alertSound = AudibleAlert.prompt
+
+      pm.send("selfdriveState", cs)
+      frame += 1
+      rk.keep_time()
+
+  def beepd_thread(self, test=False):
+    if test:
+      threading.Thread(target=self.test_beepd_thread, daemon=True).start()
+
+    sm = messaging.SubMaster(['selfdriveState'])
+    rk = Ratekeeper(20)
+
+    while True:
+      sm.update(0)
+      self.get_audible_alert(sm)
+      rk.keep_time()
+
+def main():
+  s = Beepd()
+  s.beepd_thread(test=False)  # 改成 True 可启用模拟测试数据
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/controlsd.py

@@ -0,0 +1,356 @@
+#!/usr/bin/env python3
+import math
+from typing import SupportsFloat
+
+from cereal import car, log
+import cereal.messaging as messaging
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
+from openpilot.common.swaglog import cloudlog
+import numpy as np
+from collections import deque
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.vehicle_model import VehicleModel
+
+from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature, get_lag_adjusted_curvature
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
+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.longcontrol import LongControl
+
+
+from openpilot.common.realtime import DT_CTRL, DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+
+State = log.SelfdriveState.OpenpilotState
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+
+ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
+
+
+class Controls:
+  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")
+
+    self.CI = interfaces[self.CP.carFingerprint](self.CP)
+
+    self.disable_dm = False
+
+    self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
+                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
+                                   'carrotMan', 'lateralPlan', 'radarState',
+                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
+    self.pm = messaging.PubMaster(['carControl', 'controlsState'])
+
+    self.steer_limited_by_controls = False
+    self.curvature = 0.0
+    self.desired_curvature = 0.0
+
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None
+
+    self.side_state = {
+        "left":  {"main": {"dRel": None, "lat": None}, "sub": {"dRel": None, "lat": None}},
+        "right": {"main": {"dRel": None, "lat": None}, "sub": {"dRel": None, "lat": None}},
+    }
+
+    self.LoC = LongControl(self.CP)
+    self.VM = VehicleModel(self.CP)
+    self.LaC: LatControl
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+      self.LaC = LatControlAngle(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'pid':
+      self.LaC = LatControlPID(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CI)
+
+  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)
+
+  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.params.get_float("SteerRatioRate") * 0.01
+    custom_sr = self.params.get_float("CustomSR") * 0.1
+    sr = max(custom_sr if custom_sr > 1.0 else sr, 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)
+
+    # Update Torque Params
+    if self.CP.lateralTuning.which() == 'torque':
+      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)
+
+    long_plan = self.sm['longitudinalPlan']
+    model_v2 = self.sm['modelV2']
+
+    CC = car.CarControl.new_message()
+    CC.enabled = self.sm['selfdriveState'].enabled
+
+    # carrot
+    gear = car.CarState.GearShifter
+    driving_gear = CS.gearShifter not in (gear.neutral, gear.park, gear.reverse, gear.unknown)
+    alkas = self.params.get_int("AlwaysOnLKAS") != 0
+    lateral_enabled = driving_gear and alkas
+    #self.soft_hold_active = CS.softHoldActive #car.OnroadEvent.EventName.softHold in [e.name for e in self.sm['onroadEvents']]
+
+    # Check which actuators can be enabled
+    standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
+    CC.latActive = ((self.sm['selfdriveState'].active or lateral_enabled) and CS.latEnabled and
+                    not CS.steerFaultTemporary and not CS.steerFaultPermanent and not standstill)
+    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
+
+    actuators = CC.actuators
+    actuators.longControlState = self.LoC.long_control_state
+
+    # Enable blinkers while lane changing
+    if model_v2.meta.laneChangeState != LaneChangeState.off:
+      CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
+      CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
+
+    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, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
+    t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
+    accel, aTarget, jerk = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan, self.sm['radarState'])
+    actuators.accel = float(accel)
+    actuators.aTarget = float(aTarget)
+    actuators.jerk = float(jerk)
+
+    # Steering PID loop and lateral MPC
+    lat_plan = self.sm['lateralPlan']
+    curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed)
+    self.lanefull_mode_enabled = (lat_plan.useLaneLines and curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
+    lat_smooth_seconds = self.params.get_float("LatSmoothSec") * 0.01
+    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
+    if steer_actuator_delay == 0.0:
+      steer_actuator_delay = self.sm['liveDelay'].lateralDelay
+
+    if not CC.latActive:
+      new_desired_curvature = self.curvature
+    elif self.lanefull_mode_enabled:
+      if len(lat_plan.curvatures) == 0:
+        new_desired_curvature = self.curvature
+      else:
+        def smooth_value(val, prev_val, tau):
+          alpha = 1 - np.exp(-DT_CTRL / tau) if tau > 0 else 1
+          return alpha * val + (1 - alpha) * prev_val
+
+        curvature = get_lag_adjusted_curvature(self.CP, CS.vEgo, lat_plan.psis, lat_plan.curvatures, steer_actuator_delay + lat_smooth_seconds, lat_plan.distances)
+
+        new_desired_curvature = smooth_value(curvature, self.desired_curvature, lat_smooth_seconds)
+    else:
+      new_desired_curvature = model_v2.action.desiredCurvature
+
+    self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
+
+    actuators.curvature = float(self.desired_curvature)
+    steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
+                                                       self.steer_limited_by_controls, self.desired_curvature,
+                                                       CC, curvature_limited,
+                                                       model_data=self.sm['modelV2'])
+    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, SupportsFloat):
+        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 _update_side(self, side: str, leads2, road_edge, bsd_state, hudControl):
+      def ema(prev, curr, a=0.02):
+        return curr if prev is None else prev * (1 - a) + curr * a
+
+      def set_hud(side_cap, name, val):
+        setattr(hudControl, f"lead{side_cap}{name}", float(val if val is not None else 0.0))
+
+      st = self.side_state[side]
+      if road_edge <= 2.0 or not leads2:
+        st["main"] = {"dRel": None, "lat": None}
+        st["sub"]  = {"dRel": None, "lat": None}
+        if not bsd_state:
+          return
+
+      lead_main = leads2[0] if len(leads2) > 0 else None
+      side_cap = side.capitalize()
+
+      if bsd_state:
+        set_hud(side_cap, "Dist2", 1)
+        set_hud(side_cap, "Lat2",  3.2)
+      # 첫 번째가 10m 이내라면 sub 업데이트 + 두 번째를 main으로
+      elif len(leads2) > 1 and lead_main.dRel < 10:
+        st["sub"]["dRel"] = ema(st["sub"]["dRel"], lead_main.dRel)
+        st["sub"]["lat"]  = ema(st["sub"]["lat"],  abs(lead_main.dPath))
+        set_hud(side_cap, "Dist2", st["sub"]["dRel"])
+        set_hud(side_cap, "Lat2",  st["sub"]["lat"])
+        lead_main = leads2[1]
+
+      if len(leads2) > 0:
+        st["main"]["dRel"] = ema(st["main"]["dRel"], lead_main.dRel)
+        st["main"]["lat"]  = ema(st["main"]["lat"],  abs(lead_main.dPath))
+        set_hud(side_cap, "Dist", st["main"]["dRel"])
+        set_hud(side_cap, "Lat",  st["main"]["lat"])
+
+  def publish(self, CC, lac_log):
+    CS = self.sm['carState']
+
+    # Orientation and angle rates can be useful for carcontroller
+    # Only calibrated (car) frame is relevant for the carcontroller
+    if self.calibrated_pose is not None:
+      CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+      CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
+
+    #acceleration_value = list(self.sm['liveLocationKalman'].accelerationCalibrated.value)
+    #if len(acceleration_value) > 2:
+    #  if abs(acceleration_value[0]) > 16.0:
+    #    print("Collision detected. disable openpilot, restart")
+    #    self.params.put_bool("OpenpilotEnabledToggle", False)
+    #    self.params.put_int("SoftRestartTriggered", 1)
+
+    CC.cruiseControl.override = CC.enabled and not CC.longActive and self.CP.openpilotLongitudinalControl
+    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
+
+    desired_kph = min(CS.vCruiseCluster, self.sm['carrotMan'].desiredSpeed)
+    setSpeed = float(desired_kph * CV.KPH_TO_MS)
+    speeds = self.sm['longitudinalPlan'].speeds
+    if len(speeds):
+      CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and speeds[-1] > 0.1
+      vCluRatio = CS.vCluRatio if CS.vCluRatio > 0.5 else 1.0
+      setSpeed = speeds[-1] / vCluRatio
+
+    hudControl = CC.hudControl
+
+    hudControl.activeCarrot = self.sm['carrotMan'].activeCarrot
+    hudControl.atcDistance = self.sm['carrotMan'].xDistToTurn
+
+    lp = self.sm['longitudinalPlan']
+    if self.CP.pcmCruise:
+      speed_from_pcm = self.params.get_int("SpeedFromPCM")
+      if speed_from_pcm == 1: #toyota
+        hudControl.setSpeed = float(CS.vCruiseCluster * CV.KPH_TO_MS)
+      elif speed_from_pcm == 2:
+        hudControl.setSpeed = float(max(30/3.6, desired_kph * CV.KPH_TO_MS))
+      elif speed_from_pcm == 3: # honda
+        hudControl.setSpeed = setSpeed if lp.xState == 3 else float(desired_kph * CV.KPH_TO_MS)
+      else:
+        hudControl.setSpeed = float(max(30/3.6, setSpeed))
+    else:
+      hudControl.setSpeed = setSpeed if lp.xState == 3 else float(desired_kph * CV.KPH_TO_MS)
+    hudControl.speedVisible = CC.enabled
+    hudControl.lanesVisible = CC.enabled
+    hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
+    hudControl.leadDistanceBars = self.sm['selfdriveState'].personality.raw + 1
+    hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual
+
+    radarState = self.sm['radarState']
+    leadOne = radarState.leadOne
+    hudControl.leadDistance = leadOne.dRel if leadOne.status else 0
+    hudControl.leadRelSpeed = leadOne.vRel if leadOne.status else 0
+    hudControl.leadRadar = 1 if leadOne.radar else 0
+    hudControl.leadDPath = leadOne.dPath
+
+    meta = self.sm['modelV2'].meta
+    hudControl.modelDesire = 1 if meta.desire == log.Desire.turnLeft else 2 if meta.desire == log.Desire.turnRight else 0
+
+    self._update_side("left",  radarState.leadsLeft2,  meta.distanceToRoadEdgeLeft,  CS.leftBlindspot, hudControl)
+    self._update_side("right", radarState.leadsRight2, meta.distanceToRoadEdgeRight, CS.rightBlindspot, hudControl)
+
+    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 == car.CarParams.SteerControlType.angle:
+        self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
+                                              STEER_ANGLE_SATURATION_THRESHOLD
+      else:
+        self.steer_limited_by_controls = 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['selfdriveState'].state == State.softDisabling)
+
+    lat_tuning = self.CP.lateralTuning.which()
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
+      cs.lateralControlState.angleState = lac_log
+    elif lat_tuning == 'pid':
+      cs.lateralControlState.pidState = lac_log
+    elif lat_tuning == 'torque':
+      cs.lateralControlState.torqueState = lac_log
+
+    cs.activeLaneLine = self.lanefull_mode_enabled
+    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 run(self):
+    rk = Ratekeeper(100, print_delay_threshold=None)
+    while True:
+      self.update()
+      CC, lac_log = self.state_control()
+      self.publish(CC, lac_log)
+      rk.monitor_time()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  controls = Controls()
+  controls.run()
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/lib/desire_helper.py

@@ -0,0 +1,662 @@
+from cereal import log
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.realtime import DT_MDL
+import numpy as np
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.common.params import Params
+from collections import deque
+
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = log.Desire
+
+LANE_CHANGE_SPEED_MIN = 30 * CV.KPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.
+
+BLINKER_NONE = 0
+BLINKER_LEFT = 1
+BLINKER_RIGHT = 2
+BLINKER_BOTH = 3
+
+DESIRES = {
+  LaneChangeDirection.none: {
+    LaneChangeState.off: log.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
+    LaneChangeState.laneChangeStarting: log.Desire.none,
+    LaneChangeState.laneChangeFinishing: log.Desire.none,
+  },
+  LaneChangeDirection.left: {
+    LaneChangeState.off: log.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
+    LaneChangeState.laneChangeStarting: log.Desire.laneChangeLeft,
+    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeLeft,
+  },
+  LaneChangeDirection.right: {
+    LaneChangeState.off: log.Desire.none,
+    LaneChangeState.preLaneChange: log.Desire.none,
+    LaneChangeState.laneChangeStarting: log.Desire.laneChangeRight,
+    LaneChangeState.laneChangeFinishing: log.Desire.laneChangeRight,
+  },
+}
+
+TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
+
+
+def calculate_lane_width_frog(lane, current_lane, road_edge):
+  lane_x, lane_y = np.array(lane.x), np.array(lane.y)
+  edge_x, edge_y = np.array(road_edge.x), np.array(road_edge.y)
+  current_x, current_y = np.array(current_lane.x), np.array(current_lane.y)
+
+  lane_y_interp = np.interp(current_x, lane_x[lane_x.argsort()], lane_y[lane_x.argsort()])
+  road_edge_y_interp = np.interp(current_x, edge_x[edge_x.argsort()], edge_y[edge_x.argsort()])
+
+  distance_to_lane = np.mean(np.abs(current_y - lane_y_interp))
+  distance_to_road_edge = np.mean(np.abs(current_y - road_edge_y_interp))
+
+  return min(distance_to_lane, distance_to_road_edge), distance_to_road_edge
+
+
+def calculate_lane_width(lane, lane_prob, current_lane, road_edge):
+  # t ≈ 1초 앞 기준으로 차선/도로 가장자리까지의 거리 계산
+  t = 1.0
+  current_lane_y = np.interp(t, ModelConstants.T_IDXS, current_lane.y)
+  lane_y = np.interp(t, ModelConstants.T_IDXS, lane.y)
+  distance_to_lane = abs(current_lane_y - lane_y)
+
+  road_edge_y = np.interp(t, ModelConstants.T_IDXS, road_edge.y)
+  distance_to_road_edge = abs(current_lane_y - road_edge_y)
+  distance_to_road_edge_far = abs(current_lane_y - np.interp(2.0, ModelConstants.T_IDXS, road_edge.y))
+
+  lane_valid = lane_prob > 0.5
+  return min(distance_to_lane, distance_to_road_edge), distance_to_road_edge, distance_to_road_edge_far, lane_valid
+
+
+class ExistCounter:
+  """
+  존재 여부가 노이즈처럼 튀는 신호를 히스테리시스로 안정화해 주는 카운터.
+  - true가 일정 시간 이상 지속되면 counter를 양수로 증가
+  - false가 일정 시간 이상 지속되면 counter를 음수로 감소
+  """
+  def __init__(self):
+    self.counter = 0
+    self.true_count = 0
+    self.false_count = 0
+    self.threshold = int(0.2 / DT_MDL)  # 약 0.2초 이상 지속 시 유효로 판단
+
+  def update(self, exist_flag: bool):
+    if exist_flag:
+      self.true_count += 1
+      self.false_count = 0
+      if self.true_count >= self.threshold:
+        self.counter = max(self.counter + 1, 1)
+    else:
+      self.false_count += 1
+      self.true_count = 0
+      if self.false_count >= self.threshold:
+        self.counter = min(self.counter - 1, -1)
+    return self.true_count
+
+
+class DesireHelper:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+
+    # Lane change / turn 상태
+    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.lane_change_delay = 0.0
+    self.maneuver_type = "none"  # "none" / "turn" / "lane_change"
+
+    # Desire / turn 관련
+    self.desire = log.Desire.none
+    self.turn_direction = TurnDirection.none
+    self.enable_turn_desires = True
+    self.turn_desire_state = False
+    self.desire_disable_count = 0   # turn 후 일정 시간 동안 차선변경 금지
+    self.turn_disable_count = 0     # steeringAngle 매우 클 때 turn 금지
+
+    # Lane / road edge 상태
+    self.lane_width_left = 0.0
+    self.lane_width_right = 0.0
+    self.lane_width_left_diff = 0.0
+    self.lane_width_right_diff = 0.0
+    self.distance_to_road_edge_left = 0.0
+    self.distance_to_road_edge_right = 0.0
+    self.distance_to_road_edge_left_far = 0.0
+    self.distance_to_road_edge_right_far = 0.0
+
+    self.lane_exist_left_count = ExistCounter()
+    self.lane_exist_right_count = ExistCounter()
+    self.lane_width_left_count = ExistCounter()
+    self.lane_width_right_count = ExistCounter()
+    self.road_edge_left_count = ExistCounter()
+    self.road_edge_right_count = ExistCounter()
+
+    self.available_left_lane = False
+    self.available_right_lane = False
+    self.available_left_edge = False
+    self.available_right_edge = False
+
+    self.lane_width_left_queue = deque(maxlen=int(1.0 / DT_MDL))
+    self.lane_width_right_queue = deque(maxlen=int(1.0 / DT_MDL))
+
+    self.lane_available_last = False
+    self.edge_available_last = False
+    self.lane_available_trigger = False
+    self.lane_appeared = False
+    self.lane_line_info = 0
+
+    # Blinkers / ATC
+    self.blinker_ignore = False
+    self.driver_blinker_state = BLINKER_NONE
+    self.carrot_blinker_state = BLINKER_NONE
+    self.carrot_lane_change_count = 0
+    self.carrot_cmd_index_last = 0
+    self.atc_type = ""
+    self.atc_active = 0  # 0: 없음, 1: ATC 동작 중, 2: 운전자와 ATC 상충
+
+    # Auto lane change 관련
+    self.auto_lane_change_enable = False
+    self.next_lane_change = False
+    self.blindspot_detected_counter = 0
+
+    # Keep pulse
+    self.keep_pulse_timer = 0.0
+
+    # 파라미터
+    self.laneChangeNeedTorque = 0
+    self.laneChangeBsd = 0
+    self.laneChangeDelay = 0.0
+    self.modelTurnSpeedFactor = 0.0
+    self.model_turn_speed = 0.0
+
+    # 기타
+    self.prev_desire_enabled = False
+    self.desireLog = ""
+    self.object_detected_count = 0
+
+    # ★ 현재 차선 확률 (Ego 기준 좌/우)
+    self.cur_left_prob = 1.0   # laneLineProbs[1]
+    self.cur_right_prob = 1.0  # laneLineProbs[2]
+    self.current_lane_missing = False
+  # ─────────────────────────────────────────────
+  #  Config / Model 관련
+  # ─────────────────────────────────────────────
+
+  def _update_params_periodic(self):
+    if self.frame % 100 == 0:
+      self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque")
+      self.laneChangeBsd = self.params.get_int("LaneChangeBsd")
+      self.laneChangeDelay = self.params.get_float("LaneChangeDelay") * 0.1
+      self.modelTurnSpeedFactor = self.params.get_float("ModelTurnSpeedFactor") * 0.1
+
+  def _make_model_turn_speed(self, modeldata):
+    if self.modelTurnSpeedFactor > 0:
+      model_turn_speed = np.interp(self.modelTurnSpeedFactor,
+                                   modeldata.velocity.t,
+                                   modeldata.velocity.x) * CV.MS_TO_KPH * 1.2
+      self.model_turn_speed = self.model_turn_speed * 0.9 + model_turn_speed * 0.1
+    else:
+      self.model_turn_speed = 200.0
+
+  # ─────────────────────────────────────────────
+  #  Lane / Edge 상태 계산
+  # ─────────────────────────────────────────────
+
+  def _check_lane_state(self, modeldata):
+    # 왼쪽: laneLines[0] vs 현재차선 laneLines[1], roadEdges[0]
+    lane_width_left, self.distance_to_road_edge_left, self.distance_to_road_edge_left_far, lane_prob_left = \
+      calculate_lane_width(modeldata.laneLines[0], modeldata.laneLineProbs[0],
+                           modeldata.laneLines[1], modeldata.roadEdges[0])
+
+    # 오른쪽: laneLines[3] vs 현재차선 laneLines[2], roadEdges[1]
+    lane_width_right, self.distance_to_road_edge_right, self.distance_to_road_edge_right_far, lane_prob_right = \
+      calculate_lane_width(modeldata.laneLines[3], modeldata.laneLineProbs[3],
+                           modeldata.laneLines[2], modeldata.roadEdges[1])
+
+    # 차선 존재 카운터 업데이트
+    self.lane_exist_left_count.update(lane_prob_left)
+    self.lane_exist_right_count.update(lane_prob_right)
+
+    # 1초 이동 평균 (노이즈 줄이기)
+    self.lane_width_left_queue.append(lane_width_left)
+    self.lane_width_right_queue.append(lane_width_right)
+
+    self.lane_width_left = float(np.mean(self.lane_width_left_queue))
+    self.lane_width_right = float(np.mean(self.lane_width_right_queue))
+
+    self.lane_width_left_diff = self.lane_width_left_queue[-1] - self.lane_width_left_queue[0]
+    self.lane_width_right_diff = self.lane_width_right_queue[-1] - self.lane_width_right_queue[0]
+
+    # 유효 차선/엣지 판단
+    min_lane_width = 2.5
+    self.lane_width_left_count.update(self.lane_width_left > min_lane_width)
+    self.lane_width_right_count.update(self.lane_width_right > min_lane_width)
+    self.road_edge_left_count.update(self.distance_to_road_edge_left > min_lane_width)
+    self.road_edge_right_count.update(self.distance_to_road_edge_right > min_lane_width)
+
+    available_count = int(0.2 / DT_MDL)
+    self.available_left_lane = self.lane_width_left_count.counter > available_count
+    self.available_right_lane = self.lane_width_right_count.counter > available_count
+    self.available_left_edge = self.road_edge_left_count.counter > available_count and self.distance_to_road_edge_left_far > min_lane_width
+    self.available_right_edge = self.road_edge_right_count.counter > available_count and self.distance_to_road_edge_right_far > min_lane_width
+
+    self.cur_left_prob = modeldata.laneLineProbs[1]
+    self.cur_right_prob = modeldata.laneLineProbs[2]
+
+  # ─────────────────────────────────────────────
+  #  모델 내 desire 상태 (turn 예측 등)
+  # ─────────────────────────────────────────────
+
+  def _check_desire_state(self, modeldata, carstate, maneuver_type):
+    desire_state = modeldata.meta.desireState
+    # turnLeft + turnRight 확률
+    self.turn_desire_state = (desire_state[1] + desire_state[2]) > 0.1
+
+    #if self.turn_desire_state:
+    #  self.desire_disable_count = int(2.0 / DT_MDL)
+    #else:
+    #  self.desire_disable_count = max(0, self.desire_disable_count - 1)
+
+    # steeringAngle 너무 크면 turn 자체를 일정 시간 막기
+    if abs(carstate.steeringAngleDeg) > 80:
+      self.turn_disable_count = int(10.0 / DT_MDL)
+    else:
+      self.turn_disable_count = max(0, self.turn_disable_count - 1)
+
+  # ─────────────────────────────────────────────
+  #  Blinkers / ATC 상태 업데이트
+  # ─────────────────────────────────────────────
+
+  def _update_driver_blinker(self, carstate):
+    driver_blinker_state = carstate.leftBlinker * 1 + carstate.rightBlinker * 2
+    driver_blinker_changed = driver_blinker_state != self.driver_blinker_state
+    self.driver_blinker_state = driver_blinker_state
+
+    driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT]
+    if self.laneChangeNeedTorque < 0:
+      # 운전자 깜빡이를 무시하고 차선변경 안 하는 설정
+      driver_desire_enabled = False
+
+    return driver_blinker_state, driver_blinker_changed, driver_desire_enabled
+
+  def _update_atc_blinker(self, carrotMan, v_ego, driver_blinker_state):
+    """
+    ATC에서 온 turn/lanechange 명령 기반 깜빡이 상태 갱신.
+    """
+    atc_type = carrotMan.atcType
+    atc_blinker_state = BLINKER_NONE
+
+    # ATC 기반 자동 차선변경 유지 시간
+    if self.carrot_lane_change_count > 0:
+      atc_blinker_state = self.carrot_blinker_state
+    elif carrotMan.carrotCmdIndex != self.carrot_cmd_index_last and carrotMan.carrotCmd == "LANECHANGE":
+      self.carrot_cmd_index_last = carrotMan.carrotCmdIndex
+      self.carrot_lane_change_count = int(0.2 / DT_MDL)
+      self.carrot_blinker_state = BLINKER_LEFT if carrotMan.carrotArg == "LEFT" else BLINKER_RIGHT
+      atc_blinker_state = self.carrot_blinker_state
+    elif atc_type in ["turn left", "turn right"]:
+      # 네비 turn 안내: 속도 조건을 턴 쪽으로 강제
+      if self.atc_active != 2:
+        atc_blinker_state = BLINKER_LEFT if atc_type == "turn left" else BLINKER_RIGHT
+        self.atc_active = 1
+        self.blinker_ignore = False
+    elif atc_type in ["fork left", "fork right", "atc left", "atc right"]:
+      # 분기(lanechange에 가까움)
+      if self.atc_active != 2:
+        atc_blinker_state = BLINKER_LEFT if atc_type in ["fork left", "atc left"] else BLINKER_RIGHT
+        self.atc_active = 1
+    else:
+      self.atc_active = 0
+
+    # 운전자 깜빡이와 ATC 깜빡이가 충돌할 경우 ATC 무효화
+    if driver_blinker_state != BLINKER_NONE and atc_blinker_state != BLINKER_NONE and driver_blinker_state != atc_blinker_state:
+      atc_blinker_state = BLINKER_NONE
+      self.atc_active = 2
+
+    atc_desire_enabled = atc_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT]
+
+    # blinker_ignore일 때는 깜빡이 신호를 잠시 무시
+    if driver_blinker_state == BLINKER_NONE:
+      self.blinker_ignore = False
+    if self.blinker_ignore:
+      driver_blinker_state = BLINKER_NONE
+      atc_blinker_state = BLINKER_NONE
+      atc_desire_enabled = False
+
+    # ATC 타입이 바뀌었으면 이번 프레임은 무시 (안정화 목적)
+    if self.atc_type != atc_type:
+      atc_desire_enabled = False
+    self.atc_type = atc_type
+
+    return atc_blinker_state, atc_desire_enabled
+
+  # ─────────────────────────────────────────────
+  #  Turn / LaneChange 모드 분류
+  # ─────────────────────────────────────────────
+
+  def _classify_maneuver_type(self, blinker_state, carstate, old_type):
+    """
+    깜빡이가 들어왔을 때 이번 조작이 turn인지 lane_change인지 분류.
+    - 너무 복잡하게 가지 않고, 현재 속도/감속/차선상태/모델 turn 상태 기준으로 점수화.
+    """
+    if blinker_state == BLINKER_NONE:
+      return "none"
+
+    v_kph = carstate.vEgo * CV.MS_TO_KPH
+    accel = carstate.aEgo
+
+    # 깜빡이 방향에 따라 참조할 lane/edge 선택
+    if blinker_state == BLINKER_LEFT:
+      lane_exist_counter = self.lane_exist_left_count.counter
+      lane_available = self.available_left_lane
+      edge_available = self.available_left_edge
+      lane_prob_side = self.cur_left_prob
+      edge_dist = self.distance_to_road_edge_left_far
+    else:
+      lane_exist_counter = self.lane_exist_right_count.counter
+      lane_available = self.available_right_lane
+      edge_available = self.available_right_edge
+      lane_prob_side = self.cur_right_prob
+      edge_dist = self.distance_to_road_edge_right_far
+
+    score_turn = 0
+
+    if v_kph < 30.0:
+      score_turn += 1
+    elif v_kph < 40.0 and accel < -1.0:
+      score_turn += 1
+
+    # 차로가 없고, 로드에지도 여유없고..
+    if v_kph < 40.0 and not lane_available and not edge_available:
+      score_turn += 1
+
+    # 차선이 잘 안 보이거나(교차로/삼거리 등)
+    if v_kph < 40.0 and lane_exist_counter < int(0.5 / DT_MDL):
+      score_turn += 1
+
+    # steeringAngle이 크면 턴에 가깝다고 본다
+    #if abs(carstate.steeringAngleDeg) > 45.0:
+    #  score_turn += 1
+
+    # 모델이 이미 turn을 예측 중이면 가중치
+    if self.turn_desire_state:
+      score_turn += 1
+
+    # ATC가 turn 안내 중이면 가중치
+    if self.atc_type in ["turn left", "turn right"]:
+      score_turn += 2
+    elif self.atc_type in ["fork left", "fork right", "atc left", "atc right"]:
+      score_turn -= 2  # fork/atc는 lanechange 쪽에 더 가깝게
+
+    # ★ road edge가 충분히 멀면(교차로/넓은 공간으로 판단) 턴 쪽으로 가산점
+    edge_far = edge_dist > 4.0  # 튜닝 포인트 (4~6m 정도가 무난)
+    #if edge_far:
+    #  score_turn += 1
+      
+    current_lane_missing = lane_prob_side < 0.3
+    self.current_lane_missing = current_lane_missing
+    # 튜닝 포인트: score_turn 임계값
+    if score_turn >= 2:
+      #if current_lane_missing and edge_far:
+      if edge_far:
+        return "turn"
+      else:
+        return old_type
+    else:
+      return "lane_change"
+
+  # ─────────────────────────────────────────────
+  #  메인 업데이트 루틴
+  # ─────────────────────────────────────────────
+
+  def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):
+    self.frame += 1
+    self._update_params_periodic()
+    self._make_model_turn_speed(modeldata)
+
+    # 카운터 감소
+    self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1)
+    self.lane_change_delay = max(0.0, self.lane_change_delay - DT_MDL)
+    self.blindspot_detected_counter = max(0, self.blindspot_detected_counter - 1)
+
+    v_ego = carstate.vEgo
+    below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
+
+    # Lane / desire 상태 갱신
+    self._check_lane_state(modeldata)
+    self._check_desire_state(modeldata, carstate, self.maneuver_type)
+
+    # 운전자 깜빡이
+    driver_blinker_state, driver_blinker_changed, driver_desire_enabled = self._update_driver_blinker(carstate)
+
+    # BSD 설정
+    ignore_bsd = (self.laneChangeBsd < 0)
+    block_lanechange_bsd = (self.laneChangeBsd == 1)
+
+    # ATC 깜빡이
+    atc_blinker_state, atc_desire_enabled = self._update_atc_blinker(carrotMan, v_ego, driver_blinker_state)
+
+    # 최종 깜빡이/Desire enabled 판단
+    desire_enabled = driver_desire_enabled or atc_desire_enabled
+    blinker_state = driver_blinker_state if driver_desire_enabled else atc_blinker_state
+
+    # lane_line_info (HUD용 등)
+    lane_line_info = carstate.leftLaneLine if blinker_state == BLINKER_LEFT else carstate.rightLaneLine
+
+    # BSD / 주변 차량 감지
+    if desire_enabled:
+      lane_exist_counter = self.lane_exist_left_count.counter if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter
+      lane_available = self.available_left_lane if blinker_state == BLINKER_LEFT else self.available_right_lane
+      edge_available = self.available_left_edge if blinker_state == BLINKER_LEFT else self.available_right_edge
+      self.lane_appeared = self.lane_appeared or lane_exist_counter == int(0.2 / DT_MDL)
+
+      radar = radarState.leadLeft if blinker_state == BLINKER_LEFT else radarState.leadRight
+      side_object_dist = radar.dRel + radar.vLead * 4.0 if radar.status else 255
+      object_detected = side_object_dist < v_ego * 3.0
+      if object_detected:
+        self.object_detected_count = max(1, self.object_detected_count + 1)
+      else:
+        self.object_detected_count = min(-1, self.object_detected_count - 1)
+
+      lane_line_info_edge_detect = (lane_line_info % 10 in [0, 5] and self.lane_line_info not in [0, 5])
+      self.lane_line_info = lane_line_info % 10
+    else:
+      lane_exist_counter = 0
+      lane_available = True
+      edge_available = True
+      self.lane_appeared = False
+      self.lane_available_trigger = False
+      self.object_detected_count = 0
+      lane_line_info_edge_detect = False
+      self.lane_line_info = lane_line_info % 10
+
+    # 차선/엣지 기반 lane change 가능 여부
+    lane_change_available = (lane_available or edge_available) and lane_line_info < 20  # 20 미만이면 흰색 라인
+
+    # lane_available 변화 & 폭 변화로 lane_available_trigger 계산
+    self.lane_available_trigger = False
+    if blinker_state == BLINKER_LEFT:
+      lane_width_diff = self.lane_width_left_diff
+      distance_to_road_edge = self.distance_to_road_edge_left
+      lane_width_side = self.lane_width_left
+    else:
+      lane_width_diff = self.lane_width_right_diff
+      distance_to_road_edge = self.distance_to_road_edge_right
+      lane_width_side = self.lane_width_right
+
+    if lane_width_diff > 0.8 and (lane_width_side < distance_to_road_edge):
+      self.lane_available_trigger = True
+
+    edge_availabled = (not self.edge_available_last and edge_available)
+    side_object_detected = self.object_detected_count > -0.3 / DT_MDL
+    self.lane_appeared = self.lane_appeared and distance_to_road_edge < 4.0
+
+    # Auto lane change 트리거
+    if self.carrot_lane_change_count > 0:
+      auto_lane_change_blocked = False
+      auto_lane_change_trigger = lane_change_available
+    else:
+      auto_lane_change_blocked = ((atc_blinker_state == BLINKER_LEFT) and (driver_blinker_state != BLINKER_LEFT))
+      self.auto_lane_change_enable = self.auto_lane_change_enable and not auto_lane_change_blocked
+      auto_lane_change_trigger = self.auto_lane_change_enable and edge_available and (self.lane_available_trigger or self.lane_appeared) and not side_object_detected
+      self.desireLog = f"L:{self.auto_lane_change_enable},{auto_lane_change_blocked},E:{lane_available},{edge_available},A:{self.lane_available_trigger},{self.lane_appeared},{lane_width_diff:.1f},{lane_width_side:.1f},{distance_to_road_edge:.1f}={auto_lane_change_trigger}"
+
+    # 메인 상태머신
+
+    # 0) lateral 끊기거나 너무 오래 지속되면 리셋
+    if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.turn_direction = TurnDirection.none
+      self.maneuver_type = "none"
+
+    # 1) turn 후 일정시간 동안은 아무 것도 하지 않음
+    elif self.desire_disable_count > 0:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.turn_direction = TurnDirection.none
+      self.maneuver_type = "none"
+
+    else:
+      # 깜빡이 켜져 있을 때, 이번 조작이 turn인지 lane_change인지 먼저 분류
+      if desire_enabled:
+        new_type = self._classify_maneuver_type(blinker_state, carstate, self.maneuver_type)
+      else:
+        new_type = "none"
+
+      # ★ 1) 원래 lane_change였는데 새로 보니 turn 조건 + 차선 없음이면 → 강제 전환 허용
+      if self.maneuver_type == "lane_change" and new_type == "turn" and self.lane_change_state not in [LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting]:
+        # 차선변경 도중에도 조건 만족 시 턴으로 스위칭
+        self.maneuver_type = "turn"
+        self.lane_change_state = LaneChangeState.off  # FSM 리셋 후 turn 루트로
+      # ★ 2) 그 외에는 off/pre 상태에서만 모드 변경
+      elif self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
+        self.maneuver_type = new_type
+
+      # ─ TURN 모드 처리 ─
+      if desire_enabled and self.maneuver_type == "turn" and self.enable_turn_desires: # and not carstate.standstill:
+        self.lane_change_state = LaneChangeState.off
+        if self.turn_disable_count > 0:
+          self.turn_direction = TurnDirection.none
+          self.lane_change_direction = LaneChangeDirection.none
+        else:
+          self.turn_direction = TurnDirection.turnLeft if blinker_state == BLINKER_LEFT else TurnDirection.turnRight
+          # 호환성을 위해 lane_change_direction도 turn과 동일하게 세팅
+          self.lane_change_direction = self.turn_direction
+
+      # ─ Lane Change FSM 처리 ─
+      else:
+        self.turn_direction = TurnDirection.none
+
+        # LaneChangeState.off
+        if self.lane_change_state == LaneChangeState.off:
+          if desire_enabled and not self.prev_desire_enabled and not below_lane_change_speed:
+            self.lane_change_state = LaneChangeState.preLaneChange
+            self.lane_change_ll_prob = 1.0
+            self.lane_change_delay = self.laneChangeDelay
+
+            # 맨 끝 차선이 아니면, ATC 자동 차선변경 비활성
+            lane_exist_counter_side = self.lane_exist_left_count.counter if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter
+            self.auto_lane_change_enable = False if lane_exist_counter_side > 0 or lane_change_available else True
+            self.next_lane_change = False
+
+        # LaneChangeState.preLaneChange
+        elif self.lane_change_state == LaneChangeState.preLaneChange:
+          self.lane_change_direction = LaneChangeDirection.left if blinker_state == BLINKER_LEFT else LaneChangeDirection.right
+          dir_map = {
+            LaneChangeDirection.left: (carstate.steeringTorque > 0, carstate.leftBlindspot),
+            LaneChangeDirection.right: (carstate.steeringTorque < 0, carstate.rightBlindspot),
+          }
+          torque_cond, blindspot_cond = dir_map.get(self.lane_change_direction, (False, False))
+          torque_applied = carstate.steeringPressed and torque_cond
+          blindspot_detected = blindspot_cond
+
+          # 차선이 일정시간 이상 안보이면 자동차선변경 허용
+          lane_exist_counter_side = self.lane_exist_left_count.counter if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter
+          if not lane_available or lane_exist_counter_side < int(2.0 / DT_MDL):
+            self.auto_lane_change_enable = True
+
+          # BSD
+          if blindspot_detected and not ignore_bsd:
+            self.blindspot_detected_counter = int(1.5 / DT_MDL)
+
+          if not desire_enabled or below_lane_change_speed:
+            self.lane_change_state = LaneChangeState.off
+            self.lane_change_direction = LaneChangeDirection.none
+          else:
+            # 차선변경 시작 조건
+            if (lane_change_available and self.lane_change_delay == 0) or lane_line_info_edge_detect:
+              if self.blindspot_detected_counter > 0 and not ignore_bsd:
+                if torque_applied and not block_lanechange_bsd:
+                  self.lane_change_state = LaneChangeState.laneChangeStarting
+              elif self.laneChangeNeedTorque > 0 or self.next_lane_change:
+                if torque_applied:
+                  self.lane_change_state = LaneChangeState.laneChangeStarting
+              elif driver_desire_enabled:
+                self.lane_change_state = LaneChangeState.laneChangeStarting
+              elif torque_applied or auto_lane_change_trigger or lane_line_info_edge_detect:
+                self.lane_change_state = LaneChangeState.laneChangeStarting
+
+        # LaneChangeState.laneChangeStarting
+        elif self.lane_change_state == LaneChangeState.laneChangeStarting:
+          # 원래 차선라인을 0.5초 동안 서서히 fade-out
+          self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
+          if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
+            self.lane_change_state = LaneChangeState.laneChangeFinishing
+
+        # LaneChangeState.laneChangeFinishing
+        elif self.lane_change_state == LaneChangeState.laneChangeFinishing:
+          # 1초 동안 서서히 lane line 복귀
+          self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
+          if self.lane_change_ll_prob > 0.99:
+            self.lane_change_direction = LaneChangeDirection.none
+            if desire_enabled:
+              self.lane_change_state = LaneChangeState.preLaneChange
+              self.next_lane_change = True
+            else:
+              self.lane_change_state = LaneChangeState.off
+
+    # lane_change_timer 관리
+    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
+      self.lane_change_timer = 0.0
+    else:
+      self.lane_change_timer += DT_MDL
+
+    self.lane_available_last = lane_available
+    self.edge_available_last = edge_available
+
+    self.prev_desire_enabled = desire_enabled
+
+    # 운전자가 반대 방향으로 강하게 조향하면 해당 차선변경/턴 취소
+    steering_pressed_cancel = carstate.steeringPressed and \
+                              ((carstate.steeringTorque < 0 and blinker_state == BLINKER_LEFT) or
+                               (carstate.steeringTorque > 0 and blinker_state == BLINKER_RIGHT))
+    if steering_pressed_cancel and self.lane_change_state != LaneChangeState.off:
+      self.lane_change_direction = LaneChangeDirection.none
+      self.lane_change_state = LaneChangeState.off
+      self.blinker_ignore = True
+
+    # 최종 desire 결정
+    if self.turn_direction != TurnDirection.none:
+      self.desire = TURN_DESIRES[self.turn_direction]
+      self.lane_change_direction = self.turn_direction
+    else:
+      self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
+
+    # keep pulse (LaneChangeState.preLaneChange에서 유지)
+    if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting):
+      self.keep_pulse_timer = 0.0
+    elif self.lane_change_state == LaneChangeState.preLaneChange:
+      self.keep_pulse_timer += DT_MDL
+      if self.keep_pulse_timer > 1.0:
+        self.keep_pulse_timer = 0.0
+      elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight):
+        self.desire = log.Desire.none

selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,138 @@
+import numpy as np
+from cereal import log
+from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.realtime import DT_CTRL, DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+import numpy as np
+
+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.2
+MAX_VEL_ERR = 5.0  # m/s
+
+# EU guidelines
+MAX_LATERAL_JERK = 5.0  # m/s^3
+MAX_LATERAL_ACCEL_NO_ROLL = 3.0  # m/s^2
+MAX_CURVATURE_DELTA_FRAME = 0.03 #0.019 # about 3 degree / DT_CTRL 
+
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
+
+def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay, distances):
+  if len(psis) != CONTROL_N:
+    psis = [0.0]*CONTROL_N
+    curvatures = [0.0]*CONTROL_N
+    distances = [0.0] * CONTROL_N
+  v_ego = max(MIN_SPEED, v_ego)
+
+  # TODO this needs more thought, use .2s extra for now to estimate other delays
+  delay = max(0.01, steer_actuator_delay)
+
+  # MPC can plan to turn the wheel and turn back before t_delay. This means
+  # in high delay cases some corrections never even get commanded. So just use
+  # psi to calculate a simple linearization of desired curvature
+  current_curvature_desired = curvatures[0]
+  delayed_curvature_desired = np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
+  future_curvature_desired = np.interp(1.2, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
+
+  psi = np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
+
+  distance = max(np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances), 0.001)
+  #average_curvature_desired = psi / (v_ego * delay)
+
+  # curve -> straight or reverse curve
+  if v_ego > 5 and abs(current_curvature_desired) > 0.002 and \
+     (abs(future_curvature_desired) < 0.001 or np.sign(current_curvature_desired) != np.sign(future_curvature_desired)):
+    psis_damping = 0.2
+  else:
+    psis_damping = 1.0
+  #psi *= psis_damping
+
+
+  average_curvature_desired = psi / distance
+  desired_curvature = 2 * average_curvature_desired - current_curvature_desired
+
+  # This is the "desired rate of the setpoint" not an actual desired rate
+  max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
+  safe_desired_curvature = np.clip(desired_curvature,
+                                current_curvature_desired - max_curvature_rate * DT_MDL,
+                                current_curvature_desired + max_curvature_rate * DT_MDL)
+  return safe_desired_curvature
+
+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
+
+def clip_curvature(v_ego, prev_curvature, new_curvature, roll):
+  # 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)
+
+  roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
+  max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
+  min_lat_accel = -MAX_LATERAL_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)
+  
+  new_curvature = np.clip(
+    new_curvature,
+    prev_curvature - MAX_CURVATURE_DELTA_FRAME,
+    prev_curvature + MAX_CURVATURE_DELTA_FRAME
+  )
+  
+  was_limited = limited_accel or limited_max_curv or (abs(new_curvature - prev_curvature) >= MAX_CURVATURE_DELTA_FRAME)
+
+  return float(new_curvature), was_limited
+
+def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
+  # ToDo: Try relative error, and absolute speed
+  if len(modelV2.temporalPose.trans):
+    vel_err = np.clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
+    return float(vel_err)
+  return 0.0
+
+
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
+  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
+    v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
+    v_max = np.max(speeds)
+  else:
+    v_now = 0.0
+    a_now = 0.0
+    v_target = 0.0
+    v_target_1sec = 0.0
+    a_target = 0.0
+    v_max = 0.0
+  should_stop = (v_target < vEgoStopping and
+                 v_target_1sec < vEgoStopping)
+  return a_target, should_stop, v_now, v_max #v_target #v_now
+
+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/lane_planner_2.py

@@ -0,0 +1,292 @@
+import math
+import numpy as np
+from cereal import log
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.realtime import DT_MDL
+# from openpilot.common.swaglog import cloudlog
+# from openpilot.common.logger import sLogger
+from openpilot.common.params import Params
+
+TRAJECTORY_SIZE = 33
+# positive numbers go right
+#CAMERA_OFFSET = 0 #0.08
+MIN_LANE_DISTANCE = 2.6
+MAX_LANE_DISTANCE = 3.7
+MAX_LANE_CENTERING_AWAY = 1.85
+KEEP_MIN_DISTANCE_FROM_LANE = 1.35
+KEEP_MIN_DISTANCE_FROM_EDGELANE = 1.15
+
+def clamp(num, min_value, max_value):
+  # weird broken case, do something reasonable
+  if min_value > num > max_value:
+    return (min_value + max_value) * 0.5
+  # ok, basic min/max below
+  if num < min_value:
+    return min_value
+  if num > max_value:
+    return max_value
+  return num
+
+def sigmoid(x, scale=1, offset=0):
+  return (1 / (1 + math.exp(x*scale))) + offset
+
+def lerp(start, end, t):
+  t = clamp(t, 0.0, 1.0)
+  return (start * (1.0 - t)) + (end * t)
+
+def max_abs(a, b):
+  return a if abs(a) > abs(b) else b
+
+class LanePlanner:
+  def __init__(self):
+    self.ll_t = np.zeros((TRAJECTORY_SIZE,))
+    self.ll_x = np.zeros((TRAJECTORY_SIZE,))
+    self.lll_y = np.zeros((TRAJECTORY_SIZE,))
+    self.rll_y = np.zeros((TRAJECTORY_SIZE,))
+    self.le_y = np.zeros((TRAJECTORY_SIZE,))
+    self.re_y = np.zeros((TRAJECTORY_SIZE,))
+    #self.lane_width_estimate = FirstOrderFilter(3.2, 9.95, DT_MDL)
+    self.lane_width_estimate = FirstOrderFilter(3.2, 3.0, DT_MDL)
+    self.lane_width = 3.2
+    self.lane_width_last = self.lane_width
+    self.lane_change_multiplier = 1
+    #self.lane_width_updated_count = 0
+
+    self.lll_prob = 0.
+    self.rll_prob = 0.
+    self.d_prob = 0.
+
+    self.lll_std = 0.
+    self.rll_std = 0.
+
+    self.l_lane_change_prob = 0.
+    self.r_lane_change_prob = 0.
+
+    self.debugText = ""
+    self.lane_width_left = 0.0
+    self.lane_width_right = 0.0
+    self.lane_width_left_filtered = FirstOrderFilter(1.0, 1.0, DT_MDL)
+    self.lane_width_right_filtered = FirstOrderFilter(1.0, 1.0, DT_MDL)
+    self.lane_offset_filtered = FirstOrderFilter(0.0, 2.0, DT_MDL)
+
+    self.lanefull_mode = False
+    self.d_prob_count = 0
+
+    self.params = Params()
+    self.camera_offset = self.params.get_int("CameraOffset") * 0.01
+
+  def parse_model(self, md):
+
+    lane_lines = md.laneLines
+    edges = md.roadEdges
+
+    if len(lane_lines) >= 4 and len(lane_lines[0].t) == TRAJECTORY_SIZE:
+      self.ll_t = (np.array(lane_lines[1].t) + np.array(lane_lines[2].t))/2
+      # left and right ll x is the same
+      self.ll_x = lane_lines[1].x
+      self.lll_y = np.array(lane_lines[1].y)
+      self.rll_y = np.array(lane_lines[2].y)
+      self.lll_prob = md.laneLineProbs[1]
+      self.rll_prob = md.laneLineProbs[2]
+      self.lll_std = md.laneLineStds[1]
+      self.rll_std = md.laneLineStds[2]
+
+    if len(edges[0].t) == TRAJECTORY_SIZE:
+      self.le_y = np.array(edges[0].y) + md.roadEdgeStds[0] * 0.4
+      self.re_y = np.array(edges[1].y) - md.roadEdgeStds[1] * 0.4
+    else:
+      self.le_y = self.lll_y
+      self.re_y = self.rll_y
+
+    desire_state = md.meta.desireState
+    if len(desire_state):
+      self.l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
+      self.r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
+
+  def get_d_path(self, CS, v_ego, path_t, path_xyz, curve_speed):
+    #if v_ego > 0.1:
+    #  self.lane_width_updated_count = max(0, self.lane_width_updated_count - 1)
+    # Reduce reliance on lanelines that are too far apart or
+    # will be in a few seconds
+    l_prob, r_prob = self.lll_prob, self.rll_prob
+    width_pts = self.rll_y - self.lll_y
+    prob_mods = []
+    for t_check in (0.0, 1.5, 3.0):
+      width_at_t = np.interp(t_check * (v_ego + 7), self.ll_x, width_pts)
+      #prob_mods.append(np.interp(width_at_t, [4.0, 5.0], [1.0, 0.0]))
+      prob_mods.append(np.interp(width_at_t, [4.5, 6.0], [1.0, 0.0]))
+    mod = min(prob_mods)
+    l_prob *= mod
+    r_prob *= mod
+
+    # Reduce reliance on uncertain lanelines
+    l_std_mod = np.interp(self.lll_std, [.15, .3], [1.0, 0.0])
+    r_std_mod = np.interp(self.rll_std, [.15, .3], [1.0, 0.0])
+    l_prob *= l_std_mod
+    r_prob *= r_std_mod
+
+    self.l_prob, self.r_prob = l_prob, r_prob
+
+    # Find current lanewidth
+    current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
+
+    max_updated_count = 10.0 * DT_MDL
+    both_lane_available = False
+    #speed_lane_width = np.interp(v_ego*3.6, [0., 60.], [2.8, 3.5])
+    if l_prob > 0.5 and r_prob > 0.5 and self.lane_change_multiplier > 0.5:
+      both_lane_available = True
+      #self.lane_width_updated_count = max_updated_count
+      self.lane_width_estimate.update(current_lane_width)
+      self.lane_width_last = self.lane_width_estimate.x
+    #elif self.lane_width_updated_count <= 0 and v_ego > 0.1:   # 양쪽차선이 없을때.... 일정시간후(10초)부터 speed차선폭 적용함.
+    #  self.lane_width_estimate.update(speed_lane_width)
+    else:
+      self.lane_width_estimate.update(self.lane_width_last)
+
+    self.lane_width =  self.lane_width_estimate.x
+    clipped_lane_width = min(4.0, self.lane_width)
+    path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
+    path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
+
+    # 가장 차선이 진한쪽으로 골라서..
+    self.d_prob = max(l_prob, r_prob) if not both_lane_available else 1.0
+
+    # 좌/우의 차선폭을 필터링.
+    if self.lane_width_left > 0:
+      self.lane_width_left_filtered.update(self.lane_width_left)
+      #self.lane_width_left_filtered.x = self.lane_width_left #바로적용
+    if self.lane_width_right > 0:
+      self.lane_width_right_filtered.update(self.lane_width_right)
+      #self.lane_width_right_filtered.x = self.lane_width_right #바로적용
+
+    self.adjustLaneOffset = float(self.params.get_int("AdjustLaneOffset")) * 0.01
+    self.adjustCurveOffset = self.adjustLaneOffset #float(self.params.get_int("AdjustCurveOffset")) * 0.01
+    ADJUST_OFFSET_LIMIT = 0.4 #max(self.adjustLaneOffset, self.adjustCurveOffset)
+    offset_curve = 0.0
+    ## curve offset
+    offset_curve = np.interp(abs(curve_speed), [50, 200], [self.adjustCurveOffset, 0.0]) * np.sign(curve_speed)
+
+    offset_lane = 0.0
+    if self.lane_width_left_filtered.x > 2.2 and self.lane_width_right_filtered.x > 2.2: #양쪽에 차로가 여유 있는경우
+      offset_lane = 0.0
+    elif self.lane_width_left_filtered.x < 2.0 and self.lane_width_right_filtered.x < 2.0: #양쪽에 차로가 여유 없는경우
+      offset_lane = 0.0
+    elif self.lane_width_left_filtered.x > self.lane_width_right_filtered.x:
+      offset_lane = np.interp(self.lane_width, [2.5, 2.9], [0.0, self.adjustLaneOffset]) # 차선이 좁으면 안함..
+    else:
+      offset_lane = np.interp(self.lane_width, [2.5, 2.9], [0.0, -self.adjustLaneOffset]) # 차선이 좁으면 안함..
+
+    #select lane path
+    # 차선이 좁아지면, 도로경계쪽에 있는 차선 위주로 따라가도록함.
+    if self.lane_width < 2.5:
+      if r_prob > 0.5 and self.lane_width_right_filtered.x < self.lane_width_left_filtered.x:
+        lane_path_y = path_from_right_lane
+      elif l_prob > 0.5 and self.lane_width_left_filtered.x < 2.0:
+        lane_path_y = path_from_left_lane
+      else:
+        lane_path_y = path_from_left_lane if l_prob > 0.5 or l_prob > r_prob else path_from_right_lane
+    elif l_prob > 0.7 and r_prob > 0.7:
+      lane_path_y = (path_from_left_lane + path_from_right_lane) / 2.
+      # lane_width filtering에 의해서, 점점 줄어들때, 중앙선으로 붙어가는 현상이 생김..
+      #if self.lane_width > 3.2:
+      #  lane_path_y = path_from_right_lane
+      #else:
+      #  lane_path_y = (path_from_left_lane + path_from_right_lane) / 2.
+    # 그외 진한차선을 따라가도록함.
+    else:
+      lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+
+    use_laneless_center_adjust = False
+    if use_laneless_center_adjust:
+      ## 0.5초 앞의 중심을 보도록함.
+      lane_path_y_center = np.interp(0.5, path_t, lane_path_y)
+      path_xyz_y_center = np.interp(0.5, path_t, path_xyz[:,1])
+      #lane_path_y_center = lane_path_y[0]
+      #path_xyz_y_center = path_xyz[:,1][0]
+      diff_center = (lane_path_y_center - path_xyz_y_center) if not self.lanefull_mode else 0.0
+    else:
+      diff_center = 0.0
+    #print("center = {:.2f}={:.2f}-{:.2f}, lanefull={}".format(diff_center, lane_path_y_center, path_xyz_y_center, self.lanefull_mode))
+    #diff_center = lane_path_y[5] - path_xyz[:,1][5] if not self.lanefull_mode else 0.0
+    if offset_curve * offset_lane < 0:
+      offset_total = np.clip(offset_curve + offset_lane + diff_center, - ADJUST_OFFSET_LIMIT, ADJUST_OFFSET_LIMIT)
+    else:
+      offset_total = np.clip(max(offset_curve, offset_lane, key=abs) + diff_center, - ADJUST_OFFSET_LIMIT, ADJUST_OFFSET_LIMIT)
+
+    ## self.d_prob = 0 if lane_changing
+    self.d_prob *= self.lane_change_multiplier  ## 차선변경중에는 꺼버림.
+    if self.lane_change_multiplier < 0.5:
+      #self.lane_offset_filtered.x = 0.0
+      pass
+    else:
+      self.lane_offset_filtered.update(np.interp(self.d_prob, [0, 0.3], [0, offset_total]))
+
+    ## laneless at lowspeed
+    self.d_prob *= np.interp(v_ego*3.6, [5., 10.], [0.0, 1.0])
+
+    #self.debugText = "OFFSET({:.2f}={:.2f}+{:.2f}+{:.2f}),Vc:{:.2f},dp:{:.1f},lf:{},lrw={:.1f}|{:.1f}|{:.1f}".format(
+    #  self.lane_offset_filtered.x,
+    #  diff_center, offset_lane, offset_curve,
+    #  curve_speed,
+    #  self.d_prob, self.lanefull_mode,
+    #  self.lane_width_left_filtered.x, self.lane_width, self.lane_width_right_filtered.x)
+
+    adjustLaneTime = self.params.get_float("LatMpcInputOffset") * 0.01 # 0.06
+    laneline_active = False
+    self.d_prob_count = self.d_prob_count + 1 if self.d_prob > 0.3 else 0
+    if self.lanefull_mode and self.d_prob_count > int(1 / DT_MDL):
+      laneline_active = True
+      use_dist_mode = False  ## 아무리생각해봐도.. 같은 방법인듯...
+      if use_dist_mode:
+        lane_path_y_interp = np.interp(path_xyz[:,0] + v_ego * adjustLaneTime, self.ll_x, lane_path_y)
+        path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
+      else:
+        safe_idxs = np.isfinite(self.ll_t)
+        if safe_idxs[0]:
+          lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
+          path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
+
+
+    path_xyz[:, 1] += (self.camera_offset + self.lane_offset_filtered.x)
+
+    self.offset_total = self.lane_offset_filtered.x
+
+    return path_xyz, laneline_active
+
+  def calculate_plan_yaw_and_yaw_rate(self, path_xyz):
+    if path_xyz.shape[0] < 3:
+        # 너무 짧으면 직진 가정
+        N = path_xyz.shape[0]
+        return np.zeros(N), np.zeros(N)
+
+    # x, y 추출
+    x = path_xyz[:, 0]
+    y = path_xyz[:, 1]
+
+    # 모두 동일한 점인지 확인
+    if np.allclose(x, x[0]) and np.allclose(y, y[0]):
+        return np.zeros(len(x)), np.zeros(len(x))
+
+    # 안전한 diff 계산
+    dx = np.diff(x)
+    dy = np.diff(y)
+    mask = (dx == 0) & (dy == 0)
+    dx[mask] = 1e-4
+    dy[mask] = 0.0
+
+    yaw = np.arctan2(dy, dx)
+    yaw = np.append(yaw, yaw[-1])  # N-1 → N
+    yaw = np.unwrap(yaw)
+
+    dx_full = np.clip(np.diff(x), 1e-4, None)
+    yaw_rate = np.diff(yaw) / dx_full
+    yaw_rate = np.append(yaw_rate, yaw_rate[-1])
+    yaw_rate = np.append(yaw_rate, 0.0)
+
+    # NaN/Inf 방어
+    if np.any(np.isnan(yaw_rate)) or np.any(np.isinf(yaw_rate)):
+        yaw_rate = np.zeros_like(yaw_rate)
+    if np.any(np.isnan(yaw)) or np.any(np.isinf(yaw)):
+        yaw = np.zeros_like(yaw)
+
+    return yaw, yaw_rate

selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,33 @@
+import numpy as np
+from abc import abstractmethod, ABC
+
+from openpilot.common.realtime import DT_CTRL
+
+MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
+
+
+class LatControl(ABC):
+  def __init__(self, CP, CI):
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+    self.sat_count = 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, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
+    pass
+
+  def reset(self):
+    self.sat_count = 0.
+
+  def _check_saturation(self, saturated, CS, steer_limited_by_controls, 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_controls and not CS.steeringPressed:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+    self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
+    return self.sat_count > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_angle.py

@@ -0,0 +1,30 @@
+import math
+import numpy as np
+
+from cereal import log
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+
+
+class LatControlAngle(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.sat_check_min_speed = 5.
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):  
+    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
+
+    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 = float(angle_steers_des) if not CS.steeringPressed else float(CS.steeringAngleDeg)
+    return 0, float(angle_steers_des), angle_log

selfdrive/controls/lib/latcontrol_pid.py

@@ -0,0 +1,48 @@
+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, CI):
+    super().__init__(CP, CI)
+    self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
+                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
+                             k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+
+  def reset(self):
+    super().reset()
+    self.pid.reset()
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
+    pid_log = log.ControlsState.LateralPIDState.new_message()
+    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+    error = angle_steers_des - CS.steeringAngleDeg
+
+    pid_log.steeringAngleDesiredDeg = angle_steers_des
+    pid_log.angleError = error
+    if not active:
+      output_steer = 0.0
+      pid_log.active = False
+      self.pid.reset()
+    else:
+      # offset does not contribute to resistive torque
+      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+
+      output_steer = self.pid.update(error, override=CS.steeringPressed,
+                                     feedforward=steer_feedforward, speed=CS.vEgo)
+      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_steer)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
+
+    return output_steer, angle_steers_des, pid_log

selfdrive/controls/lib/latcontrol_torque.py

@@ -0,0 +1,304 @@
+from collections import deque
+
+import math
+import numpy as np
+
+from cereal import log
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+
+from opendbc.car.interfaces import LatControlInputs
+from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+from openpilot.common.params import Params
+
+# 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 we
+# use a LOW_SPEED_FACTOR in the error. Additionally, there is
+# friction in the steering wheel that needs to be overcome to
+# move it at all, this is compensated for too.
+
+LOW_SPEED_X = [0, 10, 20, 30]
+LOW_SPEED_Y = [15, 13, 10, 5]
+LOW_SPEED_Y_NN = [12, 3, 1, 0]
+
+LAT_PLAN_MIN_IDX = 5
+
+def get_predicted_lateral_jerk(lat_accels, t_diffs):
+  # compute finite difference between subsequent model_data.acceleration.y values
+  # this is just two calls of np.diff followed by an element-wise division
+  lat_accel_diffs = np.diff(lat_accels)
+  lat_jerk = lat_accel_diffs / t_diffs
+  # return as python list
+  return lat_jerk.tolist()
+
+def sign(x):
+  return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0)
+
+def get_lookahead_value(future_vals, current_val):
+  if len(future_vals) == 0:
+    return current_val
+
+  same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)]
+
+  # if any future val has opposite sign of current val, return 0
+  if len(same_sign_vals) < len(future_vals):
+    return 0.0
+
+  # otherwise return the value with minimum absolute value
+  min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
+  return min_val
+
+# At a given roll, if pitch magnitude increases, the
+# gravitational acceleration component starts pointing
+# in the longitudinal direction, decreasing the lateral
+# acceleration component. Here we do the same thing
+# to the roll value itself, then passed to nnff.
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
+                             k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.use_steering_angle = self.torque_params.useSteeringAngle
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+
+    # carrot
+    self.frame = 0
+    self.params = Params()
+    self.lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
+    self.latAccelFactor_default = self.torque_params.latAccelFactor
+    self.latAccelOffset_default = self.torque_params.latAccelOffset
+    self.friction_default = self.torque_params.friction
+
+    # Twilsonco's Lateral Neural Network Feedforward
+    self.use_nnff = CI.use_nnff
+    self.use_nnff_lite = CI.use_nnff_lite
+
+    if self.use_nnff or self.use_nnff_lite:
+      # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
+      # however, we can "look ahead" to the future planned lateral jerk in order to guage
+      # whether the current desired lateral jerk will persist into the future, i.e.
+      # whether it's "deliberate" or not. This lets us simply ignore short-lived jerk.
+      # Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent
+      # using a "future" value that is actually planned to occur before the "current" desired
+      # value, which is offset by the steerActuatorDelay.
+      self.friction_look_ahead_v = [1.4, 2.0] # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments
+      self.friction_look_ahead_bp = [9.0, 30.0] # corresponding speeds in m/s in [0, ~40] in 1.0 increments
+
+      # Scaling the lateral acceleration "friction response" could be helpful for some.
+      # Increase for a stronger response, decrease for a weaker response.
+      self.lat_jerk_friction_factor = 0.4
+      self.lat_accel_friction_factor = 0.7 # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
+
+      # precompute time differences between ModelConstants.T_IDXS
+      self.t_diffs = np.diff(ModelConstants.T_IDXS)
+      self.desired_lat_jerk_time = self.params.get_float("SteerActuatorDelay") * 0.01 + 0.3
+
+    if self.use_nnff:
+      self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+      # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
+      # of lat accel and roll
+      # Past value is computed using previous desired lat accel and observed roll
+      self.torque_from_nn = CI.get_ff_nn
+      self.nn_friction_override = CI.lat_torque_nn_model.friction_override
+
+      # setup future time offsets
+      self.nn_time_offset = CP.steerActuatorDelay + 0.2
+      future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
+      self.nn_future_times = [i + self.nn_time_offset for i in future_times]
+      self.nn_future_times_np = np.array(self.nn_future_times)
+
+      # setup past time offsets
+      self.past_times = [-0.3, -0.2, -0.1]
+      history_check_frames = [int(abs(i)*100) for i in self.past_times]
+      self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames]
+      self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0])
+      self.roll_deque = deque(maxlen=history_check_frames[0])
+      self.error_deque = deque(maxlen=history_check_frames[0])
+      self.past_future_len = len(self.past_times) + len(self.nn_future_times)
+
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    if self.lateralTorqueCustom > 0:
+      return
+
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+
+  def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):
+    self.frame += 1
+    if self.frame % 100 == 0:
+      lateralTorqueCustom = self.params.get_int("LateralTorqueCustom")
+      if lateralTorqueCustom == 1:
+        self.torque_params.latAccelFactor = self.params.get_float("LateralTorqueAccelFactor")*0.001
+        self.torque_params.friction = self.params.get_float("LateralTorqueFriction")*0.001
+        lateralTorqueKp = self.params.get_float("LateralTorqueKpV")*0.01
+        lateralTorqueKi = self.params.get_float("LateralTorqueKiV")*0.01
+        lateralTorqueKf = self.params.get_float("LateralTorqueKf")*0.01
+        lateralTorqueKd = self.params.get_float("LateralTorqueKd")*0.01
+        self.pid._k_p = [[0], [lateralTorqueKp]]
+        self.pid._k_i = [[0], [lateralTorqueKi]]
+        self.pid.k_f = lateralTorqueKf
+        self.pid._k_d = [[0], [lateralTorqueKd]]
+        self.torque_params.latAccelOffset = self.latAccelOffset_default
+      elif self.lateralTorqueCustom > 1:  # 1 -> 0, reset to default
+        self.torque_params.latAccelFactor = self.latAccelFactor_default
+        self.torque_params.friction = self.friction_default
+        self.torque_params.latAccelOffset = self.latAccelOffset_default
+      self.lateralTorqueCustom = lateralTorqueCustom
+
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    nn_log = None
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+      angle_steers_des = float(CS.steeringAngleDeg)
+    else:
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+      angle_steers_des += params.angleOffsetDeg
+
+      actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+      actual_lateral_jerk = 0.0
+      if self.use_steering_angle:
+        actual_curvature = actual_curvature_vm
+        curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+        if self.use_nnff or self.use_nnff_lite:
+          actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
+          actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
+      else:
+        actual_curvature_llk = CC.angularVelocity[2] / CS.vEgo #llk.angularVelocityCalibrated.value[2] / CS.vEgo
+        actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_llk])
+        curvature_deadzone = 0.0
+      desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+
+      # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
+      # desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
+      actual_lateral_accel = actual_curvature * CS.vEgo ** 2
+      lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+      low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
+      setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
+      measurement = actual_lateral_accel + low_speed_factor * actual_curvature
+
+      lateral_jerk_setpoint = 0
+      lateral_jerk_measurement = 0
+      lookahead_lateral_jerk = 0
+
+      model_good = model_data is not None and len(model_data.orientation.x) >= CONTROL_N
+      if model_good and (self.use_nnff or self.use_nnff_lite):
+        # prepare "look-ahead" desired lateral jerk
+        lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+        friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16)
+        predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs)
+        desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
+        lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
+
+        if self.use_steering_angle or lookahead_lateral_jerk == 0.0:
+          lookahead_lateral_jerk = 0.0
+          actual_lateral_jerk = 0.0
+          self.lat_accel_friction_factor = 1.0
+        lateral_jerk_setpoint = self.lat_jerk_friction_factor * lookahead_lateral_jerk
+        lateral_jerk_measurement = self.lat_jerk_friction_factor * actual_lateral_jerk
+
+      if self.use_nnff and model_good:
+        # update past data
+        pitch = 0
+        roll = params.roll
+        if len(CC.orientationNED) > 1:
+          pitch = self.pitch.update(CC.orientationNED[1])
+          roll = roll_pitch_adjust(roll, pitch)
+        self.roll_deque.append(roll)
+        self.lateral_accel_desired_deque.append(desired_lateral_accel)
+
+        # prepare past and future values
+        # adjust future times to account for longitudinal acceleration
+        adjusted_future_times = [t + 0.5*CS.aEgo*(t/max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+        past_rolls = [self.roll_deque[min(len(self.roll_deque)-1, i)] for i in self.history_frame_offsets]
+        future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, model_data.orientation.x) + roll, np.interp(t, ModelConstants.T_IDXS, model_data.orientation.y) + pitch) for t in adjusted_future_times]
+        past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque)-1, i)] for i in self.history_frame_offsets]
+        future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, model_data.acceleration.y) for t in adjusted_future_times]
+
+        # compute NNFF error response
+        nnff_setpoint_input = [CS.vEgo, setpoint, lateral_jerk_setpoint, roll] \
+                              + [setpoint] * self.past_future_len \
+                              + past_rolls + future_rolls
+        # past lateral accel error shouldn't count, so use past desired like the setpoint input
+        nnff_measurement_input = [CS.vEgo, measurement, lateral_jerk_measurement, roll] \
+                                 + [measurement] * self.past_future_len \
+                                 + past_rolls + future_rolls
+        torque_from_setpoint = self.torque_from_nn(nnff_setpoint_input)
+        torque_from_measurement = self.torque_from_nn(nnff_measurement_input)
+
+        pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+        error_blend_factor = np.interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0])
+        if error_blend_factor > 0.0:  # blend in stronger error response when in high lat accel
+          nnff_error_input = [CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0]
+          torque_from_error = self.torque_from_nn(nnff_error_input)
+          if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error):
+            pid_log.error = float(pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor)
+
+        # compute feedforward (same as nn setpoint output)
+        error = setpoint - measurement
+        friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
+        nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] \
+                   + past_lateral_accels_desired + future_planned_lateral_accels \
+                   + past_rolls + future_rolls
+        ff = self.torque_from_nn(nn_input)
+
+        # apply friction override for cars with low NN friction response
+        if self.nn_friction_override:
+          pid_log.error += self.torque_from_lateral_accel(LatControlInputs(0.0, 0.0, CS.vEgo, CS.aEgo), self.torque_params,
+                                                          friction_input, lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=False)
+        nn_log = nn_input + nnff_setpoint_input + nnff_measurement_input
+
+      else:
+        gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
+        torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
+                                                              lateral_jerk_setpoint, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
+        torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
+                                                                 lateral_jerk_measurement, lateral_accel_deadzone, friction_compensation=self.use_nnff_lite, gravity_adjusted=False)
+        pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+        error = desired_lateral_accel - actual_lateral_accel
+        if self.use_nnff_lite:
+          friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
+        else:
+          friction_input = error
+        ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
+                                            friction_input, lateral_accel_deadzone, friction_compensation=True,
+                                            gravity_adjusted=True)
+
+      freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
+      output_torque = self.pid.update(pid_log.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.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque)
+      pid_log.actualLateralAccel = float(actual_lateral_accel)
+      pid_log.desiredLateralAccel = float(desired_lateral_accel)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
+      #if nn_log is not None:
+      #  pid_log.nnLog = nn_log
+
+    # TODO left is positive in this convention
+    return -output_torque,angle_steers_des, pid_log

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": [
+        "/usr/local/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 /usr/local/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 /usr/local/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/c_generated_code/acados_sim_solver_lat.h

@@ -0,0 +1,101 @@
+/*
+ * 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.;
+ */
+
+#ifndef ACADOS_SIM_lat_H_
+#define ACADOS_SIM_lat_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX     4
+#define LAT_NZ     0
+#define LAT_NU     1
+#define LAT_NP     2
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_vde_adj_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_lat_H_

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd

@@ -0,0 +1,62 @@
+#
+# 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.;
+#
+
+cimport acados_solver_common
+
+cdef extern from "acados_solver_lat.h":
+    ctypedef struct nlp_solver_capsule "lat_solver_capsule":
+        pass
+
+    nlp_solver_capsule * acados_create_capsule "lat_acados_create_capsule"()
+    int acados_free_capsule "lat_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+    int acados_create "lat_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "lat_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "lat_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "lat_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
+    int acados_update_params "lat_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
+    int acados_update_params_sparse "lat_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+    int acados_solve "lat_acados_solve"(nlp_solver_capsule * capsule)
+    int acados_reset "lat_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
+    int acados_free "lat_acados_free"(nlp_solver_capsule * capsule)
+    void acados_print_stats "lat_acados_print_stats"(nlp_solver_capsule * capsule)
+
+    int acados_custom_update "lat_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
+
+    acados_solver_common.ocp_nlp_in *acados_get_nlp_in "lat_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_nlp_out "lat_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "lat_acados_get_sens_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "lat_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_config *acados_get_nlp_config "lat_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+    void *acados_get_nlp_opts "lat_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "lat_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "lat_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver_lat.h

@@ -0,0 +1,170 @@
+/*
+ * 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.;
+ */
+
+#ifndef ACADOS_SOLVER_lat_H_
+#define ACADOS_SOLVER_lat_H_
+
+#include "acados/utils/types.h"
+
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LAT_NX     4
+#define LAT_NZ     0
+#define LAT_NU     1
+#define LAT_NP     2
+#define LAT_NBX    2
+#define LAT_NBX0   4
+#define LAT_NBU    0
+#define LAT_NSBX   0
+#define LAT_NSBU   0
+#define LAT_NSH    0
+#define LAT_NSG    0
+#define LAT_NSPHI  0
+#define LAT_NSHN   0
+#define LAT_NSGN   0
+#define LAT_NSPHIN 0
+#define LAT_NSBXN  0
+#define LAT_NS     0
+#define LAT_NSN    0
+#define LAT_NG     0
+#define LAT_NBXN   0
+#define LAT_NGN    0
+#define LAT_NY0    5
+#define LAT_NY     5
+#define LAT_NYN    3
+#define LAT_N      32
+#define LAT_NH     0
+#define LAT_NPHI   0
+#define LAT_NHN    0
+#define LAT_NPHIN  0
+#define LAT_NR     0
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct lat_solver_capsule
+{
+    // acados objects
+    ocp_nlp_in *nlp_in;
+    ocp_nlp_out *nlp_out;
+    ocp_nlp_out *sens_out;
+    ocp_nlp_solver *nlp_solver;
+    void *nlp_opts;
+    ocp_nlp_plan_t *nlp_solver_plan;
+    ocp_nlp_config *nlp_config;
+    ocp_nlp_dims *nlp_dims;
+
+    // number of expected runtime parameters
+    unsigned int nlp_np;
+
+    /* external functions */
+    // dynamics
+
+    external_function_param_casadi *forw_vde_casadi;
+    external_function_param_casadi *expl_ode_fun;
+
+
+
+
+    // cost
+
+    external_function_param_casadi *cost_y_fun;
+    external_function_param_casadi *cost_y_fun_jac_ut_xt;
+    external_function_param_casadi *cost_y_hess;
+
+
+
+    external_function_param_casadi cost_y_0_fun;
+    external_function_param_casadi cost_y_0_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_0_hess;
+
+
+
+    external_function_param_casadi cost_y_e_fun;
+    external_function_param_casadi cost_y_e_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_e_hess;
+
+
+    // constraints
+
+
+
+
+} lat_solver_capsule;
+
+ACADOS_SYMBOL_EXPORT lat_solver_capsule * lat_acados_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int lat_acados_free_capsule(lat_solver_capsule *capsule);
+
+ACADOS_SYMBOL_EXPORT int lat_acados_create(lat_solver_capsule * capsule);
+
+ACADOS_SYMBOL_EXPORT int lat_acados_reset(lat_solver_capsule* capsule, int reset_qp_solver_mem);
+
+/**
+ * Generic version of lat_acados_create which allows to use a different number of shooting intervals than
+ * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
+ * generation, the time-steps from code generation is used.
+ */
+ACADOS_SYMBOL_EXPORT int lat_acados_create_with_discretization(lat_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
+/**
+ * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
+ * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
+ */
+ACADOS_SYMBOL_EXPORT int lat_acados_update_time_steps(lat_solver_capsule * capsule, int N, double* new_time_steps);
+/**
+ * This function is used for updating an already initialized solver with a different number of qp_cond_N.
+ */
+ACADOS_SYMBOL_EXPORT int lat_acados_update_qp_solver_cond_N(lat_solver_capsule * capsule, int qp_solver_cond_N);
+ACADOS_SYMBOL_EXPORT int lat_acados_update_params(lat_solver_capsule * capsule, int stage, double *value, int np);
+ACADOS_SYMBOL_EXPORT int lat_acados_update_params_sparse(lat_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
+
+ACADOS_SYMBOL_EXPORT int lat_acados_solve(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_free(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT void lat_acados_print_stats(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT int lat_acados_custom_update(lat_solver_capsule* capsule, double* data, int data_len);
+
+
+ACADOS_SYMBOL_EXPORT ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT void *lat_acados_get_nlp_opts(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule * capsule);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SOLVER_lat_H_

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_constraints/lat_constraints.h

@@ -0,0 +1,50 @@
+/*
+ * 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.;
+ */
+
+#ifndef lat_CONSTRAINTS
+#define lat_CONSTRAINTS
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+
+
+
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // lat_CONSTRAINTS

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_cost/lat_cost.h

@@ -0,0 +1,119 @@
+/*
+ * 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.;
+ */
+
+
+#ifndef lat_COST
+#define lat_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// Cost at initial shooting node
+
+int lat_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_0_fun_work(int *, int *, int *, int *);
+const int *lat_cost_y_0_fun_sparsity_in(int);
+const int *lat_cost_y_0_fun_sparsity_out(int);
+int lat_cost_y_0_fun_n_in(void);
+int lat_cost_y_0_fun_n_out(void);
+
+int lat_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
+const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
+int lat_cost_y_0_fun_jac_ut_xt_n_in(void);
+int lat_cost_y_0_fun_jac_ut_xt_n_out(void);
+
+int lat_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_0_hess_work(int *, int *, int *, int *);
+const int *lat_cost_y_0_hess_sparsity_in(int);
+const int *lat_cost_y_0_hess_sparsity_out(int);
+int lat_cost_y_0_hess_n_in(void);
+int lat_cost_y_0_hess_n_out(void);
+
+
+
+// Cost at path shooting node
+
+int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_fun_work(int *, int *, int *, int *);
+const int *lat_cost_y_fun_sparsity_in(int);
+const int *lat_cost_y_fun_sparsity_out(int);
+int lat_cost_y_fun_n_in(void);
+int lat_cost_y_fun_n_out(void);
+
+int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
+const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
+int lat_cost_y_fun_jac_ut_xt_n_in(void);
+int lat_cost_y_fun_jac_ut_xt_n_out(void);
+
+int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_hess_work(int *, int *, int *, int *);
+const int *lat_cost_y_hess_sparsity_in(int);
+const int *lat_cost_y_hess_sparsity_out(int);
+int lat_cost_y_hess_n_in(void);
+int lat_cost_y_hess_n_out(void);
+
+
+
+// Cost at terminal shooting node
+
+int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_e_fun_work(int *, int *, int *, int *);
+const int *lat_cost_y_e_fun_sparsity_in(int);
+const int *lat_cost_y_e_fun_sparsity_out(int);
+int lat_cost_y_e_fun_n_in(void);
+int lat_cost_y_e_fun_n_out(void);
+
+int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
+const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
+int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
+int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
+
+int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_cost_y_e_hess_work(int *, int *, int *, int *);
+const int *lat_cost_y_e_hess_sparsity_in(int);
+const int *lat_cost_y_e_hess_sparsity_out(int);
+int lat_cost_y_e_hess_n_in(void);
+int lat_cost_y_e_hess_n_out(void);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // lat_COST

selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/lat_model/lat_model.h

@@ -0,0 +1,71 @@
+/*
+ * 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.;
+ */
+
+#ifndef lat_MODEL
+#define lat_MODEL
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* explicit ODE */
+
+// explicit ODE
+int lat_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_expl_ode_fun_work(int *, int *, int *, int *);
+const int *lat_expl_ode_fun_sparsity_in(int);
+const int *lat_expl_ode_fun_sparsity_out(int);
+int lat_expl_ode_fun_n_in(void);
+int lat_expl_ode_fun_n_out(void);
+
+// explicit forward VDE
+int lat_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_expl_vde_forw_work(int *, int *, int *, int *);
+const int *lat_expl_vde_forw_sparsity_in(int);
+const int *lat_expl_vde_forw_sparsity_out(int);
+int lat_expl_vde_forw_n_in(void);
+int lat_expl_vde_forw_n_out(void);
+
+// explicit adjoint VDE
+int lat_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int lat_expl_vde_adj_work(int *, int *, int *, int *);
+const int *lat_expl_vde_adj_sparsity_in(int);
+const int *lat_expl_vde_adj_sparsity_out(int);
+int lat_expl_vde_adj_n_in(void);
+int lat_expl_vde_adj_n_out(void);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // lat_MODEL

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

@@ -0,0 +1,347 @@
+import time
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
+from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
+# from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+import cereal.messaging as messaging
+from cereal import log
+
+from openpilot.common.params import Params
+#from openpilot.selfdrive.controls.lib.lane_planner import LanePlanner
+from openpilot.selfdrive.controls.lib.lane_planner_2 import LanePlanner
+from collections import deque
+
+TRAJECTORY_SIZE = 33
+#CAMERA_OFFSET = 0.04
+
+
+PATH_COST = 1.0
+LATERAL_MOTION_COST = 0.11
+LATERAL_ACCEL_COST = 0.0
+LATERAL_JERK_COST = 0.04
+# Extreme steering rate is unpleasant, even
+# when it does not cause bad jerk.
+# TODO this cost should be lowered when low
+# speed lateral control is stable on all cars
+STEERING_RATE_COST = 700.0
+
+
+class LateralPlanner:
+  def __init__(self, CP, debug=False):
+    #self.DH = DesireHelper()
+
+    # Vehicle model parameters used to calculate lateral movement of car
+    self.factor1 = CP.wheelbase - CP.centerToFront
+    self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear)
+    self.last_cloudlog_t = 0
+    self.solution_invalid_cnt = 0
+
+    self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+    self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+    self.v_plan = np.zeros((TRAJECTORY_SIZE,))
+    self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32)
+    self.v_ego = MIN_SPEED
+    self.l_lane_change_prob = 0.0
+    self.r_lane_change_prob = 0.0
+
+    self.debug_mode = debug
+
+    self.params = Params()
+
+    self.latDebugText = ""
+    # lane_mode
+    self.LP = LanePlanner()
+    self.readParams = 0
+    self.lanelines_active = False
+    self.lanelines_active_tmp = False
+
+    self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeed")
+    self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
+    self.useLaneLineMode = False
+    self.plan_a = np.zeros((TRAJECTORY_SIZE, ))
+    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
+    self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
+    self.t_idxs = np.arange(TRAJECTORY_SIZE)
+    self.y_pts = np.zeros((TRAJECTORY_SIZE,))
+    self.d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+
+    self.lat_mpc = LateralMpc()
+    self.reset_mpc(np.zeros(4))
+    self.curve_speed = 0
+    self.lanemode_possible_count = 0
+    self.laneless_only = True
+
+  def reset_mpc(self, x0=None):
+    if x0 is None:
+      x0 = np.zeros(4)
+    self.x0 = x0
+    self.lat_mpc.reset(x0=self.x0)
+
+  def update(self, sm, carrot):
+    global LATERAL_ACCEL_COST, LATERAL_JERK_COST, STEERING_RATE_COST
+    self.readParams -= 1
+    if self.readParams <= 0:
+      self.readParams = 100
+      self.useLaneLineSpeedApply = sm['carState'].useLaneLineSpeed
+      self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
+      self.lateralPathCost = self.params.get_float("LatMpcPathCost") * 0.01
+      self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01
+      LATERAL_ACCEL_COST = self.params.get_float("LatMpcAccelCost") * 0.01
+      LATERAL_JERK_COST = self.params.get_float("LatMpcJerkCost") * 0.01
+      STEERING_RATE_COST = self.params.get_float("LatMpcSteeringRateCost")
+
+    # clip speed , lateral planning is not possible at 0 speed
+    measured_curvature = sm['controlsState'].curvature
+    v_ego_car = max(sm['carState'].vEgo, MIN_SPEED)
+    speed_kph = v_ego_car * 3.6
+    self.v_ego = v_ego_car
+    self.curve_speed = sm['carrotMan'].vTurnSpeed
+
+    # Parse model predictions
+    md = sm['modelV2']
+    model_active = False
+    if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
+      model_active = True
+      self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
+      self.t_idxs = np.array(md.position.t)
+      self.plan_yaw = np.array(md.orientation.z)
+      self.plan_yaw_rate = np.array(md.orientationRate.z)
+      self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
+      car_speed = np.linalg.norm(self.velocity_xyz, axis=1) - get_speed_error(md, v_ego_car)
+      self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
+      self.v_ego = self.v_plan[0]
+      self.plan_a = np.array(md.acceleration.x)
+      if md.velocity.x[-1] < md.velocity.x[0] * 0.7:  # TODO: 모델이 감속을 요청하는 경우 속도테이블이 레인모드를 할수 없음. 속도테이블을 새로 만들어야함..
+        self.lanemode_possible_count = 0
+        self.laneless_only = True
+      else:
+        self.lanemode_possible_count += 1
+        if self.lanemode_possible_count > int(1/DT_MDL):
+          self.laneless_only = False
+
+    # Parse model predictions
+    self.LP.parse_model(md)
+    #lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
+    #self.DH.update(sm['carState'], md, sm['carControl'].latActive, lane_change_prob, sm)
+
+    if self.useLaneLineSpeedApply == 0 or self.laneless_only:
+      self.useLaneLineMode = False
+    elif speed_kph >= self.useLaneLineSpeedApply + 2:
+      self.useLaneLineMode = True
+    elif speed_kph < self.useLaneLineSpeedApply - 2:
+      self.useLaneLineMode = False
+
+    # Turn off lanes during lane change
+    #if self.DH.desire == log.Desire.laneChangeRight or self.DH.desire == log.Desire.laneChangeLeft:
+
+    if md.meta.desire != log.Desire.none or carrot.atc_active:
+      self.LP.lane_change_multiplier = 0.0 #md.meta.laneChangeProb
+    else:
+      self.LP.lane_change_multiplier = 1.0
+
+    # lanelines calculation?
+    self.LP.lanefull_mode = self.useLaneLineMode
+    self.LP.lane_width_left = md.meta.laneWidthLeft
+    self.LP.lane_width_right = md.meta.laneWidthRight
+    self.LP.curvature = measured_curvature
+    self.path_xyz, self.lanelines_active = self.LP.get_d_path(sm['carState'], v_ego_car, self.t_idxs, self.path_xyz, self.curve_speed)
+
+    if self.lanelines_active:
+      self.plan_yaw, self.plan_yaw_rate = yaw_from_path_no_scipy(
+        self.path_xyz, self.v_plan,
+        smooth_window=5,
+        clip_rate=2.0,
+        align_first_yaw=None #md.orientation.z[0]  # 초기 정렬
+      )
+
+    self.latDebugText = self.LP.debugText
+    #self.lanelines_active = True if self.LP.d_prob > 0.3 and self.LP.lanefull_mode else False
+
+    self.path_xyz[:, 1] += self.pathOffset
+
+    self.lat_mpc.set_weights(self.lateralPathCost, self.lateralMotionCost,
+                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
+                             STEERING_RATE_COST)
+
+    y_pts = self.path_xyz[:LAT_MPC_N+1, 1]
+    heading_pts = self.plan_yaw[:LAT_MPC_N+1]
+    yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
+    self.y_pts = y_pts
+
+    assert len(y_pts) == LAT_MPC_N + 1
+    assert len(heading_pts) == LAT_MPC_N + 1
+    assert len(yaw_rate_pts) == LAT_MPC_N + 1
+    lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
+    p = np.column_stack([self.v_plan, lateral_factor])
+    self.lat_mpc.run(self.x0,
+                     p,
+                     y_pts,
+                     heading_pts,
+                     yaw_rate_pts)
+    # init state for next iteration
+    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
+    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
+    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
+    self.x0[3] = np.interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
+
+    #  Check for infeasible MPC solution
+    mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
+    t = time.monotonic()
+    if mpc_nans or self.lat_mpc.solution_status != 0:
+      self.reset_mpc()
+      self.x0[3] = measured_curvature * self.v_ego
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Lateral mpc - nan: True")
+
+    if self.lat_mpc.cost > 1e6 or mpc_nans:
+      self.solution_invalid_cnt += 1
+    else:
+      self.solution_invalid_cnt = 0
+
+    self.x_sol = self.lat_mpc.x_sol
+
+  def publish(self, sm, pm, carrot):
+    plan_solution_valid = self.solution_invalid_cnt < 2
+    plan_send = messaging.new_message('lateralPlan')
+    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
+    if not plan_send.valid:
+      #print("lateralPlan_valid=", sm.valid)
+      #print("lateralPlan_alive=", sm.alive)
+      #print("lateralPlan_freq_ok=", sm.freq_ok)
+      #print(sm.avg_freq)
+      pass
+
+    lateralPlan = plan_send.lateralPlan
+    lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    lateralPlan.dPathPoints = self.y_pts.tolist()
+    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
+    lateralPlan.distances = self.lat_mpc.x_sol[0:CONTROL_N, 0].tolist()
+
+    v_div = np.maximum(self.v_plan[:CONTROL_N], 6.0)
+    if len(self.v_plan) == TRAJECTORY_SIZE:
+      lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / v_div).tolist()
+    else:
+      lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3] / self.v_ego).tolist()
+
+    v_div2 = max(self.v_ego, 6.0)
+    lateralPlan.curvatureRates = [float(x.item() / v_div2) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+
+    lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
+    lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
+    if self.debug_mode:
+      lateralPlan.solverCost = self.lat_mpc.cost
+      lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
+      lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
+      lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
+
+    #lateralPlan.desire = self.DH.desire
+    lateralPlan.useLaneLines = self.lanelines_active
+    #lateralPlan.laneChangeState = self.DH.lane_change_state
+    #lateralPlan.laneChangeDirection = self.DH.lane_change_direction
+    lateralPlan.laneWidth = float(self.LP.lane_width)
+
+    #plan_send.lateralPlan.dPathWLinesX = [float(x) for x in self.d_path_w_lines_xyz[:, 0]]
+    #plan_send.lateralPlan.dPathWLinesY = [float(y) for y in self.d_path_w_lines_xyz[:, 1]]
+    #lateralPlan.laneWidthLeft = float(self.DH.lane_width_left)
+    #lateralPlan.laneWidthRight = float(self.DH.lane_width_right)
+
+    lateralPlan.position.x = self.x_sol[:, 0].tolist()
+    lateralPlan.position.y = self.x_sol[:, 1].tolist()
+    lateralPlan.position.z = self.path_xyz[:, 2].tolist()
+    #lateralPlan.distances = self.lat_mpc.x_sol[0:CONTROL_N, 0].tolist()
+
+    self.x_sol = self.lat_mpc.x_sol
+
+    debugText = (
+      f"{'lanemode' if self.lanelines_active else 'laneless'} | " +
+      f"{self.LP.lane_width_left:.1f}m | " +
+      f"{self.LP.lane_width:.1f}m | " +
+      f"{self.LP.lane_width_right:.1f}m | " +
+      f"{f'offset={self.LP.offset_total * 100.0:.1f}cm turn={np.clip(self.curve_speed, -200, 200):.0f}km/h' if self.lanelines_active else ''}"
+    )
+
+    lateralPlan.latDebugText = debugText
+    #lateralPlan.latDebugText = self.latDebugText
+    #lateralPlan.laneWidthLeft = float(self.DH.lane_width_left)
+    #lateralPlan.laneWidthRight = float(self.DH.lane_width_right)
+    #lateralPlan.distanceToRoadEdgeLeft = float(self.DH.distance_to_road_edge_left)
+    #lateralPlan.distanceToRoadEdgeRight = float(self.DH.distance_to_road_edge_right)
+
+    pm.send('lateralPlan', plan_send)
+
+
+def smooth_moving_avg(arr, window=5):
+  if window < 2:
+    return arr
+  if window % 2 == 0:
+    window += 1
+  pad = window // 2
+  arr_pad = np.pad(arr, (pad, pad), mode='edge')
+  kernel = np.ones(window) / window
+  return np.convolve(arr_pad, kernel, mode='same')[pad:-pad]
+
+def yaw_from_path_no_scipy(path_xyz, v_plan, smooth_window=5,
+                           clip_rate=2.0, align_first_yaw=None):
+
+  v0 = float(np.asarray(v_plan)[0]) if len(v_plan) else 0.0
+  # 저속(≤6 m/s)에서는 창을 크게
+  if v0 <= 6.0:
+    smooth_window = max(smooth_window, 9)   # 9~11 권장
+
+  N = path_xyz.shape[0]
+  x = path_xyz[:, 0].astype(float)
+  y = path_xyz[:, 1].astype(float)
+
+  if N < 5:
+    return np.zeros(N, np.float32), np.zeros(N, np.float32)
+
+  # 1) s(호길이) 계산
+  dx = np.diff(x)
+  dy = np.diff(y)
+  ds_seg = np.sqrt(dx*dx + dy*dy)
+  ds_seg[ds_seg < 0.05] = 0.05
+  s = np.zeros(N, float)
+  s[1:] = np.cumsum(ds_seg)
+  if s[-1] < 0.5:  # 총 호길이 < 0.5m면 미분 결과 의미가 약함
+    return np.zeros(N, np.float32), np.zeros(N, np.float32)
+
+  # 2) smoothing (이동평균)
+  x_smooth = smooth_moving_avg(x, smooth_window)
+  y_smooth = smooth_moving_avg(y, smooth_window)
+
+  # 3) 1·2차 도함수(s축 미분)
+  dx_ds  = np.gradient(x_smooth, s)
+  dy_ds  = np.gradient(y_smooth, s)
+  d2x_ds2 = np.gradient(dx_ds, s)
+  d2y_ds2 = np.gradient(dy_ds, s)
+
+  # 4) yaw = atan2(dy/ds, dx/ds)
+  yaw = np.unwrap(np.arctan2(dy_ds, dx_ds))
+
+  # 5) 곡률 kappa = ...
+  denom = (dx_ds*dx_ds + dy_ds*dy_ds)**1.5
+  denom[denom < 1e-9] = 1e-9
+  kappa = (dx_ds * d2y_ds2 - dy_ds * d2x_ds2) / denom
+
+  # 6) yaw_rate = kappa * v
+  v = np.asarray(v_plan, float)
+  yaw_rate = kappa * v
+  if v0 <= 6.0:
+    # 이동평균으로 미세 요동 감쇄(창 5~7)
+    yaw_rate = smooth_moving_avg(yaw_rate, window=7)
+
+  # 7) 초기 yaw 정렬 (선택)
+  if align_first_yaw is not None:
+    bias = yaw[0] - float(align_first_yaw)
+    yaw = yaw - bias
+
+  # 8) 안정화
+  yaw     = np.where(np.isfinite(yaw), yaw, 0.0)
+  yaw_rate = np.where(np.isfinite(yaw_rate), yaw_rate, 0.0)
+  yaw_rate = np.clip(yaw_rate, -abs(clip_rate), abs(clip_rate))
+
+  return yaw.astype(np.float32), yaw_rate.astype(np.float32)

selfdrive/controls/lib/ldw.py

@@ -0,0 +1,41 @@
+from cereal import log
+from openpilot.common.realtime import DT_CTRL
+from openpilot.common.conversions import Conversions as 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,136 @@
+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.common.params import Params
+
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
+
+def long_control_state_trans(CP, active, long_control_state, v_ego,
+                             should_stop, brake_pressed, cruise_standstill, a_ego, stopping_accel, radarState):
+  stopping_condition = should_stop
+  starting_condition = (not should_stop and
+                        not cruise_standstill and
+                        not brake_pressed)
+  started_condition = v_ego > CP.vEgoStarting
+
+  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:
+        if starting_condition and CP.startingState:
+          long_control_state = LongCtrlState.starting
+        else:
+          long_control_state = LongCtrlState.pid
+
+    elif long_control_state == LongCtrlState.stopping:
+      if starting_condition and CP.startingState:
+        long_control_state = LongCtrlState.starting
+      elif starting_condition:
+        long_control_state = LongCtrlState.pid
+
+    elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
+      if stopping_condition:
+        stopping_accel = stopping_accel if stopping_accel < 0.0 else -0.5
+        leadOne = radarState.leadOne
+        fcw_stop = leadOne.status and leadOne.dRel < 4.0
+        if a_ego > stopping_accel or fcw_stop: # and v_ego < 1.0:
+          long_control_state = LongCtrlState.stopping
+        if long_control_state == LongCtrlState.starting:
+          long_control_state = LongCtrlState.stopping
+      elif started_condition:
+        long_control_state = LongCtrlState.pid
+  return long_control_state
+
+class LongControl:
+  def __init__(self, CP):
+    self.CP = CP
+    self.long_control_state = LongCtrlState.off
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL)
+    self.last_output_accel = 0.0
+
+
+    self.params = Params()
+    self.readParamCount = 0
+    self.stopping_accel = 0
+    self.j_lead = 0.0
+
+    self.use_accel_pid = False
+    if CP.brand == "toyota":
+      self.use_accel_pid = True
+
+  def reset(self):
+    self.pid.reset()
+
+  def update(self, active, CS, long_plan, accel_limits, t_since_plan, radarState):
+
+    soft_hold_active = CS.softHoldActive > 0
+    a_target_ff = long_plan.aTarget
+    v_target_now = long_plan.vTargetNow
+    j_target_now = long_plan.jTargetNow
+    should_stop = long_plan.shouldStop
+
+    self.readParamCount += 1
+    if self.readParamCount >= 100:
+      self.readParamCount = 0
+      self.stopping_accel = self.params.get_float("StoppingAccel") * 0.01
+    elif self.readParamCount == 10:
+      if len(self.CP.longitudinalTuning.kpBP) == 1 and len(self.CP.longitudinalTuning.kiBP)==1:
+        longitudinalTuningKpV = self.params.get_float("LongTuningKpV") * 0.01
+        longitudinalTuningKiV = self.params.get_float("LongTuningKiV") * 0.001
+        self.pid._k_p = (self.CP.longitudinalTuning.kpBP, [longitudinalTuningKpV])
+        self.pid._k_i = (self.CP.longitudinalTuning.kiBP, [longitudinalTuningKiV])
+        self.pid._k_f = self.params.get_float("LongTuningKf") * 0.01
+
+
+    """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.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
+                                                       should_stop, CS.brakePressed,
+                                                       CS.cruiseState.standstill, CS.aEgo, self.stopping_accel, radarState)
+    if active and soft_hold_active:
+      self.long_control_state = LongCtrlState.stopping
+
+    if self.long_control_state == LongCtrlState.off:
+      self.reset()
+      output_accel = 0.
+
+    elif self.long_control_state == LongCtrlState.stopping:
+      output_accel = self.last_output_accel
+
+      if soft_hold_active:
+        output_accel = self.CP.stopAccel
+
+      stopAccel = self.stopping_accel if self.stopping_accel < 0.0 else self.CP.stopAccel
+      if output_accel > stopAccel:
+        output_accel = min(output_accel, 0.0)
+        output_accel -= self.CP.stoppingDecelRate * DT_CTRL
+      self.reset()
+
+    elif self.long_control_state == LongCtrlState.starting:
+      output_accel = self.CP.startAccel
+      self.reset()
+
+    else:  # LongCtrlState.pid
+      if self.use_accel_pid:
+        error = a_target_ff - CS.aEgo
+      else:
+        error = v_target_now - CS.vEgo
+      output_accel = self.pid.update(error, speed=CS.vEgo,
+                                     feedforward=a_target_ff)
+
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
+    return self.last_output_accel, a_target_ff, j_target_now

selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -0,0 +1,526 @@
+{
+    "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,
+                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,
+                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,
+                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,
+            0.0
+        ],
+        "yref_0": [
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
+        ],
+        "yref_e": [
+            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": []
+    },
+    "cython_include_dirs": [
+        "/usr/local/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": 8,
+        "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": 6,
+        "ny_0": 6,
+        "ny_e": 5,
+        "nz": 0
+    },
+    "json_file": "/data/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
+    "model": {
+        "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgcaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfgegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
+        "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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgcaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfgegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachdbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgcaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfgegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachdbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegdaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgcaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfgegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachdbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbg",
+        "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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegmaaaaaaaaaaaaaaaiaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaiaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaahaaaaaaaaaaaaaaaiaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaaahchfgghpfbgegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfgegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfg",
+        "u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
+        "xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
+        "z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
+    },
+    "parameter_values": [
+        -1.2,
+        1.2,
+        0.0,
+        0.0,
+        "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghh",
+        0.8,
+        "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadgpgngggpgchehpfcgchbglgfg",
+        "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaadhehpgahpfegjgdhehbgogdgfg"
+    ],
+    "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 /usr/local/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 /usr/local/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/c_generated_code/acados_sim_solver_long.h

@@ -0,0 +1,101 @@
+/*
+ * 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.;
+ */
+
+#ifndef ACADOS_SIM_long_H_
+#define ACADOS_SIM_long_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LONG_NX     3
+#define LONG_NZ     0
+#define LONG_NU     1
+#define LONG_NP     8
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_vde_adj_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+ACADOS_SYMBOL_EXPORT int long_acados_sim_create(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
+ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_long_H_

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver.pxd

@@ -0,0 +1,62 @@
+#
+# 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.;
+#
+
+cimport acados_solver_common
+
+cdef extern from "acados_solver_long.h":
+    ctypedef struct nlp_solver_capsule "long_solver_capsule":
+        pass
+
+    nlp_solver_capsule * acados_create_capsule "long_acados_create_capsule"()
+    int acados_free_capsule "long_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+    int acados_create "long_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "long_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "long_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "long_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
+    int acados_update_params "long_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
+    int acados_update_params_sparse "long_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
+    int acados_solve "long_acados_solve"(nlp_solver_capsule * capsule)
+    int acados_reset "long_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
+    int acados_free "long_acados_free"(nlp_solver_capsule * capsule)
+    void acados_print_stats "long_acados_print_stats"(nlp_solver_capsule * capsule)
+
+    int acados_custom_update "long_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
+
+    acados_solver_common.ocp_nlp_in *acados_get_nlp_in "long_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_nlp_out "long_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "long_acados_get_sens_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "long_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_config *acados_get_nlp_config "long_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+    void *acados_get_nlp_opts "long_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "long_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "long_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver_long.h

@@ -0,0 +1,172 @@
+/*
+ * 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.;
+ */
+
+#ifndef ACADOS_SOLVER_long_H_
+#define ACADOS_SOLVER_long_H_
+
+#include "acados/utils/types.h"
+
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#define LONG_NX     3
+#define LONG_NZ     0
+#define LONG_NU     1
+#define LONG_NP     8
+#define LONG_NBX    0
+#define LONG_NBX0   3
+#define LONG_NBU    0
+#define LONG_NSBX   0
+#define LONG_NSBU   0
+#define LONG_NSH    4
+#define LONG_NSG    0
+#define LONG_NSPHI  0
+#define LONG_NSHN   0
+#define LONG_NSGN   0
+#define LONG_NSPHIN 0
+#define LONG_NSBXN  0
+#define LONG_NS     4
+#define LONG_NSN    0
+#define LONG_NG     0
+#define LONG_NBXN   0
+#define LONG_NGN    0
+#define LONG_NY0    6
+#define LONG_NY     6
+#define LONG_NYN    5
+#define LONG_N      12
+#define LONG_NH     4
+#define LONG_NPHI   0
+#define LONG_NHN    0
+#define LONG_NPHIN  0
+#define LONG_NR     0
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct long_solver_capsule
+{
+    // acados objects
+    ocp_nlp_in *nlp_in;
+    ocp_nlp_out *nlp_out;
+    ocp_nlp_out *sens_out;
+    ocp_nlp_solver *nlp_solver;
+    void *nlp_opts;
+    ocp_nlp_plan_t *nlp_solver_plan;
+    ocp_nlp_config *nlp_config;
+    ocp_nlp_dims *nlp_dims;
+
+    // number of expected runtime parameters
+    unsigned int nlp_np;
+
+    /* external functions */
+    // dynamics
+
+    external_function_param_casadi *forw_vde_casadi;
+    external_function_param_casadi *expl_ode_fun;
+
+
+
+
+    // cost
+
+    external_function_param_casadi *cost_y_fun;
+    external_function_param_casadi *cost_y_fun_jac_ut_xt;
+    external_function_param_casadi *cost_y_hess;
+
+
+
+    external_function_param_casadi cost_y_0_fun;
+    external_function_param_casadi cost_y_0_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_0_hess;
+
+
+
+    external_function_param_casadi cost_y_e_fun;
+    external_function_param_casadi cost_y_e_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_e_hess;
+
+
+    // constraints
+    external_function_param_casadi *nl_constr_h_fun_jac;
+    external_function_param_casadi *nl_constr_h_fun;
+
+
+
+
+} long_solver_capsule;
+
+ACADOS_SYMBOL_EXPORT long_solver_capsule * long_acados_create_capsule(void);
+ACADOS_SYMBOL_EXPORT int long_acados_free_capsule(long_solver_capsule *capsule);
+
+ACADOS_SYMBOL_EXPORT int long_acados_create(long_solver_capsule * capsule);
+
+ACADOS_SYMBOL_EXPORT int long_acados_reset(long_solver_capsule* capsule, int reset_qp_solver_mem);
+
+/**
+ * Generic version of long_acados_create which allows to use a different number of shooting intervals than
+ * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
+ * generation, the time-steps from code generation is used.
+ */
+ACADOS_SYMBOL_EXPORT int long_acados_create_with_discretization(long_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
+/**
+ * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
+ * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
+ */
+ACADOS_SYMBOL_EXPORT int long_acados_update_time_steps(long_solver_capsule * capsule, int N, double* new_time_steps);
+/**
+ * This function is used for updating an already initialized solver with a different number of qp_cond_N.
+ */
+ACADOS_SYMBOL_EXPORT int long_acados_update_qp_solver_cond_N(long_solver_capsule * capsule, int qp_solver_cond_N);
+ACADOS_SYMBOL_EXPORT int long_acados_update_params(long_solver_capsule * capsule, int stage, double *value, int np);
+ACADOS_SYMBOL_EXPORT int long_acados_update_params_sparse(long_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
+
+ACADOS_SYMBOL_EXPORT int long_acados_solve(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_free(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT void long_acados_print_stats(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT int long_acados_custom_update(long_solver_capsule* capsule, double* data, int data_len);
+
+
+ACADOS_SYMBOL_EXPORT ocp_nlp_in *long_acados_get_nlp_in(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_nlp_out(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_out *long_acados_get_sens_out(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_solver *long_acados_get_nlp_solver(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_config *long_acados_get_nlp_config(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT void *long_acados_get_nlp_opts(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_dims *long_acados_get_nlp_dims(long_solver_capsule * capsule);
+ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *long_acados_get_nlp_plan(long_solver_capsule * capsule);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SOLVER_long_H_

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constraints.h

@@ -0,0 +1,66 @@
+/*
+ * 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.;
+ */
+
+#ifndef long_CONSTRAINTS
+#define long_CONSTRAINTS
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+
+
+
+int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
+const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
+const int *long_constr_h_fun_jac_uxt_zt_sparsity_out(int);
+int long_constr_h_fun_jac_uxt_zt_n_in(void);
+int long_constr_h_fun_jac_uxt_zt_n_out(void);
+
+int long_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_constr_h_fun_work(int *, int *, int *, int *);
+const int *long_constr_h_fun_sparsity_in(int);
+const int *long_constr_h_fun_sparsity_out(int);
+int long_constr_h_fun_n_in(void);
+int long_constr_h_fun_n_out(void);
+
+
+
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // long_CONSTRAINTS

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost.h

@@ -0,0 +1,119 @@
+/*
+ * 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.;
+ */
+
+
+#ifndef long_COST
+#define long_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// Cost at initial shooting node
+
+int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_0_fun_work(int *, int *, int *, int *);
+const int *long_cost_y_0_fun_sparsity_in(int);
+const int *long_cost_y_0_fun_sparsity_out(int);
+int long_cost_y_0_fun_n_in(void);
+int long_cost_y_0_fun_n_out(void);
+
+int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
+const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
+int long_cost_y_0_fun_jac_ut_xt_n_in(void);
+int long_cost_y_0_fun_jac_ut_xt_n_out(void);
+
+int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_0_hess_work(int *, int *, int *, int *);
+const int *long_cost_y_0_hess_sparsity_in(int);
+const int *long_cost_y_0_hess_sparsity_out(int);
+int long_cost_y_0_hess_n_in(void);
+int long_cost_y_0_hess_n_out(void);
+
+
+
+// Cost at path shooting node
+
+int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_fun_work(int *, int *, int *, int *);
+const int *long_cost_y_fun_sparsity_in(int);
+const int *long_cost_y_fun_sparsity_out(int);
+int long_cost_y_fun_n_in(void);
+int long_cost_y_fun_n_out(void);
+
+int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
+const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
+int long_cost_y_fun_jac_ut_xt_n_in(void);
+int long_cost_y_fun_jac_ut_xt_n_out(void);
+
+int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_hess_work(int *, int *, int *, int *);
+const int *long_cost_y_hess_sparsity_in(int);
+const int *long_cost_y_hess_sparsity_out(int);
+int long_cost_y_hess_n_in(void);
+int long_cost_y_hess_n_out(void);
+
+
+
+// Cost at terminal shooting node
+
+int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_e_fun_work(int *, int *, int *, int *);
+const int *long_cost_y_e_fun_sparsity_in(int);
+const int *long_cost_y_e_fun_sparsity_out(int);
+int long_cost_y_e_fun_n_in(void);
+int long_cost_y_e_fun_n_out(void);
+
+int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
+const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
+int long_cost_y_e_fun_jac_ut_xt_n_in(void);
+int long_cost_y_e_fun_jac_ut_xt_n_out(void);
+
+int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_cost_y_e_hess_work(int *, int *, int *, int *);
+const int *long_cost_y_e_hess_sparsity_in(int);
+const int *long_cost_y_e_hess_sparsity_out(int);
+int long_cost_y_e_hess_n_in(void);
+int long_cost_y_e_hess_n_out(void);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // long_COST

selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_model.h

@@ -0,0 +1,71 @@
+/*
+ * 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.;
+ */
+
+#ifndef long_MODEL
+#define long_MODEL
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* explicit ODE */
+
+// explicit ODE
+int long_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_expl_ode_fun_work(int *, int *, int *, int *);
+const int *long_expl_ode_fun_sparsity_in(int);
+const int *long_expl_ode_fun_sparsity_out(int);
+int long_expl_ode_fun_n_in(void);
+int long_expl_ode_fun_n_out(void);
+
+// explicit forward VDE
+int long_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_expl_vde_forw_work(int *, int *, int *, int *);
+const int *long_expl_vde_forw_sparsity_in(int);
+const int *long_expl_vde_forw_sparsity_out(int);
+int long_expl_vde_forw_n_in(void);
+int long_expl_vde_forw_n_out(void);
+
+// explicit adjoint VDE
+int long_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int long_expl_vde_adj_work(int *, int *, int *, int *);
+const int *long_expl_vde_adj_sparsity_in(int);
+const int *long_expl_vde_adj_sparsity_out(int);
+int long_expl_vde_adj_n_in(void);
+int long_expl_vde_adj_n_out(void);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // long_MODEL

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,535 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from cereal import log
+from opendbc.car.interfaces import ACCEL_MIN
+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
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+# from openpilot.selfdrive.carrot.carrot_functions import CarrotPlanner
+from openpilot.selfdrive.carrot.carrot_functions import XState
+
+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")
+
+SOURCES = ['lead0', 'lead1', 'cruise', 'e2e']
+
+X_DIM = 3
+U_DIM = 1
+PARAM_DIM = 8
+COST_E_DIM = 5
+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 = 5.0
+A_CHANGE_COST = 250.
+A_CHANGE_COST_STARTING = 30.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .25
+LEAD_DANGER_FACTOR = 0.8 # 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 = 6.0
+
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.moreRelaxed:
+    return 1.0
+  elif 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.moreRelaxed:
+    return 2.0
+  elif 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=None, comfort_brake=COMFORT_BRAKE, stop_distance=STOP_DISTANCE):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return (v_ego**2) / (2 * comfort_brake) + t_follow * v_ego + stop_distance
+
+def desired_follow_distance(v_ego, v_lead, comfort_brake, stop_distance, t_follow=None):
+  if t_follow is None:
+    t_follow = get_T_FOLLOW()
+  return get_safe_obstacle_distance(v_ego, t_follow, comfort_brake, stop_distance) - get_stopped_equivalence_factor(v_lead)
+
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  v_ego = SX.sym('v_ego')
+  a_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')
+  prev_a = SX.sym('prev_a')
+  lead_t_follow = SX.sym('lead_t_follow')
+  lead_danger_factor = SX.sym('lead_danger_factor')
+  comfort_brake = SX.sym('comfort_brake')
+  stop_distance = SX.sym('stop_distance')
+  model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor, comfort_brake, stop_distance)
+
+  # 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]
+  prev_a = ocp.model.p[3]
+  lead_t_follow = ocp.model.p[4]
+  lead_danger_factor = ocp.model.p[5]
+  comfort_brake = ocp.model.p[6]
+  stop_distance = ocp.model.p[7]
+
+  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, comfort_brake, stop_distance)
+
+  # 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,
+           a_ego - prev_a,
+           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, 0.0, lead_t_follow, LEAD_DANGER_FACTOR, comfort_brake, stop_distance])
+
+
+  # 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, mode='acc', dt=DT_MDL):
+    self.mode = mode
+    self.dt = dt
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+
+    self.a_change_cost = A_CHANGE_COST
+    self.j_lead = 0.0
+
+    self.reset()
+    self.source = SOURCES[2]
+
+    self.t_follow = 1.0
+    self.desired_distance = 0.0
+    self.lead_danger_factor = LEAD_DANGER_FACTOR
+
+
+  def reset(self):
+    # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.solver.reset()
+    # self.solver.options_set('print_level', 2)
+    self.v_solution = np.zeros(N+1)
+    self.a_solution = np.zeros(N+1)
+    self.prev_a = np.array(self.a_solution)
+    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.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N,1))
+    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.set_weights()
+
+  def set_cost_weights(self, cost_weights, constraint_cost_weights):
+    W = np.asfortranarray(np.diag(cost_weights))
+    for i in range(N):
+      # TODO don't hardcode A_CHANGE_COST idx
+      # reduce the cost on (a-a_prev) later in the horizon.
+      W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
+      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, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_factor=1.0, a_change_cost_starting=A_CHANGE_COST_STARTING):
+    #jerk_factor = get_jerk_factor(personality)
+    if self.mode == 'acc':
+      a_change_cost = self.a_change_cost if prev_accel_constraint else a_change_cost_starting
+      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    elif self.mode == 'blended':
+      a_change_cost = 40.0 if prev_accel_constraint else 0
+      cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
+      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
+    else:
+      raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
+    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, j_lead):
+    j_lead_tau = np.interp(j_lead, [-2.0, 0.0, 2.0], [0.2, 2.0, 0.1]) # tau: 2: 2sec, 1: 4sec, 0.5: 10sec
+    j_lead_traj = j_lead * np.exp(-j_lead_tau * (T_IDXS**2)/2.)
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.) + j_lead_traj
+    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(self, lead, j_lead):
+    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 = ((v_ego + v_lead)/2) * (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.)
+
+    if a_lead < -2.0 and j_lead > 0.5:
+      a_lead = a_lead + j_lead
+      a_lead = min(a_lead, -0.5)
+      a_lead_tau = max(a_lead_tau, 1.5)
+
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, j_lead)
+    return lead_xv, v_lead
+
+  def set_accel_limits(self, min_a, max_a):
+    # TODO this sets a max accel limit, but the minimum limit is only for cruise decel
+    # needs refactor
+    self.cruise_min_a = min_a
+    self.max_a = max_a
+
+  def update(self, carrot, reset_state, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
+    t_follow = carrot.get_T_FOLLOW(personality)
+    v_ego = self.x0[1]
+    a_ego = self.x0[2]
+    self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+
+    if radarstate.leadOne.status:
+      j_lead = radarstate.leadOne.jLead
+      self.j_lead = j_lead * 0.1 + self.j_lead * 0.9
+    else:
+      self.j_lead = 0.0
+
+    lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne, np.clip(self.j_lead * carrot.j_lead_factor, -1.0, 1.0))
+    lead_xv_1, _ = self.process_lead(radarstate.leadTwo, 0.0)
+
+    mode = self.mode
+    comfort_brake = carrot.comfort_brake
+    stop_distance = carrot.stop_distance
+
+    if mode == 'blended':
+      stop_x = 1000.0
+    else:
+      v_cruise, stop_x, mode = carrot.v_cruise, carrot.stop_dist, carrot.mode
+      desired_distance = desired_follow_distance(v_ego, lead_v_0, comfort_brake, stop_distance, t_follow)
+      t_follow = carrot.dynamic_t_follow(t_follow, radarstate.leadOne, desired_distance, self.prev_a)
+
+    # 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])
+
+    self.desired_distance = desired_follow_distance(v_ego, lead_v_0, comfort_brake, stop_distance, t_follow)
+
+    self.params[:,0] = ACCEL_MIN if not reset_state else a_ego
+    # negative accel constraint causes problems because negative speed is not allowed
+    self.params[:,1] = max(0.0, self.max_a if not reset_state else a_ego)
+
+    # Update in ACC mode or ACC/e2e blend
+    if mode == 'acc':
+      #self.params[:,5] = LEAD_DANGER_FACTOR
+      # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
+      # when the leads are no factor.
+      v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+      # TODO does this make sense when max_a is negative?
+      v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
+      v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
+                                 v_lower,
+                                 v_upper)
+      cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow, comfort_brake, stop_distance)
+
+      adjust_dist = carrot.trafficStopDistanceAdjust if v_ego > 0.1 else -2.0
+      if 50 < stop_x + adjust_dist < cruise_obstacle[0]:
+        stop_x = cruise_obstacle[0] - adjust_dist
+      x2 = stop_x * np.ones(N+1) + adjust_dist
+
+      x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle, x2])
+      self.source = SOURCES[np.argmin(x_obstacles[0])]
+
+      if v_cruise == 0 and self.source == 'cruise':
+        self.params[:,0] = - carrot.autoNaviSpeedDecelRate
+      #elif self.source in ['cruise', 'e2e']:
+      #  self.params[:,0] = - COMFORT_BRAKE
+
+      # These are not used in ACC mode
+      x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
+
+      if radarstate.leadOne.status:
+        self.a_change_cost = np.interp(abs(self.j_lead), [0.3, 2.0], [A_CHANGE_COST, 20])
+      else:
+        self.a_change_cost = A_CHANGE_COST
+
+      #safe_distance = lead_0_obstacle[0] - get_safe_obstacle_distance(v_ego, comfort_brake, stop_distance)
+      self.lead_danger_factor = LEAD_DANGER_FACTOR #np.interp(safe_distance, [-30.0, 0.0], [0.9, LEAD_DANGER_FACTOR]) # 鞚搓备鞝侅毄頃橂媹, 靷碃氚╈韯± 臧愳啀鞁œ 雱堧 旮夓爼瓯绊晿電旉矁 臧欖潓.
+      self.params[:,5] = self.lead_danger_factor
+
+    elif mode == 'blended':
+      self.params[:,5] = 1.0
+
+      x_obstacles = np.column_stack([lead_0_obstacle,
+                                     lead_1_obstacle])
+      cruise_target = T_IDXS * np.clip(v_cruise, v_ego - 2.0, 1e3) + x[0]
+      xforward = ((v[1:] + v[:-1]) / 2) * (T_IDXS[1:] - T_IDXS[:-1])
+      x = np.cumsum(np.insert(xforward, 0, x[0]))
+
+      x_and_cruise = np.column_stack([x, cruise_target])
+      x = np.min(x_and_cruise, axis=1)
+
+      self.source = 'e2e' if x_and_cruise[1,0] < x_and_cruise[1,1] else 'cruise'
+
+    else:
+      raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner update')
+
+    self.yref[:,1] = x
+    self.yref[:,2] = v
+    self.yref[:,3] = a
+    self.yref[:,5] = j
+    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[:,2] = np.min(x_obstacles, axis=1)
+    self.params[:,3] = np.copy(self.prev_a)
+    self.params[:,4] = t_follow
+    self.params[:,6] = comfort_brake
+    self.params[:,7] = stop_distance
+
+    self.t_follow = t_follow
+
+    self.run()
+    if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
+            radarstate.leadOne.modelProb > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
+
+    # Check if it got within lead comfort range
+    # TODO This should be done cleaner
+    if self.mode == 'blended':
+      if any((lead_0_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow, comfort_brake, stop_distance))- self.x_sol[:,0] < 0.0):
+        self.source = 'lead0'
+      if any((lead_1_obstacle - get_safe_obstacle_distance(self.x_sol[:,1], t_follow, comfort_brake, stop_distance))- self.x_sol[:,0] < 0.0) and \
+         (lead_1_obstacle[0] - lead_0_obstacle[0]):
+        self.source = 'lead1'
+
+  def run(self):
+    # t0 = time.monotonic()
+    # reset = 0
+    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])
+
+    # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
+    # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, \
+    # integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
+    # res = self.solver.get_residuals()
+    # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
+    # self.solver.print_statistics()
+
+    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]
+
+    self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
+
+    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()
+      # reset = 1
+    # print(f"long_mpc timings: total internal {self.solve_time:.2e}, external: {(time.monotonic() - t0):.2e} qp {self.time_qp_solution:.2e}, \
+    # lin {self.time_linearization:.2e} qp_iter {qp_iter}, reset {reset}")
+
+
+if __name__ == "__main__":
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)

selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,283 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+
+import cereal.messaging as messaging
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.common.conversions import Conversions as CV
+from openpilot.common.filter_simple import FirstOrderFilter
+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, N
+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, get_speed_error, 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.params import Params
+
+
+LON_MPC_STEP = 0.2  # first step is 0.2s
+A_CRUISE_MIN = -2.0 #-1.2
+A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6]
+A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+ALLOW_THROTTLE_THRESHOLD = 0.5
+MIN_ALLOW_THROTTLE_SPEED = 2.5
+
+# Lookup table for turns
+_A_TOTAL_MAX_V = [2.4, 4.8] #[1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
+LAT_WEIGHT = 0.7
+
+
+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 limit_accel_in_turns_org(v_ego, angle_steers, a_target, CP):
+  """
+  This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
+  this should avoid accelerating when losing the target in turns
+  """
+  # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
+  # The lookup table for turns should also be updated if we do this
+  steer_abs = abs(angle_steers)
+  if v_ego > 20 or (v_ego > 25 and steer_abs < 3.0):
+    return a_target
+  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) * LAT_WEIGHT
+  a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+
+  return [a_target[0], min(a_target[1], a_x_allowed)]
+
+def limit_accel_in_turns(v_ego, curvature, a_target, a_lat_max):
+  """
+  v_ego    : m/s
+  curvature: 1/m  (조향에서 이미 나온 곡률, sign 포함)
+  a_target : [a_min, a_max]
+  a_lat_max: 허용 최대 횡가속 (예: 6.0 ~ 8.0 m/s^2)
+
+  return   : [a_min, 제한된 a_max]
+  """
+  # 아주 저속이면 굳이 제한 안 걸어도 됨
+  if v_ego < 0.1 or a_lat_max <= 0.0:
+    return a_target
+
+  # 횡가속
+  a_y = v_ego * v_ego * curvature        # m/s^2
+  a_y_abs = abs(a_y)
+  a_lat_max_abs = abs(a_lat_max)
+
+  # a_total^2 = a_x^2 + a_y^2 <= a_lat_max^2 라고 보고,
+  # 남은 a_x 여유를 계산
+  if a_y_abs >= a_lat_max_abs:
+    a_x_allowed = 0.0
+  else:
+    a_x_allowed = math.sqrt(a_lat_max_abs**2 - a_y_abs**2)
+
+  # a_target = [min, max] 중에서 max만 줄여줌
+  return [a_target[0], min(a_target[1], a_x_allowed)]
+
+class LongitudinalPlanner:
+  def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL):
+    self.CP = CP
+    self.mpc = LongitudinalMpc(dt=dt)
+    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.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
+    self.output_a_target = 0.0
+    self.output_v_target_now = 0.0
+    self.output_j_target_now = 0.0
+    self.output_should_stop = 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)
+    self.solverExecutionTime = 0.0
+
+    self.vCluRatio = 1.0
+
+    self.v_cruise_kph = 0.0
+
+    self.params = Params()
+
+  @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, carrot):
+    self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
+
+    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)
+
+    self.v_cruise_kph = carrot.update(sm, v_cruise_kph, self.mpc.mode)
+    self.mpc.mode = carrot.mode
+    v_cruise = self.v_cruise_kph * CV.KPH_TO_MS
+
+    vCluRatio = sm['carState'].vCluRatio
+    if vCluRatio > 0.5:
+      self.vCluRatio = vCluRatio
+      v_cruise *= vCluRatio
+
+    v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
+
+    long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
+    force_slow_decel = sm['controlsState'].forceDecel
+
+    # 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
+    reset_state = reset_state or not v_cruise_initialized or carrot.soft_hold_active
+
+    # No change cost when user is controlling the speed, or when standstill
+    prev_accel_constraint = not (reset_state or sm['carState'].standstill)
+
+    if self.mpc.mode == 'acc':
+      #accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+      accel_limits = [A_CRUISE_MIN, carrot.get_carrot_accel(v_ego)]
+      steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
+      #accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP)
+      a_lat_max = 4.0
+      accel_limits_turns = limit_accel_in_turns(v_ego, sm['controlsState'].desiredCurvature, accel_limits, a_lat_max)
+    else:
+      accel_limits = [ACCEL_MIN, ACCEL_MAX]
+      accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
+
+    if reset_state:
+      self.v_desired_filter.x = v_ego
+      # Clip aEgo to cruise limits to prevent large accelerations when becoming active
+      self.a_desired = np.clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
+      
+      self.mpc.prev_a = np.full(N+1, self.a_desired) ## carrot
+      accel_limits_turns[0] = accel_limits_turns[0] = 0.0 ## carrot
+
+    # Prevent divergence, smooth in current v_ego
+    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
+    x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])
+    # Don't clip at low speeds since throttle_prob doesn't account for creep
+    if self.params.get_int("CommaLongAcc") > 0:
+      self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+    else:
+      self.allow_throttle = True
+
+    if not self.allow_throttle:
+      clipped_accel_coast = max(accel_coast, accel_limits_turns[0])
+      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast])
+      accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp)
+
+    if force_slow_decel:
+      v_cruise = 0.0
+    # clip limits, cannot init MPC outside of bounds
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
+    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
+
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply, a_change_cost_starting = carrot.aChangeCostStarting)
+    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(carrot, reset_state, sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].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 and not reset_state
+    if self.fcw:
+      cloudlog.info("FCW triggered")
+
+    # Interpolate 0.05 seconds and save as starting point for next iteration
+    a_prev = self.a_desired
+    self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
+    self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
+
+    longitudinalActuatorDelay = self.params.get_float("LongActuatorDelay")*0.01
+    vEgoStopping = self.params.get_float("VEgoStopping") * 0.01
+    action_t =  longitudinalActuatorDelay + DT_MDL
+
+    output_a_target_mpc, output_should_stop_mpc, output_v_target_mpc, _ = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                                        action_t=action_t, vEgoStopping=vEgoStopping)
+    output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+    output_should_stop_e2e = sm['modelV2'].action.shouldStop
+    output_v_target_now_e2e = sm['modelV2'].action.desiredVelocity
+
+    if self.mpc.mode == 'acc':
+      output_a_target = output_a_target_mpc
+      output_v_target_now = output_v_target_mpc
+      self.output_should_stop = output_should_stop_mpc
+    else:
+      output_a_target = min(output_a_target_mpc, output_a_target_e2e)
+      output_v_target_now = min(output_v_target_mpc, output_v_target_now_e2e)
+      self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+
+    #for idx in range(2):
+    #  accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
+    #self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
+    #self.prev_accel_clip = accel_clip
+    self.output_a_target = output_a_target
+    self.output_v_target_now = output_v_target_now
+    self.output_j_target_now = self.j_desired_trajectory[0]
+
+  def publish(self, sm, pm, carrot):
+    plan_send = messaging.new_message('longitudinalPlan')
+
+    plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'selfdriveState'])
+
+    longitudinalPlan = plan_send.longitudinalPlan
+    longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
+    longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
+    longitudinalPlan.solverExecutionTime = self.mpc.solve_time
+
+    longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
+    longitudinalPlan.accels = self.a_desired_trajectory.tolist()
+    longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
+
+    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    longitudinalPlan.fcw = self.fcw
+
+    longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.vTargetNow = float(self.output_v_target_now)
+    longitudinalPlan.jTargetNow = float(self.output_j_target_now)
+    longitudinalPlan.shouldStop = bool(self.output_should_stop)
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
+
+    longitudinalPlan.xState = carrot.xState.value
+    longitudinalPlan.trafficState = carrot.trafficState.value
+    longitudinalPlan.cruiseTarget = self.v_cruise_kph
+    longitudinalPlan.tFollow = float(self.mpc.t_follow)
+    longitudinalPlan.desiredDistance = float(self.mpc.desired_distance)
+    longitudinalPlan.events = carrot.events.to_msg()
+    longitudinalPlan.myDrivingMode = carrot.myDrivingMode.value
+
+    pm.send('longitudinalPlan', plan_send)

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -0,0 +1,47 @@
+from parameterized import parameterized
+
+from cereal import car, log
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.nissan.values import CAR as NISSAN
+from opendbc.car.vehicle_model import VehicleModel
+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
+
+
+class TestLatControl:
+
+  @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque),  (NISSAN.NISSAN_LEAF, LatControlAngle)])
+  def test_saturation(self, car_name, controller):
+    CarInterface, CarController, CarState, RadarInterface = interfaces[car_name]
+    CP = CarInterface.get_non_essential_params(car_name)
+    CI = CarInterface(CP, CarController, CarState)
+    VM = VehicleModel(CP)
+
+    controller = controller(CP.as_reader(), CI)
+
+    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)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False)
+    assert lac_log.saturated

selfdrive/controls/plannerd.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+from cereal import car
+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
+from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner
+import cereal.messaging as messaging
+from openpilot.selfdrive.carrot.carrot_functions import CarrotPlanner
+
+
+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)
+
+  ldw = LaneDepartureWarning()
+  longitudinal_planner = LongitudinalPlanner(CP)
+  lateral_planner = LateralPlanner(CP, debug=False)
+
+  pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'lateralPlan'])
+  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState', 'carrotMan'],
+                           poll='modelV2', ignore_avg_freq=['radarState'])
+  carrot = CarrotPlanner()
+
+  while True:
+    sm.update()
+    if sm.updated['modelV2']:
+      lateral_planner.update(sm, carrot)
+      lateral_planner.publish(sm, pm, carrot)
+      longitudinal_planner.update(sm, carrot)
+      longitudinal_planner.publish(sm, pm, carrot)
+
+      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,715 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+from collections import deque
+from typing import Any
+import heapq
+import copy
+
+import capnp
+from cereal import messaging, log, car
+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
+
+
+# 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 Track:
+  def __init__(self, identifier: int):
+    self.identifier = identifier
+    self.cnt = 0
+    self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL)
+
+    self.is_stopped_car_count = 0
+    self.selected_count = 0
+    self.cut_in_count = 0
+    self.measured = False
+    self.score = 0.0
+    self.in_lane_prob = 0.0
+    self.in_lane_prob_future = 0.0
+
+  def update(self, md, pt, ready, radar_reaction_factor, radar_lat_factor):
+
+    #pt_yRel = -leads_v3[0].y[0] if track_id in [0, 1] and pt.yRel == 0 and self.ready and leads_v3[0].prob > 0.5 else pt.yRel
+    self.dRel = pt.dRel
+    self.yRel = pt.yRel
+    self.vRel = pt.vRel
+
+    self.vLead = self.vLeadK = pt.vLead
+    self.aLead = self.aLeadK = pt.aLead
+    self.jLead = pt.jLead
+    self.yvLead = pt.yvRel
+    
+    self.measured = pt.measured   # measured or estimate
+    if not self.measured:
+      self.cnt = 0
+
+    self.yRel_future = self.yRel + self.yvLead * radar_lat_factor
+    self.dRel_future = self.dRel + self.vLead * radar_lat_factor
+    if ready:
+      self.d_path(md) #self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
+
+    a_lead_threshold = 0.5 * radar_reaction_factor
+    if abs(self.aLead) < a_lead_threshold and abs(self.jLead) < 0.5:
+      self.aLeadTau.x = _LEAD_ACCEL_TAU * radar_reaction_factor
+    else:
+      self.aLeadTau.update(0.0)
+
+    self.cnt += 1
+
+  def d_path(self, md):
+    lane_xs = md.laneLines[1].x
+    left_ys = md.laneLines[1].y
+    right_ys = md.laneLines[2].y
+    def d_path_interp(dRel, yRel):
+      left_lane_y = np.interp(dRel, lane_xs, left_ys)
+      right_lane_y = np.interp(dRel, lane_xs, right_ys)
+      center_y = (left_lane_y + right_lane_y) / 2.0
+      lane_half_width = abs(right_lane_y - left_lane_y) / 2.0
+      dist_from_center = yRel + center_y
+      in_lane_prob = max(0.0, 1.0 - (abs(dist_from_center) / lane_half_width))
+      return dist_from_center, in_lane_prob
+    self.dPath, self.in_lane_prob = d_path_interp(self.dRel, self.yRel)
+    self.dPath_future, self.in_lane_prob_future = d_path_interp(self.dRel_future, self.yRel_future)
+
+  def get_RadarState(self, model_prob: float = 0.0, vision_y_rel = 0.0):
+    return {
+      "dRel": float(self.dRel),
+      "yRel": float(self.yRel) if self.yRel != 0.0 else vision_y_rel,
+      "dPath" : float(self.dPath),
+      "vRel": float(self.vRel),
+      "vLead": float(self.vLead),
+      "vLeadK": float(self.vLeadK),
+      "aLead": float(self.aLead),
+      "aLeadK": float(self.aLeadK),
+      "aLeadTau": float(self.aLeadTau.x),
+      "jLead": float(self.jLead),
+      "vLat": float(self.yvLead), 
+      "status": True,
+      "fcw": self.is_potential_fcw(model_prob),
+      "modelProb": model_prob,
+      "radar": True,
+      "radarTrackId": self.identifier,
+      "score": self.score  # for debug purposes only
+    }
+
+  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):
+  diff = abs(x - mu) / max(b, 1e-4)
+  return 0.0 if diff > 50.0 else math.exp(-diff)
+
+def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
+  offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
+  #vel_tolerance = 25.0 if lead.prob > 0.99 else 10.0
+  max_vision_dist = max(offset_vision_dist * 1.25, 5.0)
+  min_vision_dist = max(offset_vision_dist * 0.8, 1.0)
+  max_vision_dist2 = max(offset_vision_dist * 1.45, 5.0)
+  min_vision_dist2 = 1.5 #max(offset_vision_dist * 0.3, 1.0)
+  max_offset_vision_vel = max(lead.v[0] * np.interp(lead.prob, [0.8, 0.98], [0.3, 0.5]), 5.0) # 확률이 낮으면 속도오차를 줄임.
+
+  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_y2 = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0] * 2)  # for cut-in
+    prob_v = laplacian_pdf(c.vLead, lead.v[0], lead.vStd[0])
+
+    #weight_v = np.interp(c.vLead, [0, 10], [0.3, 1])
+    score = prob_d * prob_y * prob_v # * weight_v
+    score2 = prob_d * prob_y2 * prob_v # * weight_v
+
+    return score, score2 #prob_d * prob_y * prob_v * weight_v
+  
+  def vel_sane(c):
+    return (abs(c.vLead - lead.v[0]) < max_offset_vision_vel) or (c.vLead > 3)
+  def dist_sane(c, second=False):
+    if second:
+      return min_vision_dist2 < c.dRel < max_vision_dist2
+    return min_vision_dist < c.dRel < max_vision_dist
+  def y_sane(c, second=False):
+    if second:
+      return abs(c.yRel + lead.y[0]) < 4.0
+    return abs(c.yRel + lead.y[0]) < 2.0
+ 
+
+  first_track, second_track, extra_track = None, None, None
+  first_score, second_score, extra_score = -1e6, -1e6, -1e6
+  for c in tracks.values():
+    c.score, score2 = prob(c)
+    if c.score > first_score:
+      second_score = first_score
+      second_track = first_track
+      first_score = c.score
+      first_track = c
+    if score2 > extra_score:
+      extra_score = score2
+      extra_track = c
+      
+
+  #best_track = max(tracks.values(), key=prob)
+
+  def select_track(track, score, track2, score2, extra_track, extra_score):
+    if score < 0.0001:
+      return None
+    
+    best_track = None
+    if dist_sane(track) and vel_sane(track):
+      if y_sane(track):
+        if lead.prob > 0.5:
+          best_track = track
+        elif lead.prob > 0.4 and track.selected_count > 0: # 비젼이 희미하지만 직전에 선택된 트랙인경우
+          best_track = track
+      elif lead.prob > 0.6:
+        best_track = track
+    elif dist_sane(track) and y_sane(track, True):  # stopped-car
+      if score2 > 0.00001 and dist_sane(track2) and y_sane(track2) and vel_sane(track2):
+        best_track = track2
+      elif track.selected_count > 0:
+        best_track = track
+      else:
+        track.is_stopped_car_count += 2
+        if track.is_stopped_car_count > int(1.0/DT_MDL):
+          best_track = track
+    #elif dist_sane(track) and vel_sane(track) and lead.prob > 0.5:
+    #  best_track = track
+    elif offset_vision_dist < 90 and lead.prob > 0.65:
+      # wide y detect, for cut-in
+      if extra_score > score and dist_sane(extra_track, True) and vel_sane(extra_track) and y_sane(extra_track, True):
+        best_track = extra_track
+      # wide dRel, y detect, for cut-in
+      elif dist_sane(track, True) and vel_sane(track) and y_sane(track, True):
+        best_track = track
+      elif score2 > 0.0001 and dist_sane(track2, True) and vel_sane(track2) and y_sane(track2, True):
+        best_track = track2
+    return best_track
+    
+  best_track = select_track(first_track, first_score, second_track, second_score, extra_track, extra_score)
+
+  for c in tracks.values():
+    if c is best_track:
+      best_track.selected_count += 1
+    else:
+      c.selected_count = 0
+      c.is_stopped_car_count = max(0, c.is_stopped_car_count - 1)
+      
+  return best_track
+
+def get_RadarState_from_vision(md, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float):
+  lead_v_rel_pred = lead_msg.v[0] - model_v_ego
+  dRel = float(lead_msg.x[0] - RADAR_TO_CAMERA)
+  yRel = float(-lead_msg.y[0])
+  dPath = yRel + np.interp(dRel, md.position.x, md.position.y)
+  return {
+    "dRel": float(dRel),
+    "yRel": yRel,
+    "dPath" : float(dPath),
+    "vRel": float(lead_v_rel_pred),
+    "vLead": float(v_ego + lead_v_rel_pred),
+    "vLeadK": float(v_ego + lead_v_rel_pred),
+    "aLead": float(lead_msg.a[0]),
+    "aLeadK": float(lead_msg.a[0]),
+    "aLeadTau": 0.3,
+    "jLead": 0.0,
+    "vLat" : 0.0,
+    "fcw": False,
+    "modelProb": float(lead_msg.prob),
+    "status": True,
+    "radar": False,
+    "radarTrackId": -1,
+  }
+
+class VisionTrack:
+  def __init__(self, radar_ts):
+    self.radar_ts = radar_ts
+    self.dRel = 0.0
+    self.vRel = 0.0
+    self.yRel = 0.0
+    self.vLead = 0.0
+    self.aLead = 0.0
+    self.vLeadK = 0.0
+    self.aLeadK = 0.0
+    self.aLeadTau = _LEAD_ACCEL_TAU
+    self.prob = 0.0
+    self.status = False
+
+    self.dRel_last = 0.0
+    self.vLead_last = 0.0
+    self.alpha = 0.02
+    self.alpha_a = 0.02
+
+    self.vLat = 0.0
+
+    self.v_ego = 0.0
+    self.cnt = 0
+
+    self.dPath = 0.0
+
+  def get_lead(self, md):
+    #aLeadK = 0.0 if self.mixRadarInfo in [3] else clip(self.aLeadK, self.aLead - 1.0, self.aLead + 1.0)
+    return {
+      "dRel": self.dRel,
+      "yRel": self.yRel,
+      #"dPath": self.dPath,
+      "vRel": self.vRel,
+      "vLead": self.vLead,
+      "vLeadK": self.vLeadK,    ## TODO: 아직 vLeadK는 엉망인듯...
+      "aLead": self.aLead,
+      "aLeadK": self.aLeadK,
+      "aLeadTau": self.aLeadTau,
+      "jLead": 0.0,
+      "vLat": 0.0,
+      "fcw": False,
+      "modelProb": self.prob,
+      "status": self.status,
+      "radar": False,
+      "radarTrackId": -1,
+      #"aLead": self.aLead,
+      #"vLat": self.vLat,
+    }
+
+  def reset(self):
+    self.status = False
+    self.aLeadTau = _LEAD_ACCEL_TAU
+
+    self.vRel = 0.0
+    self.vLead = self.vLeadK = self.v_ego
+    self.aLead = self.aLeadK = 0.0
+    self.vLat = 0.0
+
+  def update(self, lead_msg, model_v_ego, v_ego, md):
+
+    lead_v_rel_pred = lead_msg.v[0] - model_v_ego
+    self.prob = lead_msg.prob
+    self.v_ego = v_ego
+    if self.prob > .5:
+      dRel = float(lead_msg.x[0]) - RADAR_TO_CAMERA
+      if abs(self.dRel - dRel) > 5.0:
+        self.cnt = 0
+      self.dRel = dRel
+
+      self.yRel = float(-lead_msg.y[0])
+      dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
+      a_lead_vision = lead_msg.a[0]
+      if self.cnt < 20 or self.prob < 0.97: # 레이더측정시 cnt는 0, 레이더사라지고 1초간 비젼데이터 그대로 사용
+        self.vRel = lead_v_rel_pred
+        self.vLead = float(v_ego + lead_v_rel_pred)
+        self.aLead = a_lead_vision
+        self.vLat = 0.0
+      else:
+        v_rel = (self.dRel - self.dRel_last) / self.radar_ts
+        v_rel = self.vRel * (1. - self.alpha) + v_rel * self.alpha
+
+        #self.vRel = lead_v_rel_pred if self.mixRadarInfo == 3 else (lead_v_rel_pred + self.vRel) / 2
+        model_weight = np.interp(self.prob, [0.97, 1.0], [0.4, 0.0])  # prob가 높으면 v_rel(dRel미분값)에 가중치를 줌.
+        self.vRel = float(lead_v_rel_pred * model_weight + v_rel * (1. - model_weight))
+        #self.vRel = (lead_v_rel_pred + v_rel) / 2
+        self.vLead = float(v_ego + self.vRel)
+
+        a_lead = (self.vLead - self.vLead_last) / self.radar_ts * 0.2 #0.5 -> 0.2 vel 미분적용을 줄임.
+        self.aLead = self.aLead * (1. - self.alpha_a) + a_lead * self.alpha_a
+        if abs(a_lead_vision) > abs(self.aLead): # or self.mixRadarInfo == 3:
+          self.aLead = a_lead_vision
+
+        vLat_alpha = 0.002
+        self.vLat = self.vLat * (1. - vLat_alpha) + (dPath - self.dPath) / self.radar_ts * vLat_alpha
+
+      self.dPath = dPath
+
+      self.vLeadK= self.vLead
+      self.aLeadK = self.aLead
+
+      self.status = True
+      self.cnt += 1
+    else:
+      self.reset()
+      self.cnt = 0
+      self.dPath = self.yRel + np.interp(v_ego ** 2 / (2 * 2.5), md.position.x, md.position.y)
+
+    self.dRel_last = self.dRel
+    self.vLead_last = self.vLead
+
+    # Learn if constant acceleration
+    #aLeadTauValue = self.aLeadTauPos if self.aLead > self.aLeadTauThreshold else self.aLeadTauNeg
+    if abs(self.aLead) < 0.3: #self.aLeadTauThreshold:
+      self.aLeadTau = 0.2 #aLeadTauValue
+    else:
+      #self.aLeadTau = min(self.aLeadTau * 0.9, aLeadTauValue)
+      self.aLeadTau *= 0.9
+
+class RadarD:
+  def __init__(self, delay: float = 0.0):
+    self.current_time = 0.0
+
+    self.tracks: dict[int, Track] = {}
+
+    self.v_ego = 0.0
+    print("###RadarD.. : delay = ", delay, int(round(delay / DT_MDL))+1)
+    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.vision_tracks = [VisionTrack(DT_MDL), VisionTrack(DT_MDL)]
+
+    self.params = Params()
+    self.enable_radar_tracks = self.params.get_int("EnableRadarTracks")
+    self.enable_corner_radar = self.params.get_int("EnableCornerRadar")
+    self.radar_lat_factor = 0.0
+
+    self.radar_detected = False
+
+
+  def update(self, sm: messaging.SubMaster, rr: car.RadarData):
+    self.ready = sm.seen['modelV2']
+    self.current_time = 1e-9*max(sm.logMonoTime.values())
+
+    self.enable_radar_tracks = self.params.get_int("EnableRadarTracks")
+    self.enable_corner_radar = self.params.get_int("EnableCornerRadar")
+    self.radar_lat_factor = self.params.get_float("RadarLatFactor") * 0.01
+    self.radar_reaction_factor = self.params.get_float("RadarReactionFactor") * 0.01
+    self.detect_cut_in = self.radar_lat_factor > 0
+
+    leads_v3 = sm['modelV2'].leadsV3
+    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']
+
+    valid_ids = set()
+    for pt in rr.points:
+      track_id = pt.trackId
+      valid_ids.add(track_id)      
+
+      if track_id not in self.tracks:
+        self.tracks[track_id] = Track(track_id)
+
+      self.tracks[track_id].update(sm['modelV2'], pt, self.ready, self.radar_reaction_factor, self.radar_lat_factor)
+
+    for tid in list(self.tracks.keys()):
+      if tid not in valid_ids:
+        self.tracks.pop(tid)
+
+    # *** publish radarState ***
+    self.radar_state_valid = sm.all_checks()
+    self.radar_state = log.RadarState.new_message()
+
+    model_updated = False if self.radar_state.mdMonoTime == sm.logMonoTime['modelV2'] else True
+
+    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
+
+    if len(leads_v3) > 1:
+
+      md = sm['modelV2']
+      if model_updated:
+        if self.radar_detected:
+          self.vision_tracks[0].cnt = 0
+          self.vision_tracks[1].cnt = 0
+        self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, md)
+        self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, md)
+
+      alive_tracks = {tid: trk for tid, trk in self.tracks.items() if trk.cnt > 2 }
+      self.radar_state.leadOne, self.radar_detected = self.get_lead(sm['carState'], md, alive_tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
+      self.radar_state.leadTwo, _ = self.get_lead(sm['carState'], md, alive_tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False)
+
+      self.lane_line_available = md.laneLineProbs[1] > 0.5 and md.laneLineProbs[2] > 0.5
+      self.compute_leads(self.v_ego, alive_tracks, md)
+      if self.leadTwo is not None:
+        self.radar_state.leadTwo = self.leadTwo
+      if self.enable_radar_tracks == 3:
+        self._pick_lead_one_from_state()
+
+  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)
+
+  def get_lead(self, CS, md, tracks: dict[int, Track], index: int, lead_msg: capnp._DynamicStructReader,
+               model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]:
+
+    v_ego = self.v_ego
+    ready = self.ready
+
+    ## backup SCC radar(0, 1 trackid)
+    if self.enable_radar_tracks <= 0:
+      track_scc = tracks.get(0)
+    else:
+      track_scc = tracks.pop(0, None)
+
+    # Determine leads, this is where the essential logic happens
+    if len(tracks) > 0 and ready and lead_msg.prob > .4:
+      track = match_vision_to_track(v_ego, lead_msg, tracks)
+    else:
+      track = None
+
+    if (track is None or lead_msg.prob < .6) and track_scc is not None and track_scc.cnt > 2:
+      #if self.enable_radar_tracks in [-1, 2] or model_v_ego < 5 or track_scc.vLead < 5.0:
+      if self.enable_radar_tracks in [-1, 2] or track_scc.vLead < 5.0:
+        track = track_scc      
+
+    lead_dict = {'status': False}
+    radar = False
+    if track is not None:
+      lead_dict = track.get_RadarState(lead_msg.prob, self.vision_tracks[0].yRel)
+      radar = True
+    elif (track is None) and ready and (lead_msg.prob > .5):
+        lead_dict = self.vision_tracks[index].get_lead(md)
+
+    if self.enable_corner_radar > 0:
+      lead_dict = self.corner_radar(CS, lead_dict)
+
+    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(lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
+          lead_dict = closest_track.get_RadarState(lead_msg.prob, self.vision_tracks[0].yRel)
+
+    return lead_dict, radar
+
+  def compute_leads(self, v_ego, tracks, md):
+    lead_msg = md.leadsV3[0] if (md is not None and len(md.position.x) == 33) else None
+    self.leadCutIn = {'status': False}
+    if lead_msg is None:
+      # reset
+      self.radar_state.leadsLeft = []
+      self.radar_state.leadsCenter = []
+      self.radar_state.leadsRight = []
+      self.radar_state.leadLeft = {'status': False}
+      self.radar_state.leadRight = {'status': False}
+      return
+    
+    left_list, right_list, center_list, cutin_list = [], [], [], []
+    for c in tracks.values():
+      y_rel_neg = - c.yRel
+      # center
+      if c.in_lane_prob > 0.1:
+        if c.cnt > 3:
+          ld = c.get_RadarState(lead_msg.prob, float(-lead_msg.y[0]))
+          ld['modelProb'] = 0.01
+          center_list.append(ld)
+
+      # left/right
+      elif y_rel_neg < 0: #left_lane_y:
+        ld = c.get_RadarState(0, 0)
+        if self.lane_line_available and c.in_lane_prob_future > 0.1 and c.cnt > int(2.0/DT_MDL):
+          if c.cut_in_count > int(0.1/DT_MDL):
+            ld['modelProb'] = 0.03
+            cutin_list.append(ld)
+          c.cut_in_count += 2
+        left_list.append(ld)
+      else:
+        ld = c.get_RadarState(0, 0)
+        if self.lane_line_available and c.in_lane_prob_future > 0.1 and c.cnt > int(2.0/DT_MDL):
+          if c.cut_in_count > int(0.1/DT_MDL):
+            ld['modelProb'] = 0.03
+            cutin_list.append(ld)
+          c.cut_in_count += 2
+        right_list.append(ld)
+
+      c.cut_in_count = max(c.cut_in_count - 1, 0)
+
+    self.radar_state.leadsLeft   = left_list
+    self.radar_state.leadsRight  = right_list
+    self.radar_state.leadsCenter = center_list
+    self.radar_state.leadsCutIn = cutin_list
+    self.leadCutIn = min(
+      (ld for ld in cutin_list if 3 < ld['dRel'] < 50 and ld['vLead'] > 4),
+      key=lambda d: d['dRel'],
+      default={'status': False}
+    )
+
+    self.radar_state.leadLeft  = min(
+        (ld for ld in left_list if ld['dRel'] > 5 and abs(ld['dPath']) < 3.5),
+        key=lambda d: d['dRel'],
+        default={'status': False}
+    )
+    self.radar_state.leadRight = min(
+        (ld for ld in right_list if ld['dRel'] > 5 and abs(ld['dPath']) < 3.5),
+        key=lambda d: d['dRel'],
+        default={'status': False}
+    )
+   
+    self.leadTwo = None
+    if self.lane_line_available:
+      self.leadCenter = min(
+          (ld for ld in center_list if ld['vLead'] > 5 and ld['radar'] and ld['dRel'] > 3.5),
+          key=lambda d: d['dRel'],
+          default=None
+      )
+      if self.radar_state.leadOne.status and self.radar_state.leadOne.radar:
+        self.leadTwo = min(
+            (ld for ld in center_list if ld['vLead'] > 5 and ld['radar'] and self.radar_state.leadOne.dRel < ld['dRel'] < 80),
+            key=lambda d: d['dRel'],
+            default=None
+        )
+        if self.leadTwo is not None:
+          self.leadTwo = copy.deepcopy(self.leadTwo)
+          #gap = self.leadTwo['dRel'] - self.radar_state.leadOne.dRel
+          #offset = 3.0 + min(gap * 0.2, 10)
+          #self.leadTwo['dRel'] = self.radar_state.leadOne.dRel + offset
+          self.leadTwo['dRel'] = max(self.radar_state.leadOne.dRel + 3.0, self.leadTwo['dRel'] - 8.0) # lead+1 차를 뒤로 8M후퇴하여, mpc에서  감자하도록함.. 최소 lead보다 3M앞에 위치하도록
+    else:
+      self.leadCenter = None
+
+    def _ok(ld):
+        return (ld.get('vLead', 0) > 2 and
+                abs(ld.get('dPath', 0)) < 4.2 and
+                ld.get('dRel', 0) > 2)
+
+    def _pick_two_with_gap(cands, min_gap=5.0):
+        xs = sorted((ld for ld in cands if _ok(ld)), key=lambda d: d['dRel'])
+        if not xs:
+            return []
+        first = xs[0]
+        second = None
+        for ld in xs[1:]:
+            # 5m 이상 떨어진 후보만 허용 (>= 5.0)
+            if (ld['dRel'] - first['dRel']) >= min_gap:
+                second = ld
+                break
+        return [first] if second is None else [first, second]
+
+    self.radar_state.leadsLeft2  = _pick_two_with_gap(left_list,  min_gap=5.0)
+    self.radar_state.leadsRight2 = _pick_two_with_gap(right_list, min_gap=5.0)
+
+  def _pick_lead_one_from_state(self):
+    chosen = None
+    detected = self.radar_detected
+
+    if self.leadCutIn and self.leadCutIn.get("status") and self.detect_cut_in:
+      if self.radar_state.leadOne.status:
+        if self.leadCutIn["dRel"] < self.radar_state.leadOne.dRel:
+          chosen = self.leadCutIn
+          chosen["modelProb"] = 0.03
+          detected = True
+      else:
+        chosen = self.leadCutIn
+        chosen["modelProb"] = 0.03
+        detected = True
+
+    elif self.leadCenter and self.leadCenter["status"]:
+      if self.radar_detected:
+        if self.radar_state.leadOne.status and self.leadCenter["dRel"] < self.radar_state.leadOne.dRel:
+          chosen = self.leadCenter
+          chosen["modelProb"] = 0.01
+      else:
+        chosen = self.leadCenter
+        chosen["modelProb"] = 0.02
+        detected = True
+
+    if chosen is not None:
+        self.radar_state.leadOne = chosen
+        self.radar_detected = detected
+
+  
+  def corner_radar(self, CS, lead_dict):
+    lat_dist = 1e6
+    long_dist = 1e6
+    if 0 < CS.leftLatDist < 2.5:
+      lat_dist = CS.leftLatDist
+      long_dist = CS.leftLongDist
+    if 0 < CS.rightLatDist < 2.5 and CS.rightLongDist < long_dist:
+      lat_dist = -CS.rightLatDist
+      long_dist = CS.rightLongDist
+
+    if lat_dist == 0.0 or abs(lat_dist) >= 2.5 or long_dist == 1e6:
+      return lead_dict
+    
+    if lead_dict['status']:
+      if lead_dict['dRel'] > long_dist:
+        lead_dict['dRel'] = long_dist
+        lead_dict['yRel'] = lat_dist
+        lead_dict['vRel'] = 0.0
+        lead_dict['vLead'] = CS.vEgo if CS.vEgo < lead_dict['vLead'] else lead_dict['vLead']
+        lead_dict['vLeadK'] = lead_dict['vLead']
+        lead_dict['aLead'] = CS.aEgo if CS.aEgo < lead_dict['aLead'] else lead_dict['aLead']
+        lead_dict['aLeadK'] = lead_dict['aLead']
+        lead_dict['aLeadTau'] = _LEAD_ACCEL_TAU
+        lead_dict['jLead'] = 0.0
+        lead_dict['vLat'] = 0.0
+        lead_dict['modelProb'] = 1.0
+        lead_dict['radarTrackId'] = -1
+        lead_dict['radar'] = True
+    else:
+      lead_dict['status'] = True
+      lead_dict['dRel'] = long_dist
+      lead_dict['yRel'] = lat_dist
+      lead_dict['vRel'] = 0.0
+      lead_dict['vLead'] = CS.vEgo
+      lead_dict['vLeadK'] = CS.vEgo
+      lead_dict['aLead'] = CS.aEgo
+      lead_dict['aLeadK'] = CS.aEgo
+      lead_dict['aLeadTau'] = _LEAD_ACCEL_TAU
+      lead_dict['jLead'] = 0.0
+      lead_dict['vLat'] = 0.0
+      lead_dict['modelProb'] = 1.0
+      lead_dict['radarTrackId'] = -1
+      lead_dict['radar'] = True
+
+    return lead_dict
+
+# 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")
+
+  # *** setup messaging
+  sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='modelV2')
+  #sm = messaging.SubMaster(['modelV2', 'carState', 'liveTracks'], poll='liveTracks')
+  pm = messaging.PubMaster(['radarState'])
+
+  RD = RadarD(CP.radarDelay)
+
+  while 1:
+    sm.update()
+
+    RD.update(sm, sm['liveTracks'])
+    RD.publish(pm)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/controls/tests/test_following_distance.py

@@ -0,0 +1,40 @@
+import pytest
+import itertools
+from parameterized import parameterized_class
+
+from cereal import log
+
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW
+from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
+
+
+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_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 opendbc.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 and len(state.liveTracks.errors) for state in states]
+
+    assert len(states) == 0 or all(failures)

selfdrive/controls/tests/test_longcontrol.py

@@ -0,0 +1,56 @@
+from cereal import car
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+
+
+
+
+class TestLongControlStateTransition:
+
+  def test_stay_stopped(self):
+    CP = car.CarParams.new_message()
+    active = True
+    current_state = LongCtrlState.stopping
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+    assert next_state == LongCtrlState.stopping
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.pid
+    active = False
+    next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+    assert next_state == LongCtrlState.off
+
+def test_engage():
+  CP = car.CarParams.new_message()
+  active = True
+  current_state = LongCtrlState.off
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=True, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=True, cruise_standstill=False)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True)
+  assert next_state == LongCtrlState.stopping
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid
+
+def test_starting():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  active = True
+  current_state = LongCtrlState.starting
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.starting
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False)
+  assert next_state == LongCtrlState.pid