sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -0,0 +1,104 @@
+from openpilot.cereal import log, custom
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
+
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+TurnDirection = custom.ModelDataV2SP.TurnDirection
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+LANE_CHANGE_TIME_MAX = 10.
+LANE_CHANGE_START_TIME = 0.5
+
+TURN_DESIRES = {
+  TurnDirection.none: log.Desire.none,
+  TurnDirection.turnLeft: log.Desire.turnLeft,
+  TurnDirection.turnRight: log.Desire.turnRight,
+}
+
+class DesireHelper:
+  def __init__(self):
+    self.lane_change_state = LaneChangeState.off
+    self.lane_change_direction = LaneChangeDirection.none
+    self.lane_change_timer = 0.0
+    self.prev_one_blinker = False
+    self.desire = log.Desire.none
+    self.alc = AutoLaneChangeController(self)
+    self.lane_turn_controller = LaneTurnController(self)
+    self.lane_turn_direction = TurnDirection.none
+
+  @staticmethod
+  def get_lane_change_direction(CS):
+    return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
+
+  def update(self, carstate, lateral_active, lane_change_prob):
+    self.alc.update_params()
+    self.lane_turn_controller.update_params()
+    v_ego = carstate.vEgo
+    one_blinker = carstate.leftBlinker != carstate.rightBlinker
+    below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
+
+    # Lane turn controller update
+    self.lane_turn_controller.update_lane_turn(blindspot_left=carstate.leftBlindspot, blindspot_right=carstate.rightBlindspot,
+                                               left_blinker=carstate.leftBlinker, right_blinker=carstate.rightBlinker, v_ego=v_ego)
+    self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
+
+    if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX or self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF:
+      self.lane_change_state = LaneChangeState.off
+      self.lane_change_direction = LaneChangeDirection.none
+      self.lane_change_timer = 0.0
+    else:
+      if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
+        self.lane_change_state = LaneChangeState.preLaneChange
+        self.lane_change_timer = 0.0
+        # Initialize lane change direction to prevent UI alert flicker
+        self.lane_change_direction = self.get_lane_change_direction(carstate)
+
+      elif self.lane_change_state == LaneChangeState.preLaneChange:
+        # Update lane change direction
+        self.lane_change_direction = self.get_lane_change_direction(carstate)
+
+        torque_applied = carstate.steeringPressed and \
+                         ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
+                          (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
+
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+
+        self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
+
+        if not one_blinker or below_lane_change_speed:
+          self.lane_change_state = LaneChangeState.off
+          self.lane_change_direction = LaneChangeDirection.none
+          self.lane_change_timer = 0.0
+        elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected:
+          self.lane_change_state = LaneChangeState.laneChangeStarting
+          self.lane_change_timer = 0.0
+
+      elif self.lane_change_state == LaneChangeState.laneChangeStarting:
+        self.lane_change_timer += DT_MDL
+
+        if lane_change_prob < 0.02 and self.lane_change_timer >= LANE_CHANGE_START_TIME:
+          self.lane_change_timer = 0.0
+          if one_blinker:
+            self.lane_change_state = LaneChangeState.preLaneChange
+            self.lane_change_direction = self.get_lane_change_direction(carstate)
+          else:
+            self.lane_change_state = LaneChangeState.off
+            self.lane_change_direction = LaneChangeDirection.none
+
+    self.prev_one_blinker = one_blinker and lateral_active
+
+    if self.lane_turn_direction != TurnDirection.none:
+      self.desire = TURN_DESIRES[self.lane_turn_direction]
+    else:
+      self.desire = log.Desire.none
+      if self.lane_change_state == LaneChangeState.laneChangeStarting:
+        if self.lane_change_direction == LaneChangeDirection.left:
+          self.desire = log.Desire.laneChangeLeft
+        elif self.lane_change_direction == LaneChangeDirection.right:
+          self.desire = log.Desire.laneChangeRight
+
+    self.alc.update_state()

openpilot/selfdrive/controls/lib/drive_helpers.py

@@ -0,0 +1,70 @@
+import numpy as np
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.realtime import DT_CTRL, DT_MDL
+
+MIN_SPEED = 1.0
+CONTROL_N = 17
+CAR_ROTATION_RADIUS = 0.0
+# This is a turn radius smaller than most cars can achieve
+MAX_CURVATURE = 0.2
+MAX_VEL_ERR = 5.0  # m/s
+MIN_STABLE_DELAY = 0.3
+
+# EU guidelines
+MAX_LATERAL_JERK = 5.0  # m/s^3
+MAX_LATERAL_ACCEL_NO_ROLL = 3.0  # m/s^2
+
+
+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) -> tuple[float, bool]:
+  # This function respects ISO lateral jerk and acceleration limits + a max curvature
+  v_ego = max(v_ego, MIN_SPEED)
+  max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2)  # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
+  new_curvature = np.clip(new_curvature,
+                          prev_curvature - max_curvature_rate * DT_CTRL,
+                          prev_curvature + max_curvature_rate * DT_CTRL)
+
+  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)
+  return float(new_curvature), limited_accel or limited_max_curv
+
+
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3):
+  if len(speeds) == len(t_idxs):
+    v_now = speeds[0]
+    a_now = accels[0]
+    if action_t < MIN_STABLE_DELAY:
+      v_target = v_now + (action_t / MIN_STABLE_DELAY) * (np.interp(MIN_STABLE_DELAY, t_idxs, speeds) - v_now)
+    else:
+      v_target = np.interp(action_t, t_idxs, speeds)
+    a_target = 2 * (v_target - v_now) / (action_t) - a_now
+  else:
+    v_now = 0.0
+    v_target = 0.0
+    a_target = 0.0
+  should_stop = (v_now < vEgoStopping and a_target < 0.1)
+  return a_target, should_stop
+
+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):
+  if action_t < MIN_STABLE_DELAY:
+    psi_target = (action_t / MIN_STABLE_DELAY) * np.interp(MIN_STABLE_DELAY, t_idxs, yaws)
+  else:
+    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)

openpilot/selfdrive/controls/lib/latcontrol.py

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

openpilot/selfdrive/controls/lib/latcontrol_angle.py

@@ -0,0 +1,37 @@
+import math
+
+from openpilot.cereal import log
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+
+# TODO This is speed dependent
+STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
+
+
+class LatControlAngle(LatControl):
+  def __init__(self, CP, CP_SP, CI, dt):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.sat_check_min_speed = 5.
+    self.use_steer_limited_by_safety = CP.brand in ("tesla", "hyundai")
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
+    angle_log = log.ControlsState.LateralAngleState.new_message()
+
+    if not active:
+      angle_log.active = False
+      angle_steers_des = float(CS.steeringAngleDeg)
+    else:
+      angle_log.active = True
+      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+      angle_steers_des += params.angleOffsetDeg
+
+    if self.use_steer_limited_by_safety:
+      # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
+      angle_control_saturated = steer_limited_by_safety
+    else:
+      # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
+      # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque  # TODO: this can be improved
+      angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
+    angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
+    angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    angle_log.steeringAngleDesiredDeg = angle_steers_des
+    return 0, float(angle_steers_des), angle_log

openpilot/selfdrive/controls/lib/latcontrol_curvature.py

@@ -0,0 +1,58 @@
+import math
+
+from openpilot.cereal import log
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
+
+CURVATURE_SATURATION_THRESHOLD = 1e-3  # 1/m
+
+
+class LatControlCurvature(LatControl):
+  def __init__(self, CP, CP_SP, CI, dt):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.sat_check_min_speed = 5.
+    if CP.lateralTuning.which() == 'pid':
+      ct = CP.lateralTuning.pid
+      self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV),
+                               pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt)
+      self.kf = ct.kf
+    else:
+      self.pid = None
+      self.kf = 1.
+
+  def reset(self):
+    super().reset()
+    if self.pid is not None:
+      self.pid.reset()
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
+    curvature_log = log.ControlsState.LateralCurvatureState.new_message()
+    actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    error = desired_curvature - actual_curvature
+
+    if not active:
+      output_curvature = 0.0
+      curvature_log.active = False
+      if self.pid is not None:
+        self.pid.reset()
+    elif self.pid is None or CS.steeringPressed:
+      # no PID or override: feedforward only
+      if self.pid is not None:
+        self.pid.reset()
+      output_curvature = self.kf * desired_curvature
+      curvature_log.active = True
+    else:
+      output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature)
+      curvature_log.p = float(self.pid.p)
+      curvature_log.i = float(self.pid.i)
+      curvature_log.f = float(self.pid.f)
+      curvature_log.active = True
+
+    curvature_log.error = float(error)
+    curvature_log.actualCurvature = float(actual_curvature)
+    curvature_log.desiredCurvature = float(desired_curvature)
+    curvature_log.output = float(output_curvature)
+    curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
+                                                          False, curvature_limited))
+    return 0.0, float(output_curvature), curvature_log

openpilot/selfdrive/controls/lib/latcontrol_pid.py

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

openpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -0,0 +1,122 @@
+import math
+import numpy as np
+from collections import deque
+
+from openpilot.cereal import log
+from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects the
+# proportional gain is increased at low speeds by the PID controller.
+# Additionally, there is friction in the steering wheel that needs
+# to be overcome to move it at all, this is compensated for too.
+
+KP = 0.8
+KI = 0.15
+
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.2
+JERK_LOOKAHEAD_SECONDS = 0.19
+JERK_GAIN = 0.3
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+VERSION = 1
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CP_SP, CI, dt):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
+    self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
+    self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+
+    self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
+
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
+    # Override torque params from extension
+    if self.extension.update_override_torque_params(self.torque_params):
+      self.update_limits()
+
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+    pid_log.version = VERSION
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+    self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+
+    roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+    curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+    lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+    delay_frames = int(np.clip(lat_delay / self.dt + 1, 1, self.lat_accel_request_buffer_len))
+    expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+    setpoint = expected_lateral_accel
+    error = setpoint - measurement
+
+    lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
+    raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
+    desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
+    gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+    ff = gravity_adjusted_future_lateral_accel
+    # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
+    ff -= self.torque_params.latAccelOffset
+    ff += get_friction(error + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
+      pid_log.error = float(error)
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error, speed=CS.vEgo, feedforward=ff, freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                     future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
+                                                     desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque) # TODO: log lat accel?
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    # TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log

openpilot/selfdrive/controls/lib/ldw.py

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

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -0,0 +1,89 @@
+import numpy as np
+from opendbc.car.structs 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
+
+CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
+
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
+
+def long_control_state_trans(CP, CP_SP, active, long_control_state, v_ego,
+                             should_stop, brake_pressed, cruise_standstill):
+  # Gas Interceptor
+  cruise_standstill = cruise_standstill and not CP_SP.enableGasInterceptor
+
+  starting_condition = (not should_stop and
+                        not cruise_standstill and
+                        not brake_pressed)
+
+  if not active:
+    long_control_state = LongCtrlState.off
+
+  else:
+    if long_control_state == LongCtrlState.off:
+      if not starting_condition:
+        long_control_state = LongCtrlState.stopping
+      elif 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 should_stop:
+        long_control_state = LongCtrlState.stopping
+      elif v_ego > CP.vEgoStarting:
+        long_control_state = LongCtrlState.pid
+  return long_control_state
+
+class LongControl:
+  def __init__(self, CP, CP_SP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+    self.long_control_state = LongCtrlState.off
+    self.pid = PIDController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
+                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
+                             rate=1 / DT_CTRL)
+    self.last_output_accel = 0.0
+
+  def reset(self):
+    self.pid.reset()
+
+  def update(self, active, CS, a_target, should_stop, accel_limits):
+    """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, self.CP_SP, active, self.long_control_state, CS.vEgo,
+                                                       should_stop, CS.brakePressed,
+                                                       CS.cruiseState.standstill)
+    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 output_accel > self.CP.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
+      error = a_target - CS.aEgo
+      output_accel = self.pid.update(error, speed=CS.vEgo,
+                                     feedforward=a_target)
+
+    self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
+    return self.last_output_accel

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

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

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json

@@ -0,0 +1,524 @@
+{
+    "acados_include_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/include",
+    "acados_lib_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/lib",
+    "code_export_directory": "/data/openpilot/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": 6,
+        "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/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
+    "model": {
+        "con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
+        "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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfggh",
+        "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": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
+        "u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
+        "x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
+        "xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
+        "z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
+    },
+    "parameter_values": [
+        -1.2,
+        1.2,
+        0.0,
+        0.0,
+        1.45,
+        0.75
+    ],
+    "problem_class": "OCP",
+    "shared_lib_ext": ".so",
+    "solver_options": {
+        "Tsim": 0.06944444444444445,
+        "alpha_min": 0.05,
+        "alpha_reduction": 0.7,
+        "collocation_type": "GAUSS_LEGENDRE",
+        "custom_templates": [],
+        "custom_update_copy": true,
+        "custom_update_filename": "",
+        "custom_update_header_filename": "",
+        "eps_sufficient_descent": 0.0001,
+        "exact_hess_constr": 1,
+        "exact_hess_cost": 1,
+        "exact_hess_dyn": 1,
+        "ext_cost_num_hess": 0,
+        "ext_fun_compile_flags": "-O2",
+        "full_step_dual": 0,
+        "globalization": "FIXED_STEP",
+        "globalization_use_SOC": 0,
+        "hessian_approx": "GAUSS_NEWTON",
+        "hpipm_mode": "BALANCE",
+        "initialize_t_slacks": 0,
+        "integrator_type": "ERK",
+        "levenberg_marquardt": 0.0,
+        "line_search_use_sufficient_descent": 0,
+        "model_external_shared_lib_dir": null,
+        "model_external_shared_lib_name": null,
+        "nlp_solver_ext_qp_res": 0,
+        "nlp_solver_max_iter": 100,
+        "nlp_solver_step_length": 1.0,
+        "nlp_solver_tol_comp": 1e-06,
+        "nlp_solver_tol_eq": 1e-06,
+        "nlp_solver_tol_ineq": 1e-06,
+        "nlp_solver_tol_stat": 1e-06,
+        "nlp_solver_type": "SQP_RTI",
+        "print_level": 0,
+        "qp_solver": "PARTIAL_CONDENSING_HPIPM",
+        "qp_solver_cond_N": 1,
+        "qp_solver_cond_ric_alg": 1,
+        "qp_solver_iter_max": 10,
+        "qp_solver_ric_alg": 1,
+        "qp_solver_tol_comp": 0.001,
+        "qp_solver_tol_eq": 0.001,
+        "qp_solver_tol_ineq": 0.001,
+        "qp_solver_tol_stat": 0.001,
+        "qp_solver_warm_start": 0,
+        "regularize_method": null,
+        "shooting_nodes": [
+            0.0,
+            0.06944444444444445,
+            0.2777777777777778,
+            0.625,
+            1.1111111111111112,
+            1.7361111111111114,
+            2.5,
+            3.4027777777777786,
+            4.444444444444445,
+            5.625,
+            6.9444444444444455,
+            8.402777777777777,
+            10.0
+        ],
+        "sim_method_jac_reuse": [
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0,
+            0
+        ],
+        "sim_method_newton_iter": 3,
+        "sim_method_newton_tol": 0.0,
+        "sim_method_num_stages": [
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4
+        ],
+        "sim_method_num_steps": [
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1,
+            1
+        ],
+        "tf": 10.0,
+        "time_steps": [
+            0.06944444444444445,
+            0.20833333333333334,
+            0.3472222222222222,
+            0.48611111111111116,
+            0.6250000000000002,
+            0.7638888888888886,
+            0.9027777777777786,
+            1.041666666666666,
+            1.1805555555555554,
+            1.3194444444444455,
+            1.4583333333333313,
+            1.5972222222222232
+        ]
+    }
+}

openpilot/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 = /usr/local/venv/lib/python3.12/site-packages/acados/install/include
+LIB_PATH = /usr/local/venv/lib/python3.12/site-packages/acados/install/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/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/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

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_sim_solver_long.c

@@ -0,0 +1,294 @@
+/*
+ * 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.;
+ */
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "long_model/long_model.h"
+#include "acados_sim_solver_long.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * long_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int long_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int long_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    const int nx = LONG_NX;
+    const int nu = LONG_NU;
+    const int nz = LONG_NZ;
+    const int np = LONG_NP;
+    bool tmp_bool;
+
+    
+    double Tsim = 0.06944444444444445;
+
+    
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_vde_adj_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &long_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &long_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &long_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &long_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &long_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &long_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
+
+    capsule->sim_vde_adj_casadi->casadi_fun = &long_expl_vde_adj;
+    capsule->sim_vde_adj_casadi->casadi_n_in = &long_expl_vde_adj_n_in;
+    capsule->sim_vde_adj_casadi->casadi_n_out = &long_expl_vde_adj_n_out;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_in = &long_expl_vde_adj_sparsity_in;
+    capsule->sim_vde_adj_casadi->casadi_sparsity_out = &long_expl_vde_adj_sparsity_out;
+    capsule->sim_vde_adj_casadi->casadi_work = &long_expl_vde_adj_work;
+    external_function_param_casadi_create(capsule->sim_vde_adj_casadi, np);
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &long_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &long_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &long_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &long_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &long_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &long_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
+
+    
+
+    // sim plan & config
+    sim_solver_plan_t plan;
+    plan.sim_solver = ERK;
+
+    // create correct config based on plan
+    sim_config * long_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = long_sim_config;
+
+    // sim dims
+    void *long_sim_dims = sim_dims_create(long_sim_config);
+    capsule->acados_sim_dims = long_sim_dims;
+    sim_dims_set(long_sim_config, long_sim_dims, "nx", &nx);
+    sim_dims_set(long_sim_config, long_sim_dims, "nu", &nu);
+    sim_dims_set(long_sim_config, long_sim_dims, "nz", &nz);
+
+
+    // sim opts
+    sim_opts *long_sim_opts = sim_opts_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_opts = long_sim_opts;
+    int tmp_int = 3;
+    sim_opts_set(long_sim_config, long_sim_opts, "newton_iter", &tmp_int);
+    double tmp_double = 0.0;
+    sim_opts_set(long_sim_config, long_sim_opts, "newton_tol", &tmp_double);
+    sim_collocation_type collocation_type = GAUSS_LEGENDRE;
+    sim_opts_set(long_sim_config, long_sim_opts, "collocation_type", &collocation_type);
+
+ 
+    tmp_int = 4;
+    sim_opts_set(long_sim_config, long_sim_opts, "num_stages", &tmp_int);
+    tmp_int = 1;
+    sim_opts_set(long_sim_config, long_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = 0;
+    sim_opts_set(long_sim_config, long_sim_opts, "jac_reuse", &tmp_bool);
+
+
+    // sim in / out
+    sim_in *long_sim_in = sim_in_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_in = long_sim_in;
+    sim_out *long_sim_out = sim_out_create(long_sim_config, long_sim_dims);
+    capsule->acados_sim_out = long_sim_out;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "T", &Tsim);
+
+    // model functions
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_vde_forw", capsule->sim_forw_vde_casadi);
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_vde_adj", capsule->sim_vde_adj_casadi);
+    long_sim_config->model_set(long_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+
+    // sim solver
+    sim_solver *long_sim_solver = sim_solver_create(long_sim_config,
+                                               long_sim_dims, long_sim_opts);
+    capsule->acados_sim_solver = long_sim_solver;
+
+
+    /* initialize parameter values */
+    double* p = calloc(np, sizeof(double));
+    
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    long_acados_sim_update_params(capsule, p, np);
+    free(p);
+
+
+    /* initialize input */
+    // x
+    double x0[3];
+    for (int ii = 0; ii < 3; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "x", x0);
+
+
+    // u
+    double u0[1];
+    for (int ii = 0; ii < 1; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[12];
+    for (int ii = 0; ii < 12; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < 3; ii++)
+        S_forw[ii + ii * 3 ] = 1.0;
+
+
+    sim_in_set(long_sim_config, long_sim_dims,
+               long_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute(long_sim_solver, long_sim_in, long_sim_out);
+
+    return status;
+}
+
+
+int long_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in long_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int long_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_vde_adj_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+
+    return 0;
+}
+
+
+int long_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = LONG_NP;
+
+    if (casadi_np != np) {
+        printf("long_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * long_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * long_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+

openpilot/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     6
+
+#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_

openpilot/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)

openpilot/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     6
+#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_

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_fun.c

@@ -0,0 +1,187 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_constr_h_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+
+/* long_constr_h_fun:(i0[3],i1,i2[],i3[6])->(o0[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=arg[3]? arg[3][0] : 0;
+  a2=(a1-a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[3]? arg[3][1] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a2=(a2-a1);
+  a1=arg[3]? arg[3][5] : 0;
+  a3=casadi_sq(a0);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a0);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a1=(a1*a3);
+  a2=(a2-a1);
+  a1=10.;
+  a0=(a0+a1);
+  a2=(a2/a0);
+  if (res[0]!=0) res[0][3]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_constraints/long_constr_h_fun_jac_uxt_zt.c

@@ -0,0 +1,213 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_constr_h_fun_jac_uxt_zt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_s6 CASADI_PREFIX(s6)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+static const casadi_int casadi_s5[12] = {4, 4, 0, 1, 2, 3, 5, 2, 3, 3, 1, 2};
+static const casadi_int casadi_s6[3] = {4, 0, 0};
+
+/* long_constr_h_fun_jac_uxt_zt:(i0[3],i1,i2[],i3[6])->(o0[4],o1[4x4,5nz],o2[4x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a1=arg[0]? arg[0][2] : 0;
+  a2=arg[3]? arg[3][0] : 0;
+  a2=(a1-a2);
+  if (res[0]!=0) res[0][1]=a2;
+  a2=arg[3]? arg[3][1] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a2=(a2-a1);
+  a1=arg[3]? arg[3][5] : 0;
+  a3=casadi_sq(a0);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a0);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a3=(a1*a3);
+  a2=(a2-a3);
+  a3=10.;
+  a3=(a0+a3);
+  a2=(a2/a3);
+  if (res[0]!=0) res[0][3]=a2;
+  a5=1.;
+  if (res[1]!=0) res[1][0]=a5;
+  if (res[1]!=0) res[1][1]=a5;
+  a5=-1.;
+  if (res[1]!=0) res[1][2]=a5;
+  a5=(1./a3);
+  a5=(-a5);
+  if (res[1]!=0) res[1][3]=a5;
+  a5=2.0000000000000001e-01;
+  a0=(a0+a0);
+  a5=(a5*a0);
+  a5=(a5+a4);
+  a1=(a1*a5);
+  a1=(a1/a3);
+  a2=(a2/a3);
+  a1=(a1+a2);
+  a1=(-a1);
+  if (res[1]!=0) res[1][4]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_constr_h_fun_jac_uxt_zt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_constr_h_fun_jac_uxt_zt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_constr_h_fun_jac_uxt_zt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_constr_h_fun_jac_uxt_zt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_constr_h_fun_jac_uxt_zt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    case 1: return casadi_s5;
+    case 2: return casadi_s6;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_constr_h_fun_jac_uxt_zt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 3*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/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

openpilot/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

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun.c

@@ -0,0 +1,184 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_cost_y_0_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_fun_jac_ut_xt.c

@@ -0,0 +1,210 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
+static const casadi_int casadi_s5[3] = {6, 0, 0};
+
+/* long_cost_y_0_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  if (res[1]!=0) res[1][6]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 3*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_0_hess.c

@@ -0,0 +1,209 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_0_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
+
+/* long_cost_y_0_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_0_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_0_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_0_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_0_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_0_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_0_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun.c

@@ -0,0 +1,182 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+
+/* long_cost_y_e_fun:(i0[3],i1[],i2[],i3[6])->(o0[5]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_fun_jac_ut_xt.c

@@ -0,0 +1,207 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s4[14] = {3, 5, 0, 2, 3, 4, 5, 6, 0, 1, 0, 1, 2, 2};
+static const casadi_int casadi_s5[3] = {5, 0, 0};
+
+/* long_cost_y_e_fun_jac_ut_xt:(i0[3],i1[],i2[],i3[6])->(o0[5],o1[3x5,6nz],o2[5x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 3*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_e_hess.c

@@ -0,0 +1,209 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_e_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[3] = {0, 0, 0};
+static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[9] = {3, 3, 0, 1, 3, 3, 1, 0, 1};
+
+/* long_cost_y_e_hess:(i0[3],i1[],i2[],i3[5],i4[6])->(o0[3x3,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_e_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_e_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_e_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_e_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_e_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_e_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun.c

@@ -0,0 +1,184 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_cost_y_fun:(i0[3],i1,i2[],i3[6])->(o0[6]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a4=(a4*a2);
+  a3=(a3+a4);
+  a4=6.;
+  a3=(a3+a4);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a2=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a2;
+  a1=arg[3]? arg[3][3] : 0;
+  a2=(a2-a1);
+  if (res[0]!=0) res[0][4]=a2;
+  a2=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a2;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_fun_jac_ut_xt.c

@@ -0,0 +1,210 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_fun_jac_ut_xt_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_s5 CASADI_PREFIX(s5)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[16] = {4, 6, 0, 2, 3, 4, 5, 6, 7, 1, 2, 1, 2, 3, 3, 0};
+static const casadi_int casadi_s5[3] = {6, 0, 0};
+
+/* long_cost_y_fun_jac_ut_xt:(i0[3],i1,i2[],i3[6])->(o0[6],o1[4x6,7nz],o2[6x0]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a2, a3, a4, a5;
+  a0=arg[3]? arg[3][2] : 0;
+  a1=arg[0]? arg[0][0] : 0;
+  a0=(a0-a1);
+  a2=arg[0]? arg[0][1] : 0;
+  a3=casadi_sq(a2);
+  a4=5.;
+  a3=(a3/a4);
+  a4=arg[3]? arg[3][4] : 0;
+  a5=(a4*a2);
+  a3=(a3+a5);
+  a5=6.;
+  a3=(a3+a5);
+  a0=(a0-a3);
+  a3=10.;
+  a3=(a2+a3);
+  a0=(a0/a3);
+  if (res[0]!=0) res[0][0]=a0;
+  if (res[0]!=0) res[0][1]=a1;
+  if (res[0]!=0) res[0][2]=a2;
+  a1=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][3]=a1;
+  a5=arg[3]? arg[3][3] : 0;
+  a1=(a1-a5);
+  if (res[0]!=0) res[0][4]=a1;
+  a1=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][5]=a1;
+  a1=(1./a3);
+  a1=(-a1);
+  if (res[1]!=0) res[1][0]=a1;
+  a1=2.0000000000000001e-01;
+  a2=(a2+a2);
+  a1=(a1*a2);
+  a1=(a1+a4);
+  a1=(a1/a3);
+  a0=(a0/a3);
+  a1=(a1+a0);
+  a1=(-a1);
+  if (res[1]!=0) res[1][1]=a1;
+  a1=1.;
+  if (res[1]!=0) res[1][2]=a1;
+  if (res[1]!=0) res[1][3]=a1;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a1;
+  if (res[1]!=0) res[1][6]=a1;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_fun_jac_ut_xt_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_fun_jac_ut_xt_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    case 1: return casadi_s4;
+    case 2: return casadi_s5;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_fun_jac_ut_xt_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 3*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_cost/long_cost_y_hess.c

@@ -0,0 +1,209 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_cost_y_hess_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+#define casadi_s4 CASADI_PREFIX(s4)
+#define casadi_sq CASADI_PREFIX(sq)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+casadi_real casadi_sq(casadi_real x) { return x*x;}
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[3] = {0, 0, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s4[10] = {4, 4, 0, 0, 1, 3, 3, 2, 1, 2};
+
+/* long_cost_y_hess:(i0[3],i1,i2[],i3[6],i4[6])->(o0[4x4,3nz]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
+  a0=arg[3]? arg[3][0] : 0;
+  a1=arg[0]? arg[0][1] : 0;
+  a2=10.;
+  a2=(a1+a2);
+  a3=(1./a2);
+  a3=(a3/a2);
+  a3=(a0*a3);
+  if (res[0]!=0) res[0][0]=a3;
+  a3=(a0/a2);
+  a4=(a3/a2);
+  if (res[0]!=0) res[0][1]=a4;
+  a5=2.0000000000000001e-01;
+  a6=(a1+a1);
+  a6=(a5*a6);
+  a7=arg[4]? arg[4][4] : 0;
+  a6=(a6+a7);
+  a6=(a6/a2);
+  a8=arg[4]? arg[4][2] : 0;
+  a9=arg[0]? arg[0][0] : 0;
+  a8=(a8-a9);
+  a9=casadi_sq(a1);
+  a10=5.;
+  a9=(a9/a10);
+  a10=(a7*a1);
+  a9=(a9+a10);
+  a10=6.;
+  a9=(a9+a10);
+  a8=(a8-a9);
+  a8=(a8/a2);
+  a9=(a8/a2);
+  a6=(a6+a9);
+  a6=(a6/a2);
+  a8=(a8/a2);
+  a8=(a8/a2);
+  a6=(a6+a8);
+  a0=(a0*a6);
+  a7=(a7*a4);
+  a0=(a0+a7);
+  a7=2.;
+  a3=(a5*a3);
+  a7=(a7*a3);
+  a1=(a1+a1);
+  a5=(a5*a4);
+  a1=(a1*a5);
+  a7=(a7-a1);
+  a0=(a0-a7);
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_cost_y_hess_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_cost_y_hess_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_cost_y_hess_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_cost_y_hess_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    case 3: return casadi_s3;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_cost_y_hess_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s4;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_cost_y_hess_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_ode_fun.c

@@ -0,0 +1,157 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_ode_fun_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_expl_ode_fun:(i0[3],i1,i2[6])->(o0[3]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_ode_fun_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_in(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_ode_fun_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_ode_fun_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_ode_fun_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_ode_fun_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 3;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_ode_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 3*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_vde_adj.c

@@ -0,0 +1,163 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_vde_adj_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s2[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+static const casadi_int casadi_s3[8] = {4, 1, 0, 4, 0, 1, 2, 3};
+
+/* long_expl_vde_adj:(i0[3],i1[3],i2,i3[6])->(o0[4]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0;
+  a0=0.;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[1]? arg[1][0] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[1]? arg[1][1] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  a0=arg[1]? arg[1][2] : 0;
+  if (res[0]!=0) res[0][3]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_adj_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_in(void) { return 4;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_adj_n_out(void) { return 1;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_adj_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_adj_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s0;
+    case 2: return casadi_s1;
+    case 3: return casadi_s2;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_adj_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4;
+  if (sz_res) *sz_res = 1;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_adj_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 1*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/long_model/long_expl_vde_forw.c

@@ -0,0 +1,189 @@
+/* This file was automatically generated by CasADi 3.6.7.
+ *  It consists of: 
+ *   1) content generated by CasADi runtime: not copyrighted
+ *   2) template code copied from CasADi source: permissively licensed (MIT-0)
+ *   3) user code: owned by the user
+ *
+ */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* How to prefix internal symbols */
+#ifdef CASADI_CODEGEN_PREFIX
+  #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
+  #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
+  #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
+#else
+  #define CASADI_PREFIX(ID) long_expl_vde_forw_ ## ID
+#endif
+
+#include <math.h>
+
+#ifndef casadi_real
+#define casadi_real double
+#endif
+
+#ifndef casadi_int
+#define casadi_int int
+#endif
+
+/* Add prefix to internal symbols */
+#define casadi_f0 CASADI_PREFIX(f0)
+#define casadi_s0 CASADI_PREFIX(s0)
+#define casadi_s1 CASADI_PREFIX(s1)
+#define casadi_s2 CASADI_PREFIX(s2)
+#define casadi_s3 CASADI_PREFIX(s3)
+
+/* Symbol visibility in DLLs */
+#ifndef CASADI_SYMBOL_EXPORT
+  #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
+    #if defined(STATIC_LINKED)
+      #define CASADI_SYMBOL_EXPORT
+    #else
+      #define CASADI_SYMBOL_EXPORT __declspec(dllexport)
+    #endif
+  #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
+    #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
+  #else
+    #define CASADI_SYMBOL_EXPORT
+  #endif
+#endif
+
+static const casadi_int casadi_s0[7] = {3, 1, 0, 3, 0, 1, 2};
+static const casadi_int casadi_s1[15] = {3, 3, 0, 3, 6, 9, 0, 1, 2, 0, 1, 2, 0, 1, 2};
+static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
+static const casadi_int casadi_s3[10] = {6, 1, 0, 6, 0, 1, 2, 3, 4, 5};
+
+/* long_expl_vde_forw:(i0[3],i1[3x3],i2[3],i3,i4[6])->(o0[3],o1[3x3],o2[3]) */
+static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
+  casadi_real a0, a1;
+  a0=arg[0]? arg[0][1] : 0;
+  if (res[0]!=0) res[0][0]=a0;
+  a0=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][1]=a0;
+  a0=arg[3]? arg[3][0] : 0;
+  if (res[0]!=0) res[0][2]=a0;
+  a0=arg[1]? arg[1][1] : 0;
+  if (res[1]!=0) res[1][0]=a0;
+  a0=arg[1]? arg[1][2] : 0;
+  if (res[1]!=0) res[1][1]=a0;
+  a0=0.;
+  if (res[1]!=0) res[1][2]=a0;
+  a1=arg[1]? arg[1][4] : 0;
+  if (res[1]!=0) res[1][3]=a1;
+  a1=arg[1]? arg[1][5] : 0;
+  if (res[1]!=0) res[1][4]=a1;
+  if (res[1]!=0) res[1][5]=a0;
+  a1=arg[1]? arg[1][7] : 0;
+  if (res[1]!=0) res[1][6]=a1;
+  a1=arg[1]? arg[1][8] : 0;
+  if (res[1]!=0) res[1][7]=a1;
+  if (res[1]!=0) res[1][8]=a0;
+  a0=arg[2]? arg[2][1] : 0;
+  if (res[2]!=0) res[2][0]=a0;
+  a0=arg[2]? arg[2][2] : 0;
+  if (res[2]!=0) res[2][1]=a0;
+  a0=1.;
+  if (res[2]!=0) res[2][2]=a0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
+  return casadi_f0(arg, res, iw, w, mem);
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_alloc_mem(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_init_mem(int mem) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_free_mem(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_checkout(void) {
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_release(int mem) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_incref(void) {
+}
+
+CASADI_SYMBOL_EXPORT void long_expl_vde_forw_decref(void) {
+}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_in(void) { return 5;}
+
+CASADI_SYMBOL_EXPORT casadi_int long_expl_vde_forw_n_out(void) { return 3;}
+
+CASADI_SYMBOL_EXPORT casadi_real long_expl_vde_forw_default_in(casadi_int i) {
+  switch (i) {
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_in(casadi_int i) {
+  switch (i) {
+    case 0: return "i0";
+    case 1: return "i1";
+    case 2: return "i2";
+    case 3: return "i3";
+    case 4: return "i4";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const char* long_expl_vde_forw_name_out(casadi_int i) {
+  switch (i) {
+    case 0: return "o0";
+    case 1: return "o1";
+    case 2: return "o2";
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_in(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    case 3: return casadi_s2;
+    case 4: return casadi_s3;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT const casadi_int* long_expl_vde_forw_sparsity_out(casadi_int i) {
+  switch (i) {
+    case 0: return casadi_s0;
+    case 1: return casadi_s1;
+    case 2: return casadi_s0;
+    default: return 0;
+  }
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5;
+  if (sz_res) *sz_res = 3;
+  if (sz_iw) *sz_iw = 0;
+  if (sz_w) *sz_w = 0;
+  return 0;
+}
+
+CASADI_SYMBOL_EXPORT int long_expl_vde_forw_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
+  if (sz_arg) *sz_arg = 5*sizeof(const casadi_real*);
+  if (sz_res) *sz_res = 3*sizeof(casadi_real*);
+  if (sz_iw) *sz_iw = 0*sizeof(casadi_int);
+  if (sz_w) *sz_w = 0*sizeof(casadi_real);
+  return 0;
+}
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif

openpilot/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

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_long.c

@@ -0,0 +1,216 @@
+/*
+ * 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.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+#include "acados_solver_long.h"
+
+// blasfeo
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+
+#define NX     LONG_NX
+#define NZ     LONG_NZ
+#define NU     LONG_NU
+#define NP     LONG_NP
+#define NBX    LONG_NBX
+#define NBX0   LONG_NBX0
+#define NBU    LONG_NBU
+#define NSBX   LONG_NSBX
+#define NSBU   LONG_NSBU
+#define NSH    LONG_NSH
+#define NSG    LONG_NSG
+#define NSPHI  LONG_NSPHI
+#define NSHN   LONG_NSHN
+#define NSGN   LONG_NSGN
+#define NSPHIN LONG_NSPHIN
+#define NSBXN  LONG_NSBXN
+#define NS     LONG_NS
+#define NSN    LONG_NSN
+#define NG     LONG_NG
+#define NBXN   LONG_NBXN
+#define NGN    LONG_NGN
+#define NY0    LONG_NY0
+#define NY     LONG_NY
+#define NYN    LONG_NYN
+#define NH     LONG_NH
+#define NPHI   LONG_NPHI
+#define NHN    LONG_NHN
+#define NPHIN  LONG_NPHIN
+#define NR     LONG_NR
+
+
+int main()
+{
+
+    long_solver_capsule *acados_ocp_capsule = long_acados_create_capsule();
+    // there is an opportunity to change the number of shooting intervals in C without new code generation
+    int N = LONG_N;
+    // allocate the array and fill it accordingly
+    double* new_time_steps = NULL;
+    int status = long_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
+
+    if (status)
+    {
+        printf("long_acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    ocp_nlp_config *nlp_config = long_acados_get_nlp_config(acados_ocp_capsule);
+    ocp_nlp_dims *nlp_dims = long_acados_get_nlp_dims(acados_ocp_capsule);
+    ocp_nlp_in *nlp_in = long_acados_get_nlp_in(acados_ocp_capsule);
+    ocp_nlp_out *nlp_out = long_acados_get_nlp_out(acados_ocp_capsule);
+    ocp_nlp_solver *nlp_solver = long_acados_get_nlp_solver(acados_ocp_capsule);
+    void *nlp_opts = long_acados_get_nlp_opts(acados_ocp_capsule);
+
+    // initial condition
+    int idxbx0[NBX0];
+    idxbx0[0] = 0;
+    idxbx0[1] = 1;
+    idxbx0[2] = 2;
+
+    double lbx0[NBX0];
+    double ubx0[NBX0];
+    lbx0[0] = 0.0;
+    ubx0[0] = 0.0;
+    lbx0[1] = 0.0;
+    ubx0[1] = 0.0;
+    lbx0[2] = 0.0;
+    ubx0[2] = 0.0;
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+
+    // initialization for state values
+    double x_init[NX];
+    x_init[0] = 0.0;
+    x_init[1] = 0.0;
+    x_init[2] = 0.0;
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[2] = 0.0;
+    p[3] = 0.0;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    for (int ii = 0; ii <= N; ii++)
+    {
+        long_acados_update_params(acados_ocp_capsule, ii, p, NP);
+    }
+  
+
+    // prepare evaluation
+    int NTIMINGS = 1;
+    double min_time = 1e12;
+    double kkt_norm_inf;
+    double elapsed_time;
+    int sqp_iter;
+
+    double xtraj[NX * (N+1)];
+    double utraj[NU * N];
+
+
+    // solve ocp in loop
+    int rti_phase = 0;
+
+    for (int ii = 0; ii < NTIMINGS; ii++)
+    {
+        // initialize solution
+        for (int i = 0; i < N; i++)
+        {
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+        }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
+        ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
+        status = long_acados_solve(acados_ocp_capsule);
+        ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
+        min_time = MIN(elapsed_time, min_time);
+    }
+
+    /* print solution and statistics */
+    for (int ii = 0; ii <= nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
+    for (int ii = 0; ii < nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
+
+    printf("\n--- xtraj ---\n");
+    d_print_exp_tran_mat( NX, N+1, xtraj, NX);
+    printf("\n--- utraj ---\n");
+    d_print_exp_tran_mat( NU, N, utraj, NU );
+    // ocp_nlp_out_print(nlp_solver->dims, nlp_out);
+
+    printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
+
+    if (status == ACADOS_SUCCESS)
+    {
+        printf("long_acados_solve(): SUCCESS!\n");
+    }
+    else
+    {
+        printf("long_acados_solve() failed with status %d.\n", status);
+    }
+
+    // get solution
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
+    ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
+
+    long_acados_print_stats(acados_ocp_capsule);
+
+    printf("\nSolver info:\n");
+    printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
+           sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
+
+    // free solver
+    status = long_acados_free(acados_ocp_capsule);
+    if (status) {
+        printf("long_acados_free() returned status %d. \n", status);
+    }
+    // free solver capsule
+    status = long_acados_free_capsule(acados_ocp_capsule);
+    if (status) {
+        printf("long_acados_free_capsule() returned status %d. \n", status);
+    }
+
+    return status;
+}

openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/main_sim_long.c

@@ -0,0 +1,127 @@
+/*
+ * 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.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_long.h"
+
+#define NX     LONG_NX
+#define NZ     LONG_NZ
+#define NU     LONG_NU
+#define NP     LONG_NP
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = long_acados_sim_solver_create_capsule();
+    status = long_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = long_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = long_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = long_acados_get_sim_out(capsule);
+    void *acados_sim_dims = long_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[NX];
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+
+  
+    x_current[0] = 0.0;
+    x_current[1] = 0.0;
+    x_current[2] = 0.0;
+    
+  
+
+
+    // initial value for control input
+    double u0[NU];
+    u0[0] = 0.0;
+    // set parameters
+    double p[NP];
+    p[0] = -1.2;
+    p[1] = 1.2;
+    p[2] = 0.0;
+    p[3] = 0.0;
+    p[4] = 1.45;
+    p[5] = 0.75;
+
+    long_acados_sim_update_params(capsule, p, NP);
+  
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = long_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < NX; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = long_acados_sim_free(capsule);
+    if (status) {
+        printf("long_acados_sim_free() returned status %d. \n", status);
+    }
+
+    long_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}

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

@@ -0,0 +1,385 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from openpilot.cereal import log
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
+from openpilot.selfdrive.modeld.constants import index_function
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+
+if __name__ == '__main__':  # generating code
+  from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
+
+from casadi import SX, vertcat
+
+MODEL_NAME = 'long'
+LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
+
+LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise)
+
+X_DIM = 3
+U_DIM = 1
+PARAM_DIM = 6
+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.
+A_CHANGE_COST = 200.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .25
+LEAD_DANGER_FACTOR = 0.75
+LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
+
+# Fewer timestamps don't hurt performance and lead to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N) for idx in range(N+1)]
+
+T_IDXS = np.array(T_IDXS_LST)
+FCW_IDXS = T_IDXS < 5.0
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+COMFORT_BRAKE = 2.5
+STOP_DISTANCE = 6.0
+CRUISE_MIN_ACCEL = -1.2
+CRUISE_MAX_ACCEL = 1.6
+MIN_X_LEAD_FACTOR = 0.5
+
+def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.0
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 0.5
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+
+def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
+  if personality==log.LongitudinalPersonality.relaxed:
+    return 1.75
+  elif personality==log.LongitudinalPersonality.standard:
+    return 1.45
+  elif personality==log.LongitudinalPersonality.aggressive:
+    return 1.25
+  else:
+    raise NotImplementedError("Longitudinal personality not supported")
+
+def get_stopped_equivalence_factor(v_lead):
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
+
+def get_safe_obstacle_distance(v_ego, t_follow):
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = MODEL_NAME
+
+  # states
+  x_ego, v_ego, a_ego = SX.sym('x_ego'), SX.sym('v_ego'), SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  a_min = SX.sym('a_min')
+  a_max = SX.sym('a_max')
+  x_obstacle = SX.sym('x_obstacle')
+  a_prev = SX.sym('a_prev')
+  lead_t_follow = SX.sym('lead_t_follow')
+  lead_danger_factor = SX.sym('lead_danger_factor')
+  model.p = vertcat(a_min, a_max, x_obstacle, a_prev, lead_t_follow, lead_danger_factor)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+def gen_long_ocp():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = T_IDXS[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.zeros((COST_DIM, COST_DIM))
+  Q = np.zeros((COST_E_DIM, COST_E_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  a_min, a_max = ocp.model.p[0], ocp.model.p[1]
+  x_obstacle = ocp.model.p[2]
+  a_prev = ocp.model.p[3]
+  lead_t_follow = ocp.model.p[4]
+  lead_danger_factor = ocp.model.p[5]
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego, lead_t_follow)
+
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
+  costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
+           x_ego,
+           v_ego,
+           a_ego,
+           a_ego - a_prev,
+           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, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
+
+
+  # We put all constraint cost weights to 0 and only set them at runtime
+  cost_weights = np.zeros(CONSTR_DIM)
+  ocp.cost.zl = cost_weights
+  ocp.cost.Zl = cost_weights
+  ocp.cost.Zu = cost_weights
+  ocp.cost.zu = cost_weights
+
+  ocp.constraints.lh = np.zeros(CONSTR_DIM)
+  ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.idxsh = np.arange(CONSTR_DIM)
+
+  # The HPIPM solver can give decent solutions even when it is stopped early
+  # Which is critical for our purpose where compute time is strictly bounded
+  # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
+  # does not cause issues since the problem is well bounded.
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
+  ocp.solver_options.qp_solver_cond_N = 1
+
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
+  ocp.solver_options.qp_solver_iter_max = 10
+  ocp.solver_options.qp_tol = 1e-3
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = T_IDXS
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc:
+  def __init__(self, dt=DT_MDL):
+    self.dt = dt
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
+    self.reset()
+    self.source = LongitudinalPlanSource.cruise
+
+  def reset(self):
+    self.solver.reset()
+
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.v_solution = np.zeros(N+1)
+    self.a_solution = np.zeros(N+1)
+    self.j_solution = np.zeros(N)
+    self.a_prev = np.array(self.a_solution)
+    self.yref = np.zeros((N+1, COST_DIM))
+
+    for i in range(N):
+      self.solver.cost_set(i, "yref", self.yref[i])
+    self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params = np.zeros((N+1, PARAM_DIM))
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+
+    self.last_cloudlog_t = 0
+    self.crash_cnt = 0.0
+    self.solution_status = 0
+    # timers
+    self.solve_time = 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 = get_jerk_factor(personality)
+    a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
+    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]
+    self.set_cost_weights(cost_weights, constraint_cost_weights)
+
+  def set_cur_state(self, v, a):
+    v_prev = self.x0[1]
+    self.x0[1] = v
+    self.x0[2] = a
+    if abs(v_prev - v) > 2.:  # probably only helps if v < v_prev
+      for i in range(N+1):
+        self.solver.set(i, 'x', self.x0)
+
+  @staticmethod
+  def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
+    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
+    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
+    return lead_xv
+
+  def process_lead(self, lead):
+    v_ego = self.x0[1]
+    if lead is not None and lead.present:
+      x_lead = lead.dRel
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
+    else:
+      # Fake a fast lead car, so mpc can keep running in the same mode
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = MIN_X_LEAD_FACTOR * (v_ego + v_lead) * (v_ego - v_lead) / (-ACCEL_MIN * 2)
+    x_lead = np.clip(x_lead, min_x_lead, 1e8)
+    v_lead = np.clip(v_lead, 0.0, 1e8)
+    a_lead = np.clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+    return lead_xv
+
+  def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard):
+    t_follow = get_T_FOLLOW(personality)
+    v_ego = self.x0[1]
+
+    lead_xv_0 = self.process_lead(radarstate.leadOne)
+    lead_xv_1 = self.process_lead(radarstate.leadTwo)
+
+    # 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])
+
+    # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
+    # when the leads are no factor.
+    v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
+    # TODO does this make sense when max_a is negative?
+    v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 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)
+
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
+    self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
+
+    self.yref[:,:] = 0.0
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+
+    self.params[:,0] = ACCEL_MIN
+    self.params[:,1] = ACCEL_MAX
+    self.params[:,2] = np.min(x_obstacles, axis=1)
+    self.params[:,3] = np.copy(self.a_prev)
+    self.params[:,4] = t_follow
+    self.params[:,5] = LEAD_DANGER_FACTOR
+
+    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
+
+  def run(self):
+    for i in range(N+1):
+      self.solver.set(i, 'p', self.params[i])
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+
+    self.solution_status = self.solver.solve()
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+
+    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.a_prev = 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()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+import math
+import numpy as np
+
+import openpilot.cereal.messaging as messaging
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.common.constants import 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, LongitudinalPlanSource
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
+from openpilot.common.swaglog import cloudlog
+
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+
+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.4
+MIN_ALLOW_THROTTLE_SPEED = 2.5
+
+# Lookup table for turns
+_A_TOTAL_MAX_V = [1.7, 3.2]
+_A_TOTAL_MAX_BP = [20., 40.]
+
+def get_max_accel(v_ego):
+  return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
+
+def get_coast_accel(pitch):
+  return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
+
+def limit_accel_in_turns(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
+  a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
+  a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+  a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
+
+  return [a_target[0], min(a_target[1], a_x_allowed)]
+
+
+class LongitudinalPlanner(LongitudinalPlannerSP):
+  def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
+    self.CP = CP
+    self.mpc = LongitudinalMpc(dt=dt)
+    LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
+    self.fcw = False
+    self.dt = dt
+    self.allow_throttle = True
+
+    self.a_desired = init_a
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
+    self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
+    self.output_a_target = 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)
+
+  def update(self, sm):
+    LongitudinalPlannerSP.update(self, sm)
+
+    if len(sm['carControl'].orientationNED) == 3:
+      accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
+    else:
+      accel_coast = ACCEL_MAX
+
+    v_ego = sm['carState'].vEgo
+    v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
+    v_cruise = v_cruise_kph * CV.KPH_TO_MS
+    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
+
+    # No change cost when user is controlling the speed, or when standstill
+    prev_accel_constraint = not (reset_state or sm['carState'].standstill)
+
+    accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+    steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
+    accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
+
+    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_clip[0], accel_clip[1])
+
+    # Prevent divergence, smooth in current v_ego
+    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
+    throttle_probs = sm['modelV2'].meta.disengagePredictions.gasPressProbs
+    throttle_prob = throttle_probs[1] if len(throttle_probs) > 1 else 1.0
+    # Don't clip at low speeds since throttle_prob doesn't account for creep
+    self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
+
+    if not self.allow_throttle:
+      clipped_accel_coast = max(accel_coast, accel_clip[0])
+      clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
+      accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
+
+    # Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
+    v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
+
+    if force_slow_decel:
+      v_cruise = 0.0
+
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
+    self.mpc.update(sm['radarState'], v_cruise, 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
+    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
+
+    action_t =  self.CP.longitudinalActuatorDelay + DT_MDL
+    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+                                                                        action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
+    output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
+    output_should_stop_e2e = sm['modelV2'].action.shouldStop
+
+    if self.is_e2e(sm):
+      output_a_target = min(output_a_target_e2e, output_a_target_mpc)
+      self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+      if output_a_target < output_a_target_mpc:
+        self.mpc.source = LongitudinalPlanSource.e2e
+    else:
+      output_a_target = output_a_target_mpc
+      self.output_should_stop = 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
+
+  def publish(self, sm, pm):
+    plan_send = messaging.new_message('longitudinalPlan')
+
+    plan_send.valid = sm.all_checks()
+
+    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.present
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    longitudinalPlan.fcw = self.fcw
+
+    longitudinalPlan.aTarget = float(self.output_a_target)
+    longitudinalPlan.shouldStop = bool(self.output_should_stop)
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = bool(self.allow_throttle)
+
+    pm.send('longitudinalPlan', plan_send)
+
+    self.publish_longitudinal_plan_sp(sm, pm)