Merge branch 'devel-en' into devel-zhs

# Conflicts: # apk/ai.comma.plus.offroad.apk
2026-10-01 03:43:42 +08:00 · 2019-08-14 10:47:01 +10:00
parent 8e189d018d 1e0f989d3f
commit 2b51f256dc
129 changed files with 4607 additions and 901 deletions
@@ -12,7 +12,7 @@ from selfdrive.config import Conversions as CV
 from selfdrive.services import service_list
 from selfdrive.boardd.boardd import can_list_to_can_capnp
 from selfdrive.car.car_helpers import get_car, get_startup_alert
-from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
+from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import get_events, \
                                                 create_event, \
                                                 EventTypes as ET, \
@@ -21,6 +21,7 @@ from selfdrive.controls.lib.drive_helpers import get_events, \
 from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.alertmanager import AlertManager
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS
@@ -90,11 +91,16 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
    cal_status = sm['liveCalibration'].calStatus
    cal_perc = sm['liveCalibration'].calPerc

+  cal_rpy = [0,0,0]
  if cal_status != Calibration.CALIBRATED:
    if cal_status == Calibration.UNCALIBRATED:
      events.append(create_event('calibrationIncomplete', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT]))
    else:
      events.append(create_event('calibrationInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+  else:
+    rpy = sm['liveCalibration'].rpyCalib
+    if len(rpy) == 3:
+      cal_rpy = rpy

  # When the panda and controlsd do not agree on controls_allowed
  # we want to disengage openpilot. However the status from the panda goes through
@@ -112,7 +118,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space

  # Driver monitoring
  if sm.updated['driverMonitoring']:
-    driver_status.get_pose(sm['driverMonitoring'], params)
+    driver_status.get_pose(sm['driverMonitoring'], params, cal_rpy)

  if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS:
    events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
@@ -255,8 +261,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
  actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
                                              v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
  # Steering PID loop and lateral MPC
-  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
-                                                              CS.steeringPressed, CP, VM, path_plan)
+  actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)

  # Send a "steering required alert" if saturation count has reached the limit
  if LaC.sat_flag and CP.steerLimitAlert:
@@ -310,12 +315,11 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
  ldw_allowed = CS.vEgo > 12.5 and not blinker

  if len(list(sm['pathPlan'].rPoly)) == 4:
-    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
+    CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET) and right_lane_visible)
  if len(list(sm['pathPlan'].lPoly)) == 4:
-    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
+    CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET) and left_lane_visible)

  CC.hudControl.visualAlert = AM.visual_alert
-  CC.hudControl.audibleAlert = AM.audible_alert

  if not read_only:
    # send car controls over can
@@ -335,7 +339,7 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca
    "alertStatus": AM.alert_status,
    "alertBlinkingRate": AM.alert_rate,
    "alertType": AM.alert_type,
-    "alertSound": "",  # no EON sounds yet
+    "alertSound": AM.audible_alert,
    "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
    "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
    "canMonoTimes": list(CS.canMonoTimes),
@@ -372,7 +376,9 @@ def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, ca

  if CP.lateralTuning.which() == 'pid':
    dat.controlsState.lateralControlState.pidState = lac_log
-  else:
+  elif CP.lateralTuning.which() == 'lqr':
+    dat.controlsState.lateralControlState.lqrState = lac_log
+  elif CP.lateralTuning.which() == 'indi':
    dat.controlsState.lateralControlState.indiState = lac_log
  controlsstate.send(dat.to_bytes())

@@ -466,8 +472,10 @@ def controlsd_thread(gctx=None):

  if CP.lateralTuning.which() == 'pid':
    LaC = LatControlPID(CP)
-  else:
+  elif CP.lateralTuning.which() == 'indi':
    LaC = LatControlINDI(CP)
+  elif CP.lateralTuning.which() == 'lqr':
+    LaC = LatControlLQR(CP)

  driver_status = DriverStatus()

@@ -486,6 +494,9 @@ def controlsd_thread(gctx=None):
  sm['pathPlan'].sensorValid = True
  sm['pathPlan'].posenetValid = True

+  # detect sound card presence
+  sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE')
+
  # controlsd is driven by can recv, expected at 100Hz
  rk = Ratekeeper(100, print_delay_threshold=None)

@@ -498,7 +509,7 @@ def controlsd_thread(gctx=None):
  while True:
    # dragonpilot, don't check for param too often as it's a kernel call
    ts = sec_since_boot()
-    if ts - ts_last_check > 1.:
+    if ts - ts_last_check > 3.:
      dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
      ts_last_check = ts

@@ -528,6 +539,8 @@ def controlsd_thread(gctx=None):
      events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
    if not CS.canValid:
      events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if not sounds_available:
+      events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))

    if not dragon_toyota_stock_dsu:
      # Only allow engagement with brake pressed when stopped behind another stopped car
@@ -1,4 +1,4 @@
-from cereal import log
+from cereal import car, log
 from common.realtime import DT_CTRL
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.alerts import ALERTS
@@ -7,7 +7,8 @@ import copy

 AlertSize = log.ControlsState.AlertSize
 AlertStatus = log.ControlsState.AlertStatus
-
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert

 class AlertManager(object):

@@ -49,8 +50,8 @@ class AlertManager(object):
    self.alert_text_2 = ""
    self.alert_status = AlertStatus.normal
    self.alert_size = AlertSize.none
-    self.visual_alert = "none"
-    self.audible_alert = "none"
+    self.visual_alert = VisualAlert.none
+    self.audible_alert = AudibleAlert.none
    self.alert_rate = 0.

    if current_alert:
@@ -300,6 +300,13 @@ ALERTS = [
      AlertStatus.normal, AlertSize.mid,
      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),

+  Alert(
+      "soundsUnavailableNoEntry",
+      "openpilot Unavailable",
+      "Speaker not found",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
+
  Alert(
      "tooDistractedNoEntry",
      "openpilot Unavailable",
@@ -313,84 +320,84 @@ ALERTS = [
      "即刻接管控制",
      "系统过热",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "wrongGear",
      "即刻接管控制",
      "档位不在D档",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "calibrationInvalid",
      "即刻接管控制",
      "校准无效：请将传感器放于新的位置并重新校准",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "calibrationIncomplete",
      "即刻接管控制",
      "正在校准相机中",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "doorOpen",
      "即刻接管控制",
      "车门开启",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "seatbeltNotLatched",
      "即刻接管控制",
      "未系安全带",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "espDisabled",
      "即刻接管控制",
      "ESP关闭",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "lowBattery",
      "即刻接管控制",
      "电池电量过低",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "commIssue",
      "即刻接管控制",
      "雷达讯号错误：请重新发动车辆",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "radarFault",
      "即刻接管控制",
      "雷达讯号错误：请重新发动车辆",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "radarFault",
      "即刻接管控制",
      "雷达讯号错误：请重新发动车辆",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  Alert(
      "posenetInvalid",
      "TAKE CONTROL IMMEDIATELY",
      "Vision Failure: Check Camera View",
      AlertStatus.critical, AlertSize.full,
-      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
+      Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),

  # Cancellation alerts causing immediate disabling
  Alert(
@@ -676,6 +683,13 @@ ALERTS = [
      AlertStatus.normal, AlertSize.mid,
      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),

+  Alert(
+      "soundsUnavailablePermanent",
+      "Speaker not found",
+      "Reboot your EON",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
  Alert(
      "vehicleModelInvalid",
      "车辆参数识别失败",
@@ -5,31 +5,40 @@ from common.filter_simple import FirstOrderFilter
 from common.params import Params
 params = Params()

-_AWARENESS_TIME = 180        # 3 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME = 20.    # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME = 5.  # a second alert is issued 5s before start decelerating the car
-_DISTRACTED_TIME = 7.
-_DISTRACTED_PRE_TIME = 4.
-_DISTRACTED_PROMPT_TIME = 2.
-# model output refers to center of cropped image, so need to apply the x displacement offset
+_AWARENESS_TIME = 90.        # 1.5 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME_TILL_TERMINAL = 20.    # a first alert is issued 20s before expiration
+_AWARENESS_PROMPT_TIME_TILL_TERMINAL = 5.  # a second alert is issued 5s before start decelerating the car
+_DISTRACTED_TIME = 10.
+_DISTRACTED_PRE_TIME_TILL_TERMINAL = 7.
+_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 5.
+
 _FACE_THRESHOLD = 0.4
-_PITCH_WEIGHT = 1.5  # pitch matters a lot more
+_EYE_THRESHOLD = 0.4
+_BLINK_THRESHOLD = 0.2 # 0.225
+_PITCH_WEIGHT = 1.35 # 1.5  # pitch matters a lot more
 _METRIC_THRESHOLD = 0.4
-_PITCH_POS_ALLOWANCE = 0.08   # rad, to not be too sensitive on positive pitch
-_PITCH_NATURAL_OFFSET = 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
-_YAW_NATURAL_OFFSET = 0.08    # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
-_STD_THRESHOLD = 0.1          # above this standard deviation consider the measurement invalid
+_PITCH_POS_ALLOWANCE = 0.04 # 0.08  # rad, to not be too sensitive on positive pitch
+_PITCH_NATURAL_OFFSET = 0.12  # 0.1   # people don't seem to look straight when they drive relaxed, rather a bit up
+_YAW_NATURAL_OFFSET = 0.08  # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
 _DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 _VARIANCE_FILTER_TS = 20.     # 0.008Hz

 MAX_TERMINAL_ALERTS = 3      # not allowed to engage after 3 terminal alerts
+
+# model output refers to center of cropped image, so need to apply the x displacement offset
 RESIZED_FOCAL = 320.0
 H, W, FULL_W = 320, 160, 426

-def head_orientation_from_descriptor(angles_desc, pos_desc):
+class DistractedType(object):
+  NOT_DISTRACTED = 0
+  BAD_POSE = 1
+  BAD_BLINK = 2
+
+def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
  # the output of these angles are in device frame
  # so from driver's perspective, pitch is up and yaw is right
-  # TODO this should be calibrated
+
+  # TODO: calibrate based on position
  pitch_prnet = angles_desc[0]
  yaw_prnet = angles_desc[1]
  roll_prnet = angles_desc[2]
@@ -41,6 +50,11 @@ def head_orientation_from_descriptor(angles_desc, pos_desc):
  roll = roll_prnet
  pitch = pitch_prnet + pitch_focal_angle
  yaw = -yaw_prnet + yaw_focal_angle
+
+  # no calib for roll
+  pitch -= rpy_calib[1]
+  yaw -= rpy_calib[2]
+
  return np.array([roll, pitch, yaw])


@@ -52,15 +66,21 @@ class _DriverPose():
    self.yaw_offset = 0.
    self.pitch_offset = 0.

+class _DriverBlink():
+  def __init__(self):
+    self.left_blink = 0.
+    self.right_blink = 0.
+
+

 def _monitor_hysteresis(variance_level, monitor_valid_prev):
  var_thr = 0.63 if monitor_valid_prev else 0.37
  return variance_level < var_thr

-
 class DriverStatus():
  def __init__(self, monitor_on=False):
    self.pose = _DriverPose()
+    self.blink = _DriverBlink()
    self.monitor_on = monitor_on
    self.monitor_param_on = monitor_on
    self.monitor_valid = True   # variance needs to be low
@@ -72,7 +92,8 @@ class DriverStatus():
    self.ts_last_check = 0.
    self.face_detected = False
    self.terminal_alert_cnt = 0
-    self._set_timers()
+    self.step_change = 0.
+    self._set_timers(self.monitor_on)

    # dragonpilot
    self.dp_last_check = 0.
@@ -83,75 +104,87 @@ class DriverStatus():
    self.variance_filter.x = 0.
    self.monitor_valid = True

-  def _set_timers(self):
-    if self.monitor_on:
-      self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
-      self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME
+  def _set_timers(self, active_monitoring):
+    if active_monitoring:
+      # when falling back from passive mode to active mode, reset awareness to avoid false alert
+      if self.step_change == DT_CTRL / _AWARENESS_TIME:
+        self.awareness = 1.
+      self.threshold_pre = _DISTRACTED_PRE_TIME_TILL_TERMINAL / _DISTRACTED_TIME
+      self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME
      self.step_change = DT_CTRL / _DISTRACTED_TIME
    else:
-      self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME
-      self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME
+      self.threshold_pre = _AWARENESS_PRE_TIME_TILL_TERMINAL / _AWARENESS_TIME
+      self.threshold_prompt = _AWARENESS_PROMPT_TIME_TILL_TERMINAL / _AWARENESS_TIME
      self.step_change = DT_CTRL / _AWARENESS_TIME

-  def _is_driver_distracted(self, pose):
-    # to be tuned and to learn the driver's normal pose
+  def _is_driver_distracted(self, pose, blink):
+    # TODO: natural pose calib of each driver
    pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
    yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
    # add positive pitch allowance
    if pitch_error > 0.:
      pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
    pitch_error *= _PITCH_WEIGHT
-    metric = np.sqrt(yaw_error**2 + pitch_error**2)
-    # TODO: do something with the eye states and combine them with head pose
-    return 1 if metric > _METRIC_THRESHOLD else 0
+    pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
+
+    if pose_metric > _METRIC_THRESHOLD:
+      return DistractedType.BAD_POSE
+    elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
+      return DistractedType.BAD_BLINK
+    else:
+      return DistractedType.NOT_DISTRACTED


-  def get_pose(self, driver_monitoring, params):
+  def get_pose(self, driver_monitoring, params, cal_rpy):
    if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0:
      return

-    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition)
+    self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy)
+    self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD)
+    self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD)
    self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD

-    self.driver_distracted = self._is_driver_distracted(self.pose)
+    self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0
    # first order filters
    self.driver_distraction_filter.update(self.driver_distracted)
-    self.variance_high = False #driver_monitoring.std > _STD_THRESHOLD
-
-    self.variance_filter.update(self.variance_high)

    monitor_param_on_prev = self.monitor_param_on
-    monitor_valid_prev = self.monitor_valid

    # don't check for param too often as it's a kernel call
    ts = sec_since_boot()
-    if ts - self.ts_last_check > 1.:
+    if ts - self.ts_last_check > 3.:
      self.monitor_param_on = params.get("IsDriverMonitoringEnabled") == "1"
      self.ts_last_check = ts

-    self.monitor_valid = _monitor_hysteresis(self.variance_filter.x, monitor_valid_prev)
    self.monitor_on = self.monitor_valid and self.monitor_param_on
    if monitor_param_on_prev != self.monitor_param_on:
      self._reset_filters()
-    self._set_timers()
+    self._set_timers(self.monitor_on and self.face_detected)


  def update(self, events, driver_engaged, ctrl_active, standstill):
+    if driver_engaged:
+      self.awareness = 1.
+      return events
+
    # don't check for param too often as it's a kernel call
    ts = sec_since_boot()
-    if ts - self.dp_last_check > 1.:
+    if ts - self.dp_last_check > 3.:
      self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True
      self.dp_last_check = ts

+    if not self.dragon_enable_driver_safety_check:
+      return events
+
    driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on)
    awareness_prev = self.awareness

-    if (driver_engaged and self.awareness > 0.) or not ctrl_active:
+    if (driver_engaged and self.awareness > 0) or not ctrl_active:
      # always reset if driver is in control (unless we are in red alert state) or op isn't active
-      self.awareness = 1.
+      self.awareness = min(self.awareness + (2.75*(1.-self.awareness)+1.25)*self.step_change, 1.)

-    # only update if face is detected, driver is distracted and distraction filter is high
-    if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
+    # should always be counting if distracted unless at standstill and reaching orange
+    if ((not self.monitor_on or (self.monitor_on and not self.face_detected)) or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
       not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
      self.awareness = max(self.awareness - self.step_change, -0.1)

@@ -167,7 +200,7 @@ class DriverStatus():
    elif self.awareness <= self.threshold_pre:
      # pre green alert
      alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive'
-    if alert is not None and self.dragon_enable_driver_safety_check:
+    if alert is not None:
      events.append(create_event(alert, [ET.WARNING]))

    return events
@@ -0,0 +1,74 @@
+from common.numpy_fast import interp
+import numpy as np
+from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, compute_path_pinv
+
+CAMERA_OFFSET = 0.06  # m from center car to camera
+
+
+def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
+  # This will improve behaviour when lanes suddenly widen
+  lane_width = min(4.0, lane_width)
+  l_prob = l_prob * interp(abs(l_poly[3]), [2, 2.5], [1.0, 0.0])
+  r_prob = r_prob * interp(abs(r_poly[3]), [2, 2.5], [1.0, 0.0])
+
+  path_from_left_lane = l_poly.copy()
+  path_from_left_lane[3] -= lane_width / 2.0
+  path_from_right_lane = r_poly.copy()
+  path_from_right_lane[3] += lane_width / 2.0
+
+  lr_prob = l_prob + r_prob - l_prob * r_prob
+
+  d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+  return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
+
+
+class LanePlanner(object):
+  def __init__(self):
+    self.l_poly = [0., 0., 0., 0.]
+    self.r_poly = [0., 0., 0., 0.]
+    self.p_poly = [0., 0., 0., 0.]
+    self.d_poly = [0., 0., 0., 0.]
+
+    self.lane_width_estimate = 3.7
+    self.lane_width_certainty = 1.0
+    self.lane_width = 3.7
+
+    self.l_prob = 0.
+    self.r_prob = 0.
+    self.lr_prob = 0.
+
+    self._path_pinv = compute_path_pinv()
+    self.x_points = np.arange(50)
+
+  def parse_model(self, md):
+    if len(md.leftLane.poly):
+      self.l_poly = np.array(md.leftLane.poly)
+      self.r_poly = np.array(md.rightLane.poly)
+      self.p_poly = np.array(md.path.poly)
+    else:
+      self.l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
+      self.r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
+      self.p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
+    self.l_prob = md.leftLane.prob  # left line prob
+    self.r_prob = md.rightLane.prob  # right line prob
+
+  def update_lane(self, v_ego):
+    # only offset left and right lane lines; offsetting p_poly does not make sense
+    self.l_poly[3] += CAMERA_OFFSET
+    self.r_poly[3] += CAMERA_OFFSET
+
+    self.lr_prob = self.l_prob + self.r_prob - self.l_prob * self.r_prob
+
+    # Find current lanewidth
+    self.lane_width_certainty += 0.05 * (self.l_prob * self.r_prob - self.lane_width_certainty)
+    current_lane_width = abs(self.l_poly[3] - self.r_poly[3])
+    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
+    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
+    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
+                      (1 - self.lane_width_certainty) * speed_lane_width
+
+    self.d_poly = calc_d_poly(self.l_poly, self.r_poly, self.p_poly, self.l_prob, self.r_prob, self.lane_width)
+
+  def update(self, v_ego, md):
+    self.parse_model(md)
+    self.update_lane(v_ego)
@@ -60,30 +60,3 @@ def compute_path_pinv(l=50):

 def model_polyfit(points, path_pinv):
  return np.dot(path_pinv, [float(x) for x in points])
-
-
-def calc_desired_path(l_poly,
-                      r_poly,
-                      p_poly,
-                      l_prob,
-                      r_prob,
-                      p_prob,
-                      speed,
-                      lane_width=None):
-  # this function computes the poly for the center of the lane, averaging left and right polys
-  if lane_width is None:
-    lane_width = interp(speed, _LANE_WIDTH_BP, _LANE_WIDTH_V)
-
-  # lanes in US are ~3.6m wide
-  half_lane_poly = np.array([0., 0., 0., lane_width / 2.])
-  if l_prob + r_prob > 0.01:
-    c_poly = ((l_poly - half_lane_poly) * l_prob +
-              (r_poly + half_lane_poly) * r_prob) / (l_prob + r_prob)
-    c_prob = l_prob + r_prob - l_prob * r_prob
-  else:
-    c_poly = np.zeros(4)
-    c_prob = 0.
-
-  p_weight = 1.  # predicted path weight relatively to the center of the lane
-  d_poly = list((c_poly * c_prob + p_poly * p_prob * p_weight) / (c_prob + p_prob * p_weight))
-  return d_poly, c_poly, c_prob
@@ -47,7 +47,7 @@ class LatControlINDI(object):
    self.output_steer = 0.
    self.counter = 0

-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
    # Update Kalman filter
    y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
@@ -0,0 +1,72 @@
+import numpy as np
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from common.numpy_fast import clip
+from cereal import log
+
+
+class LatControlLQR(object):
+  def __init__(self, CP, rate=100):
+    self.sat_flag = False
+    self.scale = CP.lateralTuning.lqr.scale
+    self.ki = CP.lateralTuning.lqr.ki
+
+
+    self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2))
+    self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1))
+    self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2))
+    self.K = np.array(CP.lateralTuning.lqr.k).reshape((1,2))
+    self.L = np.array(CP.lateralTuning.lqr.l).reshape((2,1))
+    self.dc_gain = CP.lateralTuning.lqr.dcGain
+
+    self.x_hat = np.array([[0], [0]])
+    self.i_unwind_rate = 0.3 / rate
+    self.i_rate = 1.0 / rate
+
+    self.reset()
+
+  def reset(self):
+    self.i_lqr = 0.0
+    self.output_steer = 0.0
+
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
+    lqr_log = log.ControlsState.LateralLQRState.new_message()
+
+    torque_scale = (0.45 + v_ego / 60.0)**2  # Scale actuator model with speed
+
+    # Subtract offset. Zero angle should correspond to zero torque
+    self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
+    angle_steers -= path_plan.angleOffset
+
+    # Update Kalman filter
+    angle_steers_k = float(self.C.dot(self.x_hat))
+    e = angle_steers - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
+
+    if v_ego < 0.3 or not active:
+      lqr_log.active = False
+      self.reset()
+    else:
+      lqr_log.active = True
+
+      # LQR
+      u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
+
+      # Integrator
+      if steer_override:
+        self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
+      else:
+        self.i_lqr += self.ki * self.i_rate * (self.angle_steers_des - angle_steers_k)
+
+      lqr_output = torque_scale * u_lqr / self.scale
+      self.i_lqr = clip(self.i_lqr, -1.0 - lqr_output, 1.0 - lqr_output) # (LQR + I) has to be between -1 and 1
+
+      self.output_steer = lqr_output + self.i_lqr
+
+      # Clip output
+      steers_max = get_steer_max(CP, v_ego)
+      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+
+    lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
+    lqr_log.i = self.i_lqr
+    lqr_log.output = self.output_steer
+    return self.output_steer, float(self.angle_steers_des), lqr_log
@@ -14,7 +14,7 @@ class LatControlPID(object):
  def reset(self):
    self.pid.reset()

-  def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
+  def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
    pid_log = log.ControlsState.LateralPIDState.new_message()
    pid_log.steerAngle = float(angle_steers)
    pid_log.steerRate = float(angle_steers_rate)
@@ -23,8 +23,8 @@ int main( )
  OnlineData v_ref; // m/s
  OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
  OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
-  OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
-  OnlineData l_prob, r_prob, p_prob;
+  OnlineData d_poly_r0, d_poly_r1, d_poly_r2, d_poly_r3;
+  OnlineData l_prob, r_prob;
  OnlineData lane_width;

  Control t;
@@ -39,26 +39,13 @@ int main( )

  auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
  auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
-  auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
+  auto poly_d = d_poly_r0*(xx*xx*xx) + d_poly_r1*(xx*xx) + d_poly_r2*xx + d_poly_r3;

-  auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
-  auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
-  auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
-
-  // given the lane width estimate, this is where we estimate the path given lane lines
-  auto path_from_left_lane = poly_l - lane_width/2.0;
-  auto path_from_right_lane = poly_r + lane_width/2.0;
-
-  // if the lanes are visible, drive in the center, otherwise follow the path
-  auto path = lr_prob * (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
-    + (1-lr_prob) * poly_p;
-
-  auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
-    + (1-lr_prob) * angle_p;
+  auto angle_d = atan(3*d_poly_r0*xx*xx + 2*d_poly_r1*xx + d_poly_r2);

  // When the lane is not visible, use an estimate of its position
-  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (path + lane_width/2.0);
-  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (path - lane_width/2.0);
+  auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (poly_d + lane_width/2.0);
+  auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (poly_d - lane_width/2.0);

  auto c_left_lane = exp(-(weighted_left_lane - yy));
  auto c_right_lane = exp(weighted_right_lane - yy);
@@ -67,12 +54,12 @@ int main( )
  Function h;

  // Distance errors
-  h << path - yy;
+  h << poly_d - yy;
  h << lr_prob * c_left_lane;
  h << lr_prob * c_right_lane;

  // Heading error
-  h << (v_ref + 1.0 ) * (angle - psi);
+  h << (v_ref + 1.0 ) * (angle_d - psi);

  // Angular rate error
  h << (v_ref + 1.0 ) * t;
@@ -88,12 +75,12 @@ int main( )
  Function hN;

  // Distance errors
-  hN << path - yy;
+  hN << poly_d - yy;
  hN << l_prob * c_left_lane;
  hN << r_prob * c_right_lane;

  // Heading errors
-  hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+  hN << (2.0 * v_ref + 1.0 ) * (angle_d - psi);

  BMatrix QN(4,4); QN.setAll(true);
  // QN(0,0) = 1.0;
@@ -125,7 +112,7 @@ int main( )
  ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
  // more than absolute max steer angle
  ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
-  ocp.setNOD(18);
+  ocp.setNOD(17);

  OCPexport mpc(ocp);
  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
@@ -65,8 +65,8 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
 }

 int run_mpc(state_t * x0, log_t * solution,
-             double l_poly[4], double r_poly[4], double p_poly[4],
-             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width){
+             double l_poly[4], double r_poly[4], double d_poly[4],
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){

  int    i;

@@ -84,16 +84,15 @@ int run_mpc(state_t * x0, log_t * solution,
    acadoVariables.od[i+8] = r_poly[2];
    acadoVariables.od[i+9] = r_poly[3];

-    acadoVariables.od[i+10] = p_poly[0];
-    acadoVariables.od[i+11] = p_poly[1];
-    acadoVariables.od[i+12] = p_poly[2];
-    acadoVariables.od[i+13] = p_poly[3];
+    acadoVariables.od[i+10] = d_poly[0];
+    acadoVariables.od[i+11] = d_poly[1];
+    acadoVariables.od[i+12] = d_poly[2];
+    acadoVariables.od[i+13] = d_poly[3];


    acadoVariables.od[i+14] = l_prob;
    acadoVariables.od[i+15] = r_prob;
-    acadoVariables.od[i+16] = p_prob;
-    acadoVariables.od[i+17] = lane_width;
+    acadoVariables.od[i+16] = lane_width;

  }

@@ -64,7 +64,7 @@ extern "C"
 /** Number of control/estimation intervals. */
 #define ACADO_N 20
 /** Number of online data values. */
-#define ACADO_NOD 18
+#define ACADO_NOD 17
 /** Number of path constraints. */
 #define ACADO_NPAC 0
 /** Number of control variables. */
@@ -114,11 +114,11 @@ real_t x[ 84 ];
 */
 real_t u[ 20 ];

-/** Matrix of size: 21 x 18 (row major format)
+/** Matrix of size: 21 x 17 (row major format)
 * 
 *  Matrix containing 21 online data vectors.
 */
-real_t od[ 378 ];
+real_t od[ 357 ];

 /** Column vector of size: 100
 * 
@@ -160,14 +160,14 @@ real_t rhs_aux[ 14 ];

 real_t rk_ttt;

-/** Row vector of size: 43 */
-real_t rk_xxx[ 43 ];
+/** Row vector of size: 42 */
+real_t rk_xxx[ 42 ];

 /** Matrix of size: 4 x 24 (row major format) */
 real_t rk_kkk[ 96 ];

-/** Row vector of size: 43 */
-real_t state[ 43 ];
+/** Row vector of size: 42 */
+real_t state[ 42 ];

 /** Column vector of size: 80 */
 real_t d[ 80 ];
@@ -184,11 +184,11 @@ real_t evGx[ 320 ];
 /** Column vector of size: 80 */
 real_t evGu[ 80 ];

-/** Column vector of size: 21 */
-real_t objAuxVar[ 21 ];
+/** Column vector of size: 11 */
+real_t objAuxVar[ 11 ];

-/** Row vector of size: 23 */
-real_t objValueIn[ 23 ];
+/** Row vector of size: 22 */
+real_t objValueIn[ 22 ];

 /** Row vector of size: 30 */
 real_t objValueOut[ 30 ];
@@ -118,7 +118,6 @@ acadoWorkspace.rk_xxx[38] = rk_eta[38];
 acadoWorkspace.rk_xxx[39] = rk_eta[39];
 acadoWorkspace.rk_xxx[40] = rk_eta[40];
 acadoWorkspace.rk_xxx[41] = rk_eta[41];
-acadoWorkspace.rk_xxx[42] = rk_eta[42];

 for (run1 = 0; run1 < 1; ++run1)
 {
@@ -43,24 +43,23 @@ acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2];
 acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3];

 acadoWorkspace.state[24] = acadoVariables.u[lRun1];
-acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[lRun1 * 17 + 16];

 ret = acado_integrate(acadoWorkspace.state, 1, lRun1);

@@ -99,51 +98,41 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
 const real_t* xd = in;
 const real_t* u = in + 4;
 const real_t* od = in + 5;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
 real_t* a = acadoWorkspace.objAuxVar;

 /* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);

 /* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
 out[1] = (((od[14]+od[15])-(od[14]*od[15]))*a[0]);
 out[2] = (((od[14]+od[15])-(od[14]*od[15]))*a[1]);
-out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
+out[3] = ((od[1]+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
 out[4] = ((od[1]+(real_t)(1.0000000000000000e+00))*u[0]);
-out[5] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[5] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
 out[6] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
 out[7] = (real_t)(0.0000000000000000e+00);
 out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
-out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[9]);
+out[9] = (((od[14]+od[15])-(od[14]*od[15]))*a[4]);
+out[10] = (((od[14]+od[15])-(od[14]*od[15]))*a[5]);
 out[11] = (real_t)(0.0000000000000000e+00);
 out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[12]);
-out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[13]);
+out[13] = (((od[14]+od[15])-(od[14]*od[15]))*a[7]);
+out[14] = (((od[14]+od[15])-(od[14]*od[15]))*a[8]);
 out[15] = (real_t)(0.0000000000000000e+00);
 out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[17] = ((od[1]+(real_t)(1.0000000000000000e+00))*a[10]);
 out[18] = (real_t)(0.0000000000000000e+00);
 out[19] = ((od[1]+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
 out[20] = (real_t)(0.0000000000000000e+00);
@@ -162,50 +151,40 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
 {
 const real_t* xd = in;
 const real_t* od = in + 4;
-/* Vector of auxiliary variables; number of elements: 21. */
+/* Vector of auxiliary variables; number of elements: 11. */
 real_t* a = acadoWorkspace.objAuxVar;

 /* Compute intermediate quantities: */
-a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
-a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
-a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
-a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
-a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
-a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
-a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
-a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
-a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
-a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
-a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
-a[16] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8]),2))));
-a[17] = ((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[6])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[7]))*a[16]);
-a[18] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
-a[19] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
-a[20] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[19]);
+a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
+a[3] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])+(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
+a[4] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[3]);
+a[5] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[3]);
+a[6] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-(od[16]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
+a[7] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[6]);
+a[8] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[6]);
+a[9] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12]),2))));
+a[10] = ((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[10])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[11]))*a[9]);

 /* Compute outputs: */
-out[0] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-xd[1]);
+out[0] = (((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])-xd[1]);
 out[1] = (od[14]*a[0]);
 out[2] = (od[15]*a[1]);
-out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[2])+(od[15]*a[3])))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[4]))-xd[2]));
-out[4] = (((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[5])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])));
+out[3] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(a[2]-xd[2]));
+out[4] = (((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]);
 out[5] = ((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00));
 out[6] = (real_t)(0.0000000000000000e+00);
 out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (od[14]*a[8]);
-out[9] = (od[14]*a[9]);
+out[8] = (od[14]*a[4]);
+out[9] = (od[14]*a[5]);
 out[10] = (real_t)(0.0000000000000000e+00);
 out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = (od[15]*a[12]);
-out[13] = (od[15]*a[13]);
+out[12] = (od[15]*a[7]);
+out[13] = (od[15]*a[8]);
 out[14] = (real_t)(0.0000000000000000e+00);
 out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*a[15])+(od[15]*a[17])))*a[18])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*a[20])));
+out[16] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*a[10]);
 out[17] = (real_t)(0.0000000000000000e+00);
 out[18] = ((((real_t)(2.0000000000000000e+00)*od[1])+(real_t)(1.0000000000000000e+00))*((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)));
 out[19] = (real_t)(0.0000000000000000e+00);
@@ -307,24 +286,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1];
 acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3];
 acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[runObj * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[runObj * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[runObj * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[runObj * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[runObj * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[runObj * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[runObj * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[runObj * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[runObj * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[runObj * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[runObj * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[runObj * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[runObj * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[runObj * 17 + 16];

 acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0];
@@ -342,24 +320,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
 acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
 acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
 acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );

 acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
@@ -4966,24 +4943,23 @@ acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1];
 acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2];
 acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3];
 acadoWorkspace.state[24] = acadoVariables.u[index];
-acadoWorkspace.state[25] = acadoVariables.od[index * 18];
-acadoWorkspace.state[26] = acadoVariables.od[index * 18 + 1];
-acadoWorkspace.state[27] = acadoVariables.od[index * 18 + 2];
-acadoWorkspace.state[28] = acadoVariables.od[index * 18 + 3];
-acadoWorkspace.state[29] = acadoVariables.od[index * 18 + 4];
-acadoWorkspace.state[30] = acadoVariables.od[index * 18 + 5];
-acadoWorkspace.state[31] = acadoVariables.od[index * 18 + 6];
-acadoWorkspace.state[32] = acadoVariables.od[index * 18 + 7];
-acadoWorkspace.state[33] = acadoVariables.od[index * 18 + 8];
-acadoWorkspace.state[34] = acadoVariables.od[index * 18 + 9];
-acadoWorkspace.state[35] = acadoVariables.od[index * 18 + 10];
-acadoWorkspace.state[36] = acadoVariables.od[index * 18 + 11];
-acadoWorkspace.state[37] = acadoVariables.od[index * 18 + 12];
-acadoWorkspace.state[38] = acadoVariables.od[index * 18 + 13];
-acadoWorkspace.state[39] = acadoVariables.od[index * 18 + 14];
-acadoWorkspace.state[40] = acadoVariables.od[index * 18 + 15];
-acadoWorkspace.state[41] = acadoVariables.od[index * 18 + 16];
-acadoWorkspace.state[42] = acadoVariables.od[index * 18 + 17];
+acadoWorkspace.state[25] = acadoVariables.od[index * 17];
+acadoWorkspace.state[26] = acadoVariables.od[index * 17 + 1];
+acadoWorkspace.state[27] = acadoVariables.od[index * 17 + 2];
+acadoWorkspace.state[28] = acadoVariables.od[index * 17 + 3];
+acadoWorkspace.state[29] = acadoVariables.od[index * 17 + 4];
+acadoWorkspace.state[30] = acadoVariables.od[index * 17 + 5];
+acadoWorkspace.state[31] = acadoVariables.od[index * 17 + 6];
+acadoWorkspace.state[32] = acadoVariables.od[index * 17 + 7];
+acadoWorkspace.state[33] = acadoVariables.od[index * 17 + 8];
+acadoWorkspace.state[34] = acadoVariables.od[index * 17 + 9];
+acadoWorkspace.state[35] = acadoVariables.od[index * 17 + 10];
+acadoWorkspace.state[36] = acadoVariables.od[index * 17 + 11];
+acadoWorkspace.state[37] = acadoVariables.od[index * 17 + 12];
+acadoWorkspace.state[38] = acadoVariables.od[index * 17 + 13];
+acadoWorkspace.state[39] = acadoVariables.od[index * 17 + 14];
+acadoWorkspace.state[40] = acadoVariables.od[index * 17 + 15];
+acadoWorkspace.state[41] = acadoVariables.od[index * 17 + 16];

 acado_integrate(acadoWorkspace.state, index == 0, index);

@@ -5026,24 +5002,23 @@ else
 {
 acadoWorkspace.state[24] = acadoVariables.u[19];
 }
-acadoWorkspace.state[25] = acadoVariables.od[360];
-acadoWorkspace.state[26] = acadoVariables.od[361];
-acadoWorkspace.state[27] = acadoVariables.od[362];
-acadoWorkspace.state[28] = acadoVariables.od[363];
-acadoWorkspace.state[29] = acadoVariables.od[364];
-acadoWorkspace.state[30] = acadoVariables.od[365];
-acadoWorkspace.state[31] = acadoVariables.od[366];
-acadoWorkspace.state[32] = acadoVariables.od[367];
-acadoWorkspace.state[33] = acadoVariables.od[368];
-acadoWorkspace.state[34] = acadoVariables.od[369];
-acadoWorkspace.state[35] = acadoVariables.od[370];
-acadoWorkspace.state[36] = acadoVariables.od[371];
-acadoWorkspace.state[37] = acadoVariables.od[372];
-acadoWorkspace.state[38] = acadoVariables.od[373];
-acadoWorkspace.state[39] = acadoVariables.od[374];
-acadoWorkspace.state[40] = acadoVariables.od[375];
-acadoWorkspace.state[41] = acadoVariables.od[376];
-acadoWorkspace.state[42] = acadoVariables.od[377];
+acadoWorkspace.state[25] = acadoVariables.od[340];
+acadoWorkspace.state[26] = acadoVariables.od[341];
+acadoWorkspace.state[27] = acadoVariables.od[342];
+acadoWorkspace.state[28] = acadoVariables.od[343];
+acadoWorkspace.state[29] = acadoVariables.od[344];
+acadoWorkspace.state[30] = acadoVariables.od[345];
+acadoWorkspace.state[31] = acadoVariables.od[346];
+acadoWorkspace.state[32] = acadoVariables.od[347];
+acadoWorkspace.state[33] = acadoVariables.od[348];
+acadoWorkspace.state[34] = acadoVariables.od[349];
+acadoWorkspace.state[35] = acadoVariables.od[350];
+acadoWorkspace.state[36] = acadoVariables.od[351];
+acadoWorkspace.state[37] = acadoVariables.od[352];
+acadoWorkspace.state[38] = acadoVariables.od[353];
+acadoWorkspace.state[39] = acadoVariables.od[354];
+acadoWorkspace.state[40] = acadoVariables.od[355];
+acadoWorkspace.state[41] = acadoVariables.od[356];

 acado_integrate(acadoWorkspace.state, 1, 19);

@@ -5114,24 +5089,23 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
 acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
 acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 18];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 18 + 1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 18 + 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 18 + 3];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 18 + 4];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 18 + 5];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 18 + 6];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 18 + 7];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 18 + 8];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 18 + 9];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 18 + 10];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 18 + 11];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 18 + 12];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 18 + 13];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 18 + 14];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 18 + 15];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 18 + 16];
-acadoWorkspace.objValueIn[22] = acadoVariables.od[lRun1 * 18 + 17];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 17];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 17 + 1];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 17 + 2];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 17 + 3];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[lRun1 * 17 + 4];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[lRun1 * 17 + 5];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[lRun1 * 17 + 6];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[lRun1 * 17 + 7];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[lRun1 * 17 + 8];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[lRun1 * 17 + 9];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[lRun1 * 17 + 10];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[lRun1 * 17 + 11];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[lRun1 * 17 + 12];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[lRun1 * 17 + 13];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[lRun1 * 17 + 14];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[lRun1 * 17 + 15];
+acadoWorkspace.objValueIn[21] = acadoVariables.od[lRun1 * 17 + 16];

 acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5];
@@ -5144,24 +5118,23 @@ acadoWorkspace.objValueIn[0] = acadoVariables.x[80];
 acadoWorkspace.objValueIn[1] = acadoVariables.x[81];
 acadoWorkspace.objValueIn[2] = acadoVariables.x[82];
 acadoWorkspace.objValueIn[3] = acadoVariables.x[83];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[360];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[361];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[362];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[363];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[364];
-acadoWorkspace.objValueIn[9] = acadoVariables.od[365];
-acadoWorkspace.objValueIn[10] = acadoVariables.od[366];
-acadoWorkspace.objValueIn[11] = acadoVariables.od[367];
-acadoWorkspace.objValueIn[12] = acadoVariables.od[368];
-acadoWorkspace.objValueIn[13] = acadoVariables.od[369];
-acadoWorkspace.objValueIn[14] = acadoVariables.od[370];
-acadoWorkspace.objValueIn[15] = acadoVariables.od[371];
-acadoWorkspace.objValueIn[16] = acadoVariables.od[372];
-acadoWorkspace.objValueIn[17] = acadoVariables.od[373];
-acadoWorkspace.objValueIn[18] = acadoVariables.od[374];
-acadoWorkspace.objValueIn[19] = acadoVariables.od[375];
-acadoWorkspace.objValueIn[20] = acadoVariables.od[376];
-acadoWorkspace.objValueIn[21] = acadoVariables.od[377];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[340];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[341];
+acadoWorkspace.objValueIn[6] = acadoVariables.od[342];
+acadoWorkspace.objValueIn[7] = acadoVariables.od[343];
+acadoWorkspace.objValueIn[8] = acadoVariables.od[344];
+acadoWorkspace.objValueIn[9] = acadoVariables.od[345];
+acadoWorkspace.objValueIn[10] = acadoVariables.od[346];
+acadoWorkspace.objValueIn[11] = acadoVariables.od[347];
+acadoWorkspace.objValueIn[12] = acadoVariables.od[348];
+acadoWorkspace.objValueIn[13] = acadoVariables.od[349];
+acadoWorkspace.objValueIn[14] = acadoVariables.od[350];
+acadoWorkspace.objValueIn[15] = acadoVariables.od[351];
+acadoWorkspace.objValueIn[16] = acadoVariables.od[352];
+acadoWorkspace.objValueIn[17] = acadoVariables.od[353];
+acadoWorkspace.objValueIn[18] = acadoVariables.od[354];
+acadoWorkspace.objValueIn[19] = acadoVariables.od[355];
+acadoWorkspace.objValueIn[20] = acadoVariables.od[356];
 acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
 acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
 acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
@@ -24,8 +24,8 @@ typedef struct {

 void init(double pathCost, double laneCost, double headingCost, double steerRateCost);
 int run_mpc(state_t * x0, log_t * solution,
-             double l_poly[4], double r_poly[4], double p_poly[4],
-             double l_prob, double r_prob, double p_prob, double curvature_factor, double v_ref, double lane_width);
+             double l_poly[4], double r_poly[4], double d_poly[4],
+             double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width);
 """)

 libmpc = ffi.dlopen(libmpc_fn)
@@ -1,66 +0,0 @@
-from common.numpy_fast import interp
-import numpy as np
-from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv
-
-CAMERA_OFFSET = 0.06  # m from center car to camera
-
-
-class ModelParser(object):
-  def __init__(self):
-    self.d_poly = [0., 0., 0., 0.]
-    self.c_poly = [0., 0., 0., 0.]
-    self.c_prob = 0.
-    self.last_model = 0.
-    self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1
-    self._path_pinv = compute_path_pinv()
-
-    self.lane_width_estimate = 3.5
-    self.lane_width_certainty = 1.0
-    self.lane_width = 3.5
-    self.l_prob = 0.
-    self.r_prob = 0.
-    self.x_points = np.arange(50)
-
-  def update(self, v_ego, md):
-    if len(md.leftLane.poly):
-      l_poly = np.array(md.leftLane.poly)
-      r_poly = np.array(md.rightLane.poly)
-      p_poly = np.array(md.path.poly)
-    else:
-      l_poly = model_polyfit(md.leftLane.points, self._path_pinv)  # left line
-      r_poly = model_polyfit(md.rightLane.points, self._path_pinv)  # right line
-      p_poly = model_polyfit(md.path.points, self._path_pinv)  # predicted path
-
-    # only offset left and right lane lines; offsetting p_poly does not make sense
-    l_poly[3] += CAMERA_OFFSET
-    r_poly[3] += CAMERA_OFFSET
-
-    p_prob = 1.  # model does not tell this probability yet, so set to 1 for now
-    l_prob = md.leftLane.prob  # left line prob
-    r_prob = md.rightLane.prob  # right line prob
-
-    # Find current lanewidth
-    lr_prob = l_prob * r_prob
-    self.lane_width_certainty += 0.05 * (lr_prob - self.lane_width_certainty)
-    current_lane_width = abs(l_poly[3] - r_poly[3])
-    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
-    speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
-    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
-                      (1 - self.lane_width_certainty) * speed_lane_width
-
-    self.lead_dist = md.lead.dist
-    self.lead_prob = md.lead.prob
-    self.lead_var = md.lead.std**2
-
-    # compute target path
-    self.d_poly, self.c_poly, self.c_prob = calc_desired_path(
-      l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego, self.lane_width)
-
-    self.r_poly = r_poly
-    self.r_prob = r_prob
-
-    self.l_poly = l_poly
-    self.l_prob = l_prob
-
-    self.p_poly = p_poly
-    self.p_prob = p_prob
@@ -2,12 +2,13 @@ import os
 import math
 import numpy as np

+# from common.numpy_fast import clip
 from common.realtime import sec_since_boot
 from selfdrive.services import service_list
 from selfdrive.swaglog import cloudlog
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
-from selfdrive.controls.lib.model_parser import ModelParser
+from selfdrive.controls.lib.lane_planner import LanePlanner
 import selfdrive.messaging as messaging

 LOG_MPC = os.environ.get('LOG_MPC', False)
@@ -21,10 +22,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_

 class PathPlanner(object):
  def __init__(self, CP):
-    self.MP = ModelParser()
-
-    self.l_poly = [0., 0., 0., 0.]
-    self.r_poly = [0., 0., 0., 0.]
+    self.LP = LanePlanner()

    self.last_cloudlog_t = 0

@@ -33,6 +31,7 @@ class PathPlanner(object):

    self.setup_mpc(CP.steerRateCost)
    self.solution_invalid_cnt = 0
+    self.path_offset_i = 0.0

  def setup_mpc(self, steer_rate_cost):
    self.libmpc = libmpc_py.libmpc
@@ -50,10 +49,6 @@ class PathPlanner(object):
    self.angle_steers_des_prev = 0.0
    self.angle_steers_des_time = 0.0

-    self.l_poly = libmpc_py.ffi.new("double[4]")
-    self.r_poly = libmpc_py.ffi.new("double[4]")
-    self.p_poly = libmpc_py.ffi.new("double[4]")
-
  def update(self, sm, CP, VM):
    v_ego = sm['carState'].vEgo
    angle_steers = sm['carState'].steeringAngle
@@ -62,23 +57,28 @@ class PathPlanner(object):
    angle_offset_average = sm['liveParameters'].angleOffsetAverage
    angle_offset_bias = sm['controlsState'].angleModelBias + angle_offset_average

-    self.MP.update(v_ego, sm['model'])
+    self.LP.update(v_ego, sm['model'])

    # Run MPC
    self.angle_steers_des_prev = self.angle_steers_des_mpc
    VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio)
    curvature_factor = VM.curvature_factor(v_ego)
-    self.l_poly = list(self.MP.l_poly)
-    self.r_poly = list(self.MP.r_poly)
-    self.p_poly = list(self.MP.p_poly)
+
+    # TODO: Check for active, override, and saturation
+    # if active:
+    #   self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0)
+    #   self.path_offset_i = clip(self.path_offset_i, -0.5,  0.5)
+    #   self.LP.d_poly[3] += self.path_offset_i
+    # else:
+    #   self.path_offset_i = 0.0

    # account for actuation delay
    self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)

    v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
    self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        self.l_poly, self.r_poly, self.p_poly,
-                        self.MP.l_prob, self.MP.r_prob, self.MP.p_prob, curvature_factor, v_ego_mpc, self.MP.lane_width)
+                        list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly),
+                        self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width)

    # reset to current steer angle if not active or overriding
    if active:
@@ -112,17 +112,16 @@ class PathPlanner(object):
    plan_send = messaging.new_message()
    plan_send.init('pathPlan')
    plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model'])
-    plan_send.pathPlan.laneWidth = float(self.MP.lane_width)
-    plan_send.pathPlan.dPoly = [float(x) for x in self.MP.d_poly]
-    plan_send.pathPlan.cPoly = [float(x) for x in self.MP.c_poly]
-    plan_send.pathPlan.cProb = float(self.MP.c_prob)
-    plan_send.pathPlan.lPoly = [float(x) for x in self.l_poly]
-    plan_send.pathPlan.lProb = float(self.MP.l_prob)
-    plan_send.pathPlan.rPoly = [float(x) for x in self.r_poly]
-    plan_send.pathPlan.rProb = float(self.MP.r_prob)
+    plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
+    plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly]
+    plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly]
+    plan_send.pathPlan.lProb = float(self.LP.l_prob)
+    plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly]
+    plan_send.pathPlan.rProb = float(self.LP.r_prob)
+
    plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
    plan_send.pathPlan.rateSteers = float(rate_desired)
-    plan_send.pathPlan.angleOffset = float(angle_offset_average)
+    plan_send.pathPlan.angleOffset = float(self.path_offset_i)
    plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
    plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
    plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid)
@@ -3,7 +3,6 @@ import math
 import numpy as np
 from common.params import Params
 from common.numpy_fast import interp
-from common.kalman.simple_kalman import KF1D

 import selfdrive.messaging as messaging
 from cereal import car
@@ -95,9 +94,7 @@ class Planner(object):
    self.longitudinalPlanSource = 'cruise'
    self.fcw_checker = FCWChecker()
    self.fcw_enabled = fcw_enabled
-
-    self.model_v_kf = KF1D([[0.0],[0.0]], _MODEL_V_A, _MODEL_V_C, _MODEL_V_K)
-    self.model_v_kf_ready = False
+    self.path_x = np.arange(192)

    self.params = Params()

@@ -112,7 +109,6 @@ class Planner(object):
      slowest = min(solutions, key=solutions.get)

      self.longitudinalPlanSource = slowest
-
      # Choose lowest of MPC and cruise
      if slowest == 'mpc1':
        self.v_acc = self.mpc1.v_mpc
@@ -145,15 +141,21 @@ class Planner(object):
    enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
    following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0

-    if not self.model_v_kf_ready:
-      self.model_v_kf.x = [[v_ego],[0.0]]
-      self.model_v_kf_ready = True
+    if len(sm['model'].path.poly):
+      path = list(sm['model'].path.poly)

-    if len(sm['model'].speed):
-      self.model_v_kf.update(sm['model'].speed[SPEED_PERCENTILE_IDX])
+      # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function
+      # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b
+      # k = y'' / (1 + y'^2)^1.5
+      y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2]
+      y_pp = 6 * path[0] * self.path_x + 2 * path[1]
+      curv = y_pp / (1. + y_p**2)**1.5

-    if self.params.get("LimitSetSpeedNeural") == "1":
-      model_speed = self.model_v_kf.x[0][0]
+      a_y_max = 2.975 - v_ego * 0.0375  # ~1.85 @ 75mph, ~2.6 @ 25mph
+      v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None))
+      model_speed = np.min(v_curvature)
+      # print(model_speed * CV.MS_TO_MPH, model_speed)
+      model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph
    else:
      model_speed = MAX_SPEED

@@ -174,11 +176,9 @@ class Planner(object):
                                                    jerk_limits[1], jerk_limits[0],
                                                    LON_MPC_STEP)

-      # accel and jerk up limits are higher here to make model not limiting accel
-      # mainly done to prevent flickering of slowdown icon
      self.v_model, self.a_model = speed_smoother(self.v_acc_start, self.a_acc_start,
                                                    model_speed,
-                                                    2*accel_limits[1], 3*accel_limits[0],
+                                                    2*accel_limits[1], accel_limits[0],
                                                    2*jerk_limits[1], jerk_limits[0],
                                                    LON_MPC_STEP)

@@ -234,13 +234,13 @@ class Planner(object):
    plan_send.plan.radarStateMonoTime = sm.logMonoTime['radarState']

    # longitudal plan
-    plan_send.plan.vCruise = self.v_cruise
-    plan_send.plan.aCruise = self.a_cruise
-    plan_send.plan.vStart = self.v_acc_start
-    plan_send.plan.aStart = self.a_acc_start
-    plan_send.plan.vTarget = self.v_acc
-    plan_send.plan.aTarget = self.a_acc
-    plan_send.plan.vTargetFuture = self.v_acc_future
+    plan_send.plan.vCruise = float(self.v_cruise)
+    plan_send.plan.aCruise = float(self.a_cruise)
+    plan_send.plan.vStart = float(self.v_acc_start)
+    plan_send.plan.aStart = float(self.a_acc_start)
+    plan_send.plan.vTarget = float(self.v_acc)
+    plan_send.plan.aTarget = float(self.a_acc)
+    plan_send.plan.vTargetFuture = float(self.v_acc_future)
    plan_send.plan.hasLead = self.mpc1.prev_lead_status
    plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource

@@ -25,14 +25,9 @@ _VLEAD_K = [[0.1988689], [0.28555364]]
 class Track(object):
  def __init__(self):
    self.ekf = None
-    self.initted = False
+    self.cnt = 0

  def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured):
-    if self.initted:
-      # pylint: disable=access-member-before-definition
-      self.vLeadPrev = self.vLead
-      self.vRelPrev = self.vRel
-
    # relative values, copy
    self.dRel = d_rel   # LONG_DIST
    self.yRel = y_rel   # -LAT_DIST
@@ -42,17 +37,12 @@ class Track(object):
    # computed velocity and accelerations
    self.vLead = self.vRel + v_ego_t_aligned

-    if not self.initted:
-      self.initted = True
-      self.aLeadTau = _LEAD_ACCEL_TAU
-      self.cnt = 1
-      self.vision_cnt = 0
-      self.vision = False
+    if self.cnt == 0:
      self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
    else:
      self.kf.update(self.vLead)

-      self.cnt += 1
+    self.cnt += 1

    self.vLeadK = float(self.kf.x[SPEED][0])
    self.aLeadK = float(self.kf.x[ACCEL][0])
@@ -67,6 +57,10 @@ class Track(object):
    # Weigh y higher since radar is inaccurate in this dimension
    return [self.dRel, self.yRel*2, self.vRel]

+  def reset_a_lead(self, aLeadK, aLeadTau):
+    self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
+    self.aLeadK = aLeadK
+    self.aLeadTau = aLeadTau

 def mean(l):
  return sum(l) / len(l)
@@ -115,11 +109,17 @@ class Cluster(object):

  @property
  def aLeadK(self):
-    return mean([t.aLeadK for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return 0.
+    else:
+      return mean([t.aLeadK for t in self.tracks if t.cnt > 1])

  @property
  def aLeadTau(self):
-    return mean([t.aLeadTau for t in self.tracks])
+    if all(t.cnt <= 1 for t in self.tracks):
+      return _LEAD_ACCEL_TAU
+    else:
+      return mean([t.aLeadTau for t in self.tracks if t.cnt > 1])

  @property
  def measured(self):
@@ -143,15 +143,24 @@ class RadarD(object):
          clusters[cluster_i] = Cluster()
        clusters[cluster_i].add(self.tracks[idens[idx]])
    elif len(track_pts) == 1:
+      # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1
+      cluster_idxs = [0]
      clusters = [Cluster()]
      clusters[0].add(self.tracks[idens[0]])
    else:
      clusters = []

+    # if a new point, reset accel to the rest of the cluster
+    for idx in xrange(len(track_pts)):
+      if self.tracks[idens[idx]].cnt <= 1:
+        aLeadK = clusters[cluster_idxs[idx]].aLeadK
+        aLeadTau = clusters[cluster_idxs[idx]].aLeadTau
+        self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau)
+
    # *** publish radarState ***
    dat = messaging.new_message()
    dat.init('radarState')
-    dat.valid = sm.all_alive_and_valid(service_list=['controlsState'])
+    dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model'])
    dat.radarState.mdMonoTime = self.last_md_ts
    dat.radarState.canMonoTimes = list(rr.canMonoTimes)
    dat.radarState.radarErrors = list(rr.errors)
@@ -1,9 +1,9 @@
 import unittest
-import copy
 import numpy as np
 from selfdrive.car.honda.interface import CarInterface
 from selfdrive.controls.lib.lateral_mpc import libmpc_py
 from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.controls.lib.lane_planner import calc_d_poly


 def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
@@ -16,13 +16,17 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,

  mpc_solution = libmpc_py.ffi.new("log_t *")

-  p_l = copy.copy(poly_l)
+  p_l = poly_l.copy()
  p_l[3] += poly_shift
-  p_r = copy.copy(poly_r)
+
+  p_r = poly_r.copy()
  p_r[3] += poly_shift
-  p_p = copy.copy(poly_p)
+
+  p_p = poly_p.copy()
  p_p[3] += poly_shift

+  d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width)
+
  CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
  VM = VehicleModel(CP)

@@ -31,7 +35,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,

  l_poly = libmpc_py.ffi.new("double[4]", map(float, p_l))
  r_poly = libmpc_py.ffi.new("double[4]", map(float, p_r))
-  p_poly = libmpc_py.ffi.new("double[4]", map(float, p_p))
+  d_poly = libmpc_py.ffi.new("double[4]", map(float, d_poly))

  cur_state = libmpc_py.ffi.new("state_t *")
  cur_state[0].x = x_init
@@ -41,7 +45,7 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,

  # converge in no more than 20 iterations
  for _ in range(20):
-    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, p_prob,
+    libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob, r_prob,
                   curvature_factor, v_ref, lane_width)

  return mpc_solution
@@ -119,3 +123,7 @@ class TestLateralMpc(unittest.TestCase):
    sol = run_mpc(y_init=y_init)
    for y in list(sol[0].y):
      self.assertGreaterEqual(y_init, abs(y))
+
+
+if __name__ == "__main__":
+  unittest.main()